#include <Servo.h>

#define LIGHT_TRESHOLD 7

#define CMD_STEP_FORWARD "f"
#define CMD_STEP_REWIND "r"
#define CMD_FAST_STEP_FORWARD "F"
#define CMD_FAST_STEP_REWIND "R"
#define CMD_FULL_REWIND "#"

#define CMD_SET_SPEED "S"
#define CMD_SET_TIC_LENGTH "T"
#define CMD_SET_TIC_MULTIPLIER "X"

#define CMD_CONFIRM_CONNECTION "V"
#define CMD_READ_BRIGHTNESS "L"
#define CMD_READ_IS_LOCKED "C"

#define CMD_MEMORY "M"

#define RESPONSE_SUN_AVAILABLE "READY TO BURN"
#define RESPONSE_SUN_DIMMED "NIGHT"

#define RESPONSE_SUN_POS "POS="
#define RESPONSE_SUN_FF "FF="
#define RESPONSE_SUN_REW "REW="
#define RESPONSE_SUN_BRIGHTNESS "LIGHT="

#define RESPONSE_PING "!"

#define RESPONSE_FLASH "FLASH"

uint8_t * heapptr, * stackptr;
void check_mem() {
  stackptr = (uint8_t *)malloc(4);          // use stackptr temporarily
  heapptr = stackptr;                     // save value of heap pointer
  free(stackptr);      // free up the memory again (sets stackptr to 0)
  stackptr =  (uint8_t *)(SP);           // save value of stack pointer
}


Servo servo1;
String buffer;
String feedBuffer;

int cfg_base = 93;
int cfg_tic = 20;
int cfg_tic_x = 1;
int cfg_speed = 10;

int cfg_delay = 300;

int ff = 0;
int rew = 0;

int pos = 0;

int servoPin = 9;
int lockPin = 10;
int brightnessPin = 14;
int failPin = 12;

unsigned long lightTimer = 0;
int lightness = 1023;
int lightnessCheckPeriod = 1000;
int roll = 0;

unsigned long pingTimerPeriod = 30000;
unsigned long pingTimer = 0;
unsigned long pingLEDTimer = 0;

unsigned long fullMillis = 17179868*2;

unsigned long fixedMillis() {
  return millisRollover()*fullMillis + millis();
}

int millisRollover() {
  // get the current millis() value for how long the microcontroller has been running
  //
  // To avoid any possiblity of missing the rollover, we use a boolean toggle that gets flipped
  //   off any time during the first half of the total millis period and
  //   then on during the second half of the total millis period.
  // This would work even if the function were only run once every 4.5 hours, though typically,
  //   the function should be called as frequently as possible to capture the actual moment of rollover.
  // The rollover counter is good for over 35 years of runtime. --Rob Faludi http://rob.faludi.com
  //
  static int numRollovers=0; // variable that permanently holds the number of rollovers since startup
  static boolean readyToRoll = false; // tracks whether we've made it halfway to rollover
  unsigned long now = millis(); // the time right now
  unsigned long halfwayMillis = 17179868; // this is halfway to the max millis value (17179868)

  if (now > halfwayMillis) { // as long as the value is greater than halfway to the max
    readyToRoll = true; // you are ready to roll over
  }

  if (readyToRoll == true && now < halfwayMillis) {
    // if we've previously made it to halfway
    // and the current millis() value is now _less_ than the halfway mark
    // then we have rolled over
    numRollovers = numRollovers++; // add one to the count the number of rollovers
    readyToRoll = false; // we're no longer past halfway
  }
  return numRollovers;
}

int stringToInt(String s)
     {
           int n;
           char carray[6];
           s.toCharArray(carray, sizeof(carray));
           n = atoi(carray);
           return n;
     }

void setup() {
  Serial.begin(28800);

  pinMode(lockPin,INPUT); /* DO NOT CHANGE TO OUTPUT!! */
  /*digitalWrite(lockPin, HIGH);*/

  pinMode(brightnessPin, INPUT);

  pinMode(failPin, OUTPUT);

  servo1.attach(servoPin);
  buffer = "";
  feedBuffer = "";
  pingTimer = fixedMillis();
  pingLEDTimer = fixedMillis();

  digitalWrite(failPin, LOW);
  Serial.println("HI "+String(fixedMillis()));
}
void processCommand(String buffer) {
   /* Confirm We Are The Sun */
   if(buffer.substring(0,1)==CMD_CONFIRM_CONNECTION) {
     say(RESPONSE_SUN_AVAILABLE);
   }

   /* Internal configuration */
   if(buffer.substring(0,1)==CMD_SET_SPEED) {
     updateSpeed(buffer.substring(1));
   }
   if(buffer.substring(0,1)==CMD_SET_TIC_LENGTH) {
     updateTic(buffer.substring(1));
   }
   if(buffer.substring(0,1)==CMD_SET_TIC_MULTIPLIER) {
     updateTicX(buffer.substring(1));
   }

   /* */
   if(buffer.substring(0,1)==CMD_READ_BRIGHTNESS) {
     readBrightness();
   }

   if(buffer.substring(0,1)==CMD_FULL_REWIND) {
     rewind();
   }

   if(buffer.substring(0,1)==CMD_STEP_FORWARD) {
     stepForward(cfg_tic, cfg_delay);
   }
   if(buffer.substring(0,1)==CMD_FAST_STEP_FORWARD) {
     stepForward(cfg_tic*cfg_tic_x, 0);
   }
   if(buffer.substring(0,1)==CMD_STEP_REWIND) {
     stepRew(cfg_tic, cfg_delay);
   }
   if(buffer.substring(0,1)==CMD_MEMORY) {
     check_mem();
     say("Memory "+String((long)heapptr)+" "+String((long)stackptr));
   }
   if(buffer.substring(0,1)==CMD_FAST_STEP_REWIND) {
     stepRew(cfg_tic*cfg_tic_x, 0);
   }
   if(buffer.substring(0,1)==RESPONSE_PING) {
     pingLEDTimer = fixedMillis();
     digitalWrite(failPin, LOW);
   }
}

boolean isLocked() {
  return digitalRead(lockPin) == HIGH;
}

void say(String s) {
  feedBuffer+=s+"\r\n";
}

void feed() {
  if(Serial.available()==0) { /* input queue empty */
    Serial.print(feedBuffer.substring(0,1));
    feedBuffer = feedBuffer.substring(1);
  }
}

void rewind() {
  servo1.write(cfg_base+(cfg_speed));

  int steps = 140;

  while(steps>0 && !isLocked()) {
    steps--;
    delay(cfg_tic);
  }

  servo1.write(cfg_base-cfg_speed);

  delay(cfg_tic*2);
  steps = 8;
  while(steps>0 && isLocked()) {
    servo1.write(cfg_base-cfg_speed);
    steps--;
    delay(cfg_tic);
    servo1.write(cfg_base);
    delay(100);
  }

  servo1.write(cfg_base);
  pos = 0;
  say(RESPONSE_SUN_DIMMED);
}

void readBrightness() {
  int brightness = analogRead(brightnessPin);
  say(RESPONSE_SUN_BRIGHTNESS+String(brightness));
}
void updateSpeed(String sp) {
  cfg_speed = stringToInt(sp);
  say("Speed = "+String(cfg_speed));
}

void updateTic(String sp) {
  cfg_tic = stringToInt(sp);
  say("Step = "+String(cfg_tic));
}

void updateTicX(String sp) {
  cfg_tic_x = stringToInt(sp);
  say("Step X = "+String(cfg_tic_x));
}

void stepForward(int length, int pause) {
  servo1.write(cfg_base-cfg_speed);
  delay(length);
  servo1.write(cfg_base);
  ff++;
  pos++;
  say(RESPONSE_SUN_POS+String(pos));

  delay(pause);
}

void stepRew(int length, int pause) {
  if(isLocked()) {
    say(RESPONSE_SUN_DIMMED);
    pos = 0;
    return;
  }
  servo1.write(cfg_base+cfg_speed);
  delay(length);
  servo1.write(cfg_base);
  rew++;
  pos--;
  say(RESPONSE_SUN_POS+String(pos));
  //Serial.println(RESPONSE_SUN_REW+String(rew));
  delay(pause);
}

void loop() {



  if ( Serial.available()) {
    char ch = Serial.read();
    if((ch!='\r')&&(ch!='\n')) {
      buffer += ch;
    }
    if(ch=='\n') {
      processCommand(buffer);
      buffer="";
    }
  }

  if(fixedMillis() - lightTimer>lightnessCheckPeriod) {
    lightTimer = fixedMillis();
    int newLightness = analogRead(brightnessPin);
    if(newLightness-lightness>LIGHT_TRESHOLD) {
      say(RESPONSE_FLASH);
    }
    lightness = newLightness;
  }
  if(fixedMillis() - pingTimer>pingTimerPeriod) {
    pingTimer = fixedMillis();
    say(RESPONSE_PING);
  }
  if(fixedMillis() - pingLEDTimer>pingTimerPeriod*2) {
    pingLEDTimer = fixedMillis();
    digitalWrite(failPin, HIGH);
  }
  feed();

  if(roll<millisRollover()) {
    say("ROLL "+roll);
    roll=millisRollover();
  }


}