#include <Servo.h>

#define commTimeout 300 //ms
#define cutoff 3.2
#define temp A5
#define cell1 A3
#define cell2 A4
#define cell3 A2
#define cell4 A1
#define STEERING_MIN 60
#define STEERING_MAX 160

#define steeringOffsetPin A0
#define relayPin 8
#define relayPin2 7
#define steeringPin 3
#define drivePin 5

#define TX_INTERVAL 100 //ms
#define DELAY 10  //ms

unsigned long old_tx;
int cells[4];
Servo steering;
int steeringOffset;
int steeringPos;
int steeringCalc;
float sFactorMin;
float sFactorMax;

int drive;
int brake;
boolean relay;
boolean relay2;
boolean setDelay;
boolean resetDelay;

boolean rumbleToggle=false;

unsigned long timeout;
unsigned long delayTimer;

char inChar;
String inStr;
String retStr;

char buff[15];

void initializePins(){
   pinMode(drivePin,OUTPUT);
   digitalWrite(drivePin,LOW);
   pinMode(relayPin,OUTPUT);
   digitalWrite(relayPin,LOW);
   pinMode(relayPin2,OUTPUT);
   digitalWrite(relayPin2,LOW);
   steering.attach(steeringPin);
}

/*-------------------------------------------Main loop---------------------------------------*/
void setup(){
  initializePins();
  Serial.begin(57600);
}

void loop(){
  getBatt();
  sendData();
  getInputs();
  checkTimeout();
  calcSteering();
  steering.write(steeringCalc);
  setOutputs();
}

/*-----------------------------------------Main functions---------------------------------*/
//  127000000

void checkTimeout(){
  if(millis()>timeout+commTimeout){
    drive=0;
    steeringPos=127;
    relay=0;
    relay2=1;
  }
}

void getBatt(){
  cells[0]=(analogRead(cell1)*0.645);
  cells[1]=(analogRead(cell2)*1.200);
  cells[2]=(analogRead(cell3)*2.519);
  cells[3]=(analogRead(cell4)*1.977);
  cells[3]-=cells[2];
  cells[2]-=cells[1];
  cells[1]-=cells[0];
  for(int i=0;i<4;i++){
    if(cells[i]<0)
      cells[i]=0;
    if(cells[i]<=(cutoff*100))
      rumbleToggle=true;
  }
}

void getInputs(){
  if(Serial.available()>9){
    inStr="";
    do{
      inChar=Serial.read();
      inStr+=inChar;
    }while(inChar!='\n');
    if(inStr.length()==10){
      inStr.substring(0,3).toCharArray(buff,4);
      steeringPos=atoi(buff);
      inStr.substring(3,6).toCharArray(buff,4);
      drive=atoi(buff);
      inStr.substring(6,8).toCharArray(buff,3);
      relay=atoi(buff);
      inStr.substring(8,9).toCharArray(buff,2);
      relay2=atoi(buff);
      timeout=millis();
    }
  }
}

void calcSteering(){
  steeringOffset=(analogRead(steeringOffsetPin)/8)+64;
  sFactorMin=127.0/(steeringOffset-STEERING_MIN);
  sFactorMax=127.0/(STEERING_MAX-steeringOffset);
  steeringCalc=(steeringOffset+((127-steeringPos)/((steeringPos<=127)? sFactorMax:sFactorMin)));
}

void sendData(){
  if(millis()>=old_tx+TX_INTERVAL){
    retStr="";
    for(int i=0;i<4;i++){
      retStr+=pad(cells[i]);
    }
    retStr+=rumbleToggle;
    retStr+=pad(analogRead(temp),4);
    retStr+="\n";
    Serial.print(retStr);
    old_tx=millis();
  }
}

void setOutputs(){
  if(relay2){
    digitalWrite(relayPin2,HIGH);
    digitalWrite(relayPin,LOW);
  }
  else if(relay){
    digitalWrite(relayPin2,HIGH);
    digitalWrite(relayPin,HIGH);
    if(!setDelay&&!resetDelay){
      delayTimer=millis();
      setDelay=true;
    }
  }
  else{
    digitalWrite(relayPin2,LOW);
    digitalWrite(relayPin,LOW);
    resetDelay=false;
  }
  analogWrite(drivePin,drive);
  /*
  if(!setDelay){
    analogWrite(drivePin,drive);
  }
  else if(millis()>delayTimer+DELAY){
    setDelay=false;
    resetDelay=true;
  }*/
}

String pad(int number){
  return pad(number,3);
}
String pad(int number,int digits){
  String ret="";
  for(int i=0; i < (digits-1) - byte(log10(number)); i++)
    ret+='0';
  ret+=number;
  return ret;
}