#include <Wire.h>
#include <Zumo32U4.h>

Zumo32U4LCD lcd;
Zumo32U4Buzzer buzzer;
Zumo32U4ButtonA buttonA;
Zumo32U4ButtonB buttonB;
Zumo32U4ButtonC buttonC;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;
LSM303 compass;
L3G gyro;
Zumo32U4Motors motors;
int j;
const uint8_t sensorThreshold = 5;


const uint16_t turnSpeedMax = 400;


const uint16_t turnSpeedMin = 300;


const uint16_t deceleration = 10;

const uint16_t acceleration = 50;

#define LEFT 0
#define RIGHT 1


bool senseDir = RIGHT;

bool turningLeft = false;



bool turningRight = false;


uint16_t turnSpeed = turnSpeedMax;

char buttonMonitor();

class Menu
{
  public:
    struct Item
    {
      const char * name;
      void (* action)();
    };

    Menu(Item * items, uint8_t itemCount)
    {
      this->items = items;
      this->itemCount = itemCount;
      lcdItemIndex = 0;
    }

    void lcdUpdate(uint8_t index)
    {
      lcd.clear();
      lcd.print(items[index].name);
      lcd.gotoXY(0, 1);
      lcd.print(F("\x7f" "A \xa5" "B C\x7e"));
    }

    void action(uint8_t index)
    {
      items[index].action();
    }


    void select()
    {
      lcdUpdate(lcdItemIndex);

      while (1)
      {
        switch (buttonMonitor())
        {
          case 'A':

            if (lcdItemIndex == 0)
            {
              lcdItemIndex = itemCount - 1;
            }
            else
            {
              lcdItemIndex--;
            }
            lcdUpdate(lcdItemIndex);
            break;

          case 'C':

            if (lcdItemIndex >= itemCount - 1)
            {
              lcdItemIndex = 0;
            }
            else
            {
              lcdItemIndex++;
            }
            lcdUpdate(lcdItemIndex);
            break;

          case 'B':

            action(lcdItemIndex);
            return;
        }
      }
    }

  private:
    Item * items;
    uint8_t itemCount;
    uint8_t lcdItemIndex;
};

const char beepBrownout[] PROGMEM = "<c8";
const char beepWelcome[] PROGMEM = ">g32>>c32";
const char beepThankYou[] PROGMEM = ">>c32>g32";
const char beepButtonA[] PROGMEM = "!c32";
const char beepButtonB[] PROGMEM = "!e32";
const char beepButtonC[] PROGMEM = "!g32";

const char backArrow[] PROGMEM = {
  0b00000,
  0b00010,
  0b00001,
  0b00101,
  0b01001,
  0b11110,
  0b01000,
  0b00100,
};

const char forwardArrows[] PROGMEM = {
  0b00000,
  0b00100,
  0b01010,
  0b10001,
  0b00100,
  0b01010,
  0b10001,
  0b00000,
};

const char reverseArrows[] PROGMEM = {
  0b00000,
  0b10001,
  0b01010,
  0b00100,
  0b10001,
  0b01010,
  0b00100,
  0b00000,
};

const char forwardArrowsSolid[] PROGMEM = {
  0b00000,
  0b00100,
  0b01110,
  0b11111,
  0b00100,
  0b01110,
  0b11111,
  0b00000,
};


const char reverseArrowsSolid[] PROGMEM = {
  0b00000,
  0b11111,
  0b01110,
  0b00100,
  0b11111,
  0b01110,
  0b00100,
  0b00000,
};

void loadCustomCharacters()
{


  lcd.loadCustomCharacter(backArrow, 7);
}

void loadCustomCharactersBarGraph()
{
  static const char levels[] PROGMEM = {
    0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63
  };
  lcd.loadCustomCharacter(levels + 0, 0);  // 1 bar
  lcd.loadCustomCharacter(levels + 1, 1);  // 2 bars
  lcd.loadCustomCharacter(levels + 2, 2);  // 3 bars
  lcd.loadCustomCharacter(levels + 3, 3);  // 4 bars
  lcd.loadCustomCharacter(levels + 4, 4);  // 5 bars
  lcd.loadCustomCharacter(levels + 5, 5);  // 6 bars
  lcd.loadCustomCharacter(levels + 6, 6);  // 7 bars
}

void loadCustomCharactersMotorDirs()
{
  lcd.loadCustomCharacter(forwardArrows, 0);
  lcd.loadCustomCharacter(reverseArrows, 1);
  lcd.loadCustomCharacter(forwardArrowsSolid, 2);
  lcd.loadCustomCharacter(reverseArrowsSolid, 3);
}


void displayBackArrow()
{
  lcd.clear();
  lcd.gotoXY(0, 1);
  lcd.print(F("\7B"));
  lcd.gotoXY(0, 0);
}


const char Pirati[] PROGMEM =
  "!T196 L8 defd L4 e dc d L8 defd L4 e L3g. L8  gab-dfd L4 fededcdfa >d  RR !T196 L4 d L8 d L4 d L8 d L4 d L8 d  ddd L4 d L8 d L4 d L8 d L4 d L8 d  ddd L4 d L8 d L4 d L8 d L4 d L8 d  d<a c L4 dd L8 de L4 ff L8 fg L4ee L8 dc c L4 d L8R <ac L4 dd L8 de L4 ff L8 fg L4eeL8 dc L4 dR L8 <ac L4 dd L8 df L4 gg L8 ga L4 b-b- L8 ag a L4d L8 Rde L4 ffg L8 a L4d L8 R df L4 ee L8 df L4e.";
const char PiratideiCaraibiTitle[] PROGMEM =
  "      Pirati dei Caribi - by Hans Zimmer       ";
void Pirates()
{
  displayBackArrow();
  uint8_t state = 3;

  uint8_t PiratideiCaraibiTitlePos = 0;
  uint16_t lastShiftTime = millis() - 2000;

  while (buttonMonitor() != 'B')
  {

    if ((uint16_t)(millis() - lastShiftTime) > 250)
    {
      lastShiftTime = millis();

      lcd.gotoXY(0, 0);
      for (uint8_t i = 0; i < 8; i++)
      {
        char c = pgm_read_byte(PiratideiCaraibiTitle + PiratideiCaraibiTitlePos + i);
        lcd.print(c);
      }
      PiratideiCaraibiTitlePos++;

      if (PiratideiCaraibiTitlePos + 8 >= strlen(PiratideiCaraibiTitle))
      {
        PiratideiCaraibiTitlePos = 0;
      }
    }

    if (!buzzer.isPlaying())
    {
      buzzer.playFromProgramSpace(Pirati);
    }
  }
}
void printBar(uint8_t height)
{
  if (height > 8) {
    height = 8;
  }
  static const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, 255};
  lcd.print(barChars[height]);
}

void lineSensorDemo()
{
  loadCustomCharactersBarGraph();
  displayBackArrow();
  lcd.gotoXY(6, 1);
  lcd.print('C');

  uint16_t lineSensorValues[3];
  char c;

  while (buttonMonitor() != 'B')
  {
    bool emittersOff = buttonC.isPressed();

    if (emittersOff)
    {
      lineSensors.read(lineSensorValues, QTR_EMITTERS_OFF);
    }
    else
    {
      lineSensors.read(lineSensorValues, QTR_EMITTERS_ON);
    }

    lcd.gotoXY(1, 0);
    for (uint8_t i = 0; i < 3; i++)
    {
      uint8_t barHeight = map(lineSensorValues[i], 0, 2000, 0, 8);
      printBar(barHeight);
      lcd.print(' ');
    }


    lcd.gotoXY(7, 1);
    if (emittersOff)
    {
      lcd.print('\xa5');  // centered dot
    }
    else
    {
      lcd.print('*');
    }
  }
}
//int j;
//const uint8_t sensorThreshold = 5;
//
//
//const uint16_t turnSpeedMax = 200;
//
//
//const uint16_t turnSpeedMin = 100;
//
//
//const uint16_t deceleration = 10;
//
//const uint16_t acceleration = 50;
//
//#define LEFT 0
//#define RIGHT 1
//
//
//bool senseDir = RIGHT;
//
//bool turningLeft = false;
//
//
//
//bool turningRight = false;
//
//
//uint16_t turnSpeed = turnSpeedMax;

void goForward() {
  motors.setSpeeds(turnSpeed, turnSpeed);
}
void goBackward() {
  motors.setSpeeds(-turnSpeed, -turnSpeed);
}
void rotateRight() {
  motors.setSpeeds(turnSpeed, -turnSpeed);
}

void rotateLeft() {
  motors.setSpeeds(-turnSpeed, turnSpeed);
}
void stopMotors() {
  motors.setSpeeds(0, 0);
}



void motorDemoHelper2(bool showEncoders)
{
  loadCustomCharactersMotorDirs();
  lcd.clear();
  lcd.gotoXY(1, 1);
  lcd.print(F("A \7B C"));

  int16_t leftSpeed = 0, rightSpeed = 0;
  int8_t leftDir = 1, rightDir = 1;
  uint16_t lastUpdateTime = millis() - 100;
  uint8_t btnCountA = 0, btnCountC = 0, instructCount = 0;

  int16_t encCountsLeft = 0, encCountsRight = 0;
  char buf[4];


//  #if 1
  
  while (buttonMonitor() != 'B')
  { proxSensors.read();
    uint8_t distanceleft = proxSensors.countsFrontWithLeftLeds();
    uint8_t distanceright = proxSensors.countsFrontWithRightLeds();
    lcd.gotoXY(0, 0);
    lcd.print(distanceleft);
    lcd.print(' ');
    lcd.print(distanceright);
    lcd.gotoXY(0, 1);
    lcd.print(distanceright ? 'R' : (distanceleft ? 'L' : ' '));
    lcd.print(' ');
    lcd.print(' ');

    if (distanceleft >= 5 , distanceright >= 5 ) {

      goBackward();
      delay(250);

      rotateRight();

      delay(250);



      for (j = 0; j < 1000; j++) {

        proxSensors.read();
        uint8_t distanceleft = proxSensors.countsFrontWithLeftLeds();
        uint8_t distanceright = proxSensors.countsFrontWithRightLeds();

        lcd.gotoXY(0, 0);
        lcd.print(distanceleft);
       lcd.print(' ');
        lcd.print(distanceright);
        lcd.gotoXY(0, 1);
       lcd.print(distanceright ? 'R' : (distanceleft ? 'L' : ' '));
        lcd.print(' ');
        lcd.print(' ');
        goForward();
        delay(10);


        if (distanceleft >= 4  ) {

          goBackward();
          delay(100);

          rotateRight();
          delay(100); ;
        }


        if (distanceright >= 4 ) {

          goBackward();
          delay(100) ;
          rotateLeft();
          delay(100) ;
        }





      }

      rotateLeft();
      delay(3000);
      stopMotors();
    }
  }
//#endif
}
void Predatore() {
  motorDemoHelper2(false);
  motorDemoHelper2(true);
}


void initInertialSensors()
{
  Wire.begin();
  compass.init();
  compass.enableDefault();
  gyro.init();
  gyro.enableDefault();
}


void printLargestAxis(int16_t x, int16_t y, int16_t z, uint16_t threshold)
{
  int16_t largest = x;
  char axis = 'X';

  if (abs(y) > abs(largest))
  {
    largest = y;
    axis = 'Y';
  }
  if (abs(z) > abs(largest))
  {
    largest = z;
    axis = 'Z';
  }

  if (abs(largest) < threshold)
  {
    lcd.print("  ");
  }
  else
  {
    bool positive = (largest > 0);
    lcd.print(positive ? '+' : '-');
    lcd.print(axis);
  }
}


const char Amore[] PROGMEM =
  "!T139 L4 R f+ L8 f+>d L4>c+ R >c+ L8 >c+>d L4 b R b L8 ba L4 b b L8 baba L2 f+ L4 f+ L4 R f+ L8 f+>d L4>c+ R >c+ L8 >c+>d L4 b R b L8 ba L4 b b L8 baba L4f+";
const char AmourTojoursTitle[] PROGMEM =
  "      L'Amour Tojours - by Gigi Dag       ";
void Amour()
{
  displayBackArrow();
  uint8_t state = 3;
  uint8_t AmourTojoursTitlePos = 0;
  uint16_t lastShiftTime = millis() - 2000;

  while (buttonMonitor() != 'B')
  {

    if ((uint16_t)(millis() - lastShiftTime) > 250)
    {
      lastShiftTime = millis();

      lcd.gotoXY(0, 0);
      for (uint8_t i = 0; i < 8; i++)
      {
        char c = pgm_read_byte(AmourTojoursTitle + AmourTojoursTitlePos + i);
        lcd.print(c);
      }
      AmourTojoursTitlePos++;

      if (AmourTojoursTitlePos + 8 >= strlen(AmourTojoursTitle))
      {
        AmourTojoursTitlePos = 0;
      }
    }

    if (!buzzer.isPlaying())
    {
      buzzer.playFromProgramSpace(Amore);
    }
  }
}


void turnRight()
{
  motors.setSpeeds(turnSpeed, -turnSpeed);

}
void stop()
{
  motors.setSpeeds(0, 0);

}
void turnLeft()
{
  motors.setSpeeds(-turnSpeed, turnSpeed);

}
void attack () {

  motors.setSpeeds(turnSpeed, turnSpeed);
  turnSpeed += acceleration;
  delay(1200);
  motors.setSpeeds(-turnSpeed, -turnSpeed);
  turnSpeed -= deceleration;
  delay(400);
  stop();
}
void motorDemoHelper(bool showEncoders)
{
  loadCustomCharactersMotorDirs();
  lcd.clear();
  lcd.gotoXY(1, 1);
  lcd.print(F("A \7B C"));

  int16_t leftSpeed = 0, rightSpeed = 0;
  int8_t leftDir = 1, rightDir = 1;
  uint16_t lastUpdateTime = millis() - 100;
  uint8_t btnCountA = 0, btnCountC = 0, instructCount = 0;

  int16_t encCountsLeft = 0, encCountsRight = 0;
  char buf[4];

  while (buttonMonitor() != 'B')
  { proxSensors.read();
    uint8_t leftValue = proxSensors.countsFrontWithLeftLeds();
    uint8_t rightValue = proxSensors.countsFrontWithRightLeds();

    // Determine if an object is visible or not.
    bool objectSeen = leftValue >= sensorThreshold || rightValue >= sensorThreshold;


    if (objectSeen)
    {

      turnSpeed -= deceleration;
      buzzer.play("!T220 L8 c<b-cc<b-cc<b-c L4<g L8<b- c<b-ce-<b-dfcfgb-gc<b-cc<b-ce-d<b- L4c L8g f<b-de-<b-d L4 c L8 b-fg c<b-cc<b-cc<b-c L4<g L8<b- c<b-ce-<b-dfcfgb-gc<b-cc<b-ce-d<b- L4c L8g f<b-de-<b-d L4 c L8 b-fg c<b-cc<b-cc<b-c L4<g L8<b- c<b-ce-<b-de-fcfgb-g c<b-cc<b-ce-d<b- L4 c L8 g f<b-de-<b-d L4c L8c b-fg  ");
      attack();
    }
    else
    {
      turnSpeed += acceleration;
    }
    turnSpeed = constrain(turnSpeed, turnSpeedMin, turnSpeedMax);

    if (objectSeen)
    {

      ledYellow(1);


      if (leftValue < rightValue)
      {

        turnRight();
        senseDir = RIGHT;
      }
      else if (leftValue > rightValue)
      {

        turnLeft();
        senseDir = LEFT;
      }
      else
      {

        stop();
      }
    }
    else
    {

      ledYellow(0);

      if (senseDir == RIGHT)
      {
        turnRight();
      }
      else
      {
        turnLeft();
      }
    }
    lcd.gotoXY(0, 0);
    lcd.print(leftValue);
    lcd.print(' ');
    lcd.print(rightValue);
    lcd.gotoXY(0, 1);
    lcd.print(turningRight ? 'R' : (turningLeft ? 'L' : ' '));
    lcd.print(' ');
    lcd.print(turnSpeed);
    lcd.print(' ');
    lcd.print(' ');
  }
  stop();
}


void motorDemo()
{
  motorDemoHelper(false);
  motorDemoHelper(true);
}




const char NuvoleBianche[] PROGMEM =
  "!T105 L8 a-gfa-gf>cgfa-gfa-gfa-gf>d-ga-a-gf >cb-a->cb-a->e-a-b->cb-a-b-a-g>e-ga-b-a-g g4 a-gfa-gf>cfga-gfa-gfa-gf>d-fga-gf>cb-a->cb-a->e-a-b->cb-a-b-a-gb-a-g>e-ga-b-a-g >a-a->c>ga->c>fa->d->e-a->d->e-a->c>e-a->c>e-gb->e-gb->a-a->ga->fa->f>fa->d->e-a->d->e-.gb- b-.4";

const char NuvolineTitle[] PROGMEM =
  "      Nuvole Bianche - by Ludovico Einaudi       ";

void Nuvoline()
{
  displayBackArrow();

  uint8_t NuvolineTitlePos = 0;
  uint16_t lastShiftTime = millis() - 2000;

  while (buttonMonitor() != 'B')
  {

    if ((uint16_t)(millis() - lastShiftTime) > 250)
    {
      lastShiftTime = millis();

      lcd.gotoXY(0, 0);
      for (uint8_t i = 0; i < 8; i++)
      {
        char c = pgm_read_byte(NuvolineTitle + NuvolineTitlePos + i);
        lcd.print(c);
      }
      NuvolineTitlePos++;

      if (NuvolineTitlePos + 8 >= strlen(NuvolineTitle))
      {
        NuvolineTitlePos = 0;
      }
    }

    if (!buzzer.isPlaying())
    {
      buzzer.playFromProgramSpace(NuvoleBianche);
    }
  }
}

void powerDemo()
{
  displayBackArrow();

  uint16_t lastDisplayTime = millis() - 2000;
  char buf[6];

  while (buttonMonitor() != 'B')
  {
    if ((uint16_t)(millis() - lastDisplayTime) > 250)
    {
      bool usbPower = usbPowerPresent();

      uint16_t batteryLevel = readBatteryMillivolts();

      lastDisplayTime = millis();
      lcd.gotoXY(0, 0);
      sprintf(buf, "%5d", batteryLevel);
      lcd.print(buf);
      lcd.print(F(" mV"));
      lcd.gotoXY(3, 1);
      lcd.print(F("USB="));
      lcd.print(usbPower ? 'Y' : 'N');
    }
  }
}

Menu::Item mainMenuItems[] = {
  { "Pirates", Pirates },
  { "LineSens", lineSensorDemo },
  { "Predator", Predatore },
  { "Amore", Amour },
  { "Avengers", motorDemo },
  //{ "Encoders", encoderDemo },
  { "Nuvoline", Nuvoline },
  { "Batterie", powerDemo },
};
Menu mainMenu(mainMenuItems, 7);


char buttonMonitor()
{
  if (buttonA.getSingleDebouncedPress())
  {
    buzzer.playFromProgramSpace(beepButtonA);
    return 'A';
  }

  if (buttonB.getSingleDebouncedPress())
  {
    buzzer.playFromProgramSpace(beepButtonB);
    return 'B';
  }

  if (buttonC.getSingleDebouncedPress())
  {
    buzzer.playFromProgramSpace(beepButtonC);
    return 'C';
  }

  return 0;
}

void setup()
{
  lineSensors.initThreeSensors();
  proxSensors.initThreeSensors();
  initInertialSensors();

  loadCustomCharacters();
  proxSensors.initFrontSensor();


  bool brownout = MCUSR >> BORF & 1;
  MCUSR = 0;

  if (brownout)
  {


    buzzer.playFromProgramSpace(beepBrownout);
    lcd.clear();
    lcd.print(F("Brownout"));
    lcd.gotoXY(0, 1);
    lcd.print(F(" reset! "));
    delay(1000);
  }
  else
  {
    buzzer.playFromProgramSpace(beepWelcome);
  }

  lcd.clear();
  lcd.print(F("Zumo The"));
  lcd.gotoXY(0, 1);
  lcd.print(F("Predator"));

  delay(1000);

  lcd.clear();
  lcd.print(F("In The"));
  lcd.gotoXY(0, 1);
  lcd.print(F("Forest"));
  delay(1000);

  lcd.clear();
  lcd.print(F("Use B to"));
  lcd.gotoXY(0, 1);
  lcd.print(F("select."));
  delay(1000);

  lcd.clear();
  lcd.print(F("Press B"));
  lcd.gotoXY(0, 1);
  lcd.print(F("-try it!"));

  while (buttonMonitor() != 'B') {}

  buzzer.playFromProgramSpace(beepThankYou);
  lcd.clear();
  lcd.print(F(" Grazie"));
  lcd.gotoXY(0, 1);
  lcd.print(F("Beddrhu!"));
  delay(1000);
}

void mainMenuSelect()
{
  lcd.clear();
  lcd.print(F("  Main"));
  lcd.gotoXY(0, 1);
  lcd.print(F("  Menu"));
  delay(1000);
  mainMenu.select();
}

void loop()
{
  mainMenuSelect();
}