#include <Servo.h>

#define CMD_FULL_REWIND "#"

#define CMD_CONFIRM_CONNECTION "V"

#define CMD_MEMORY "M"

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

#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_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;

void setup() {
  Serial.begin(28800);
  pinMode(lockPin,INPUT);
  pinMode(brightnessPin, INPUT);
  pinMode(failPin, OUTPUT);
  servo1.attach(servoPin);
  buffer = "";
  feedBuffer = "";
  digitalWrite(failPin, LOW);

}
void processCommand(String buffer) {
   /* Confirm We Are The Sun */
   if(buffer.substring(0,1)==CMD_CONFIRM_CONNECTION) {
     say(RESPONSE_SUN_AVAILABLE);
   }

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

   if(buffer.substring(0,1)==CMD_MEMORY) {
     check_mem();
     say("Memory "+String((long)heapptr)+" "+String((long)stackptr));
   }

}

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 loop() {

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

  feed();
}