#include <EEPROM.h>
#include <LibPrintf.h>

struct servoStruct_t {
  byte servoNumber;
  byte normPos;
  byte revPos;
  bool invert;
} *servos[16] ;


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("Initialising %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 = 80;
    thisServo.revPos = 100;
    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 4 LOW
  Serial.begin(9600);
  printf("Reset\n");
  digitalWrite(4, HIGH);
  delay(100);
  pinMode(4, OUTPUT);
  // 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(4, LOW);
  }
  printf("Setup complete\n");

}

void loop() {
  // read the EEPROM to discover how many servo we have
  int noOfServos = EEPROM.read(1);
  printf("Initialising %d servos\n", noOfServos);
  for (int i = 0 ; i < noOfServos; i++)
  {
    servoStruct_t thisServo;
    thisServo = EEPROM.get(2 + (i * sizeof(servoStruct_t)));
    printf("ServoNumber = %d\n,thisServo.servoNumber");
  }
  printf("Done\n");
  while (true) {
    delay(1000);
  }
}