#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)

struct servoStruct_t {
  byte servoNumber;
  byte normPos;
  byte revPos;
  bool invert;
} *servos[16] ;
int numberOfServos;
Turnout *turnouts[16];
Adafruit_PWMServoDriver pwm;

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

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(2000);
    turnout->setReverse();
    delay(2000);
    turnout->setNormal();
    delay(2000);

  }
  printf("Setup complete\n");

}

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