#include <EEPROM.h>
#include <LibPrintf.h>
#include "Turnout.h"
#define SERVOMIN  100 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)

int numberOfServos;
Turnout *turnouts[16];
Adafruit_PWMServoDriver pwm;
char keys[5];
size_t len;
servoStruct_t *servos[16];

void printEEPROM()
{
  for (int i = 0; i < 30; i++) {
    printf("%d - %d\n", i, EEPROM.read(i));
  }
}

void adjustServo(int servoNumber) {
  int val;
  const unsigned char adjustHeader[] = "N - set Normal\t R - set Reverse\t \n";
  Turnout *turnout = turnouts[servoNumber];
  // throw away anything in the buffer
  len = Serial.readBytesUntil('\n', keys, sizeof(keys));
  Serial.setTimeout(3000);
  printf(adjustHeader);
  while (true)
  {
    len = Serial.readBytesUntil('\n', keys, sizeof(keys));
    if (len == 0)
    {
      // nothing recieved
      continue;
    } else {
      switch (keys[0])
      {
        case 'N':
          turnout->setNormal();
          break;
        case 'R':
          turnout->setReverse();
          break;
        case 'Q':
        case 'q':
          printf("Done\n");
          return;
        case 'P':
          // print the current values
          printf("Servo %d Current values\t Normal=%d\t Reverse=%d\n", turnout->getNumber(), turnout->getNormal(), turnout->getReverse());
          break;
        case 'S':
          if (turnout->getState() == 0) {
            printf("Normal\n");
          } else {
            printf("Reversed\n");
          }
          break;
        case 'X':
          printf("Set new Normal Value\n");
          val = 0;
          while (val == 0) {
            val = Serial.parseInt();
            if (val == 999) {
              return;
            }
            if (val > 245) {
              printf("Too Large\n (1-245 required)\n");
              printf(adjustHeader);
            }
          }
          turnout->setNormPos((unsigned char) val);
          printf("Done %d\n", val);
          break;
        case 'Y':
          printf("Set new Reverse Value\n");
          val = 0;
          while (val == 0) {
            val = Serial.parseInt();
            if (val == 999) {
              return;
            }
            if (val > 245) {
              printf("Too Large\n (1-245 required)\n");
              printf(adjustHeader);
            }
          }
          turnout->setRevPos((unsigned char) val);
          printf("Done %d\n", val);
          break;
        case 'W':
          turnout->writeEEPROM();
          return;
        default:
          printf("Unknown Command\n");
          printf(adjustHeader);
      }

    }

  }
}

void modifyServo() {
  bool found = false;
  while (!found) {
    int servoNumber = 0;
    printf("Modifying Servo - Enter Servo Number(1-10) 99 to exit\n");
    while (servoNumber == 0) {
      servoNumber = Serial.parseInt();
    }
    if (servoNumber == 99) {
      return;
    }
    if (servoNumber > numberOfServos) {
      printf("There are only %d servos defined\n", numberOfServos);
      continue;
    }
    printf("Modifying servo %d\n", servoNumber);
    adjustServo(servoNumber - 1);
  }

}
void processKeys() {
  const byte header[] = "A/a - add servo\t M/m - modify servo\t Q/q - quit\t R - reboot\n" ;
  // throw away anything in the buffer
  len = Serial.readBytesUntil('\n', keys, sizeof(keys));
  Serial.setTimeout(3000);
  printf(header);
  while (true)
  {
    len = Serial.readBytesUntil('\n', keys, sizeof(keys));
    if (len == 0)
    {
      // nothing recieved
      continue;
    } else {
      switch (keys[0])
      {
        case 'R':
          // reset
          digitalWrite(12, LOW);
        case 'Q':
        case 'q':
          printf("Done\n");
          return;
        case 'M':
        case 'm':
          modifyServo();
          printf(header);
          break;
        case 'A':
        case 'a':
          break;
        default:
          printf("Unknown Command\n");
          printf(header);
      }

    }

  }
}

void initialiseEeprom()
{
  // we need to ask how many servos we are going to power
  Serial.setTimeout(10000);
  int noOfServos = 0;
  while ((noOfServos == 0) || (noOfServos > 12))
  {
    Serial.println("How Many Servos?");
    noOfServos = Serial. parseInt();
  }
  printf("Creating %d servos\n", noOfServos);

  EEPROM.write(1, byte(noOfServos));

  EEPROM.write(0, 0x7E);
  for (int i = 0; i < noOfServos ; i++)
  {
    servoStruct_t thisServo;
    thisServo.servoNumber = i + 1;
    thisServo.normPos = 130;
    thisServo.revPos = 160;
    thisServo.invert = false;
    EEPROM.put(2 + (i * sizeof(servoStruct_t)), thisServo);
  }
  delay(1000);
}

void setup() {
  // set up the ability to reset the arduino by command, it is done by connecting pin D4 to reset
  // when a reset is required then this is done by pulling pin 12 LOW
  Serial.begin(9600);
  printf("Reset\n");
  digitalWrite(12, HIGH);
  delay(100);
  pinMode(12, OUTPUT);
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(50);  // Analog servos run at ~50 Hz updates
  // ok lets find out what the state is if word 0 of the EEPROM is not 0x7E then it has not been
  // initialised.
  byte byte0 = EEPROM.read((0));
  if (byte0 != 0x7E) {
    initialiseEeprom();
    // reset
    digitalWrite(12, LOW);
  }
  numberOfServos = EEPROM.read(1);
  printf("Initialising %d servos\n", numberOfServos);
  for (int i = 0 ; i < numberOfServos; i++)
  {
    servoStruct_t thisServo;
    EEPROM.get(2 + (i * sizeof(servoStruct_t)), thisServo);
    printf("ServoNumber = %d\n", thisServo.servoNumber);
    servos[i] = &thisServo;
    Turnout *turnout = new Turnout(i, &pwm);
    turnouts[i] = turnout;
    turnout->setNormPos(thisServo.normPos);
    turnout->setRevPos(thisServo.revPos);
    turnout->setNormal();
    delay(100);
    turnout->setReverse();
    delay(100);
    turnout->setNormal();
    delay(100);

  }
  printf("Setup complete\n");

}

void loop() {
  printf("Done\n");
  while (true) {
    for (int i = 0; i < numberOfServos; i++)
    {
      turnouts[i]->processButtons();
    }
    delay(10);
    if (Serial.available()) {
      processKeys();
    }

  }
}