#include <Servo.h>

#define BAUD_RATE 57600

#define INPUT_DIGIT_LIMIT 5
#define STEERING_MIN 50
#define STEERING_MAX 115

#define START_CODE 300

#define Pin_ledR 11
#define Pin_ledG 3
#define Pin_ledB 6
#define Pin_headlights 9
#define Pin_brakes 4
#define Pin_steering 10
#define Pin_drive 5
#define Pin_relay 2
#define Pin_MainV A3
#define Pin_AuxV A2
#define Pin_centerPot A1

#define Datapacket_steering 0
#define Datapacket_drive 1
#define Datapacket_relay 2
#define Datapacket_headlights 3
#define Datapacket_brakes 4
#define Datapacket_ledR 5
#define Datapacket_ledG 6
#define Datapacket_ledB 7

#define LED_MOD_COUNT 100

//Data packet: 127:000:0:00:0:000:000:000:


int data[8];
int data_old[8];

Servo steering;
float sFactorMin;
float sFactorMax;
int steeringOffset;
int headlightTimer;
int headlightTimer2;
int ledCounter;
int LedI;

int counterA;
int counterB;
char inputByte;
char inputBytes[INPUT_DIGIT_LIMIT+1];

unsigned long time;
unsigned long timeB;
int c;



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

  steeringOffset=(analogRead(Pin_centerPot)/8)+64;
  steering.write(steeringOffset/1.417);
  Serial.print("Steering Center: ");
  Serial.print(steeringOffset);
  sFactorMin=127.0/((steeringOffset/1.417)-STEERING_MIN);
  sFactorMax=127.0/(STEERING_MAX-(steeringOffset/1.417));
  Serial.print(" sFmin: ");
  Serial.print(sFactorMin);
  Serial.print(" sFmax: ");
  Serial.println(sFactorMax);

  TCCR0B = TCCR0B & 0b11111000 | 0x01;
  pinMode(Pin_ledR, OUTPUT);
  pinMode(Pin_ledG, OUTPUT);
  pinMode(Pin_ledB, OUTPUT);
  pinMode(Pin_brakes, OUTPUT);
  pinMode(Pin_drive, OUTPUT);
  pinMode(Pin_relay, OUTPUT);
  steering.attach(Pin_steering);

  Serial.println("Waiting for start code");
  while(getInput(0)!=START_CODE);
  Serial.println("RC control started!");
  Serial.println("Data packet: 127:000:0:00:0:000:000:000:");
  updateValues();
}

void loop()
{
  if(Serial.available()>0){
    for(int i=0;i<8;i++)
      data_old[i]=data[i];
    updateValues();
    if(data_old[Datapacket_steering]!=data[Datapacket_steering ])
      steering.write(max(STEERING_MIN,min(STEERING_MAX,(steeringOffset/1.417)+((data[Datapacket_steering]-127)/((data[Datapacket_steering]>=127)? sFactorMax:sFactorMin)))));
    if(data_old[Datapacket_drive ]!=data[Datapacket_drive  ])
      analogWrite(Pin_drive, data[Datapacket_drive]);
    if(data_old[Datapacket_relay ]!=data[Datapacket_relay  ])
      digitalWrite(Pin_relay, data[Datapacket_relay]);
    if(data_old[Datapacket_brakes ]!=data[Datapacket_brakes  ])
      digitalWrite(Pin_brakes, data[Datapacket_brakes]);
  }
  Headlights();
  Underglow();

  //getDebug();
}

void getDebug(){
  c++;
  if(c==100){
    timeB=time;
    time=millis();
    Serial.println((time-timeB)/100);
    c=0;
    getVoltages();
  }
}

void updateValues(){
  counterA=0;
  counterB=0;
  do{
     while(!(Serial.available()));
       inputByte=Serial.read();
       if(inputByte==':'){
         inputBytes[counterA]='\n';
         data[counterB]=atoi(inputBytes);
         counterB++;
         counterA=0;
       }
       else{
         inputBytes[counterA++]=inputByte;
       }
       if(checkDelim(inputByte)||counterA==INPUT_DIGIT_LIMIT+1)
         break;
  }while(true);
}

float Vaux(float in){
  return((in/1024)*12.202);
}
float Vmain(float in){
  return((in/1024)*12.18);
}
void getVoltages(){
    Serial.print("V Aux: ");
    Serial.print(Vaux(analogRead(Pin_AuxV)));
    Serial.print(" V Main: ");
    Serial.print(Vmain(analogRead(Pin_MainV)));
}

void Headlights(){
  if(data[Datapacket_headlights ]==0){
    digitalWrite(Pin_headlights,LOW);
  } else if(data[Datapacket_headlights ]==99){
    digitalWrite(Pin_headlights,HIGH);
  } else{
    if(headlightTimer<=data[Datapacket_headlights ]){
      digitalWrite(Pin_headlights,HIGH);
    } else{
      digitalWrite(Pin_headlights,LOW);
    }
    headlightTimer++;
    if(headlightTimer==100)
      headlightTimer=0;
  }
}


void Underglow(){
  if(data[Datapacket_ledR]>=256||data[Datapacket_ledG]>=256||data[Datapacket_ledB]>=256){
    if(ledCounter==LED_MOD_COUNT){
      ledCounter=0;
      if(LedI>=1536)
        LedI=0;
      fadeRGB2(LedI);
      LedI++;
    }
    else
      ledCounter++;
  }
  else{
    if(data_old[Datapacket_ledR ]!=data[Datapacket_ledR ]||data[Datapacket_ledR ]==0)
      analogWrite(Pin_ledR,data[Datapacket_ledR ]);
    if(data_old[Datapacket_ledG ]!=data[Datapacket_ledG ]||data[Datapacket_ledG ]==0)
      analogWrite(Pin_ledG,data[Datapacket_ledG ]);
    if(data_old[Datapacket_ledB ]!=data[Datapacket_ledB ]||data[Datapacket_ledB ]==0)
      analogWrite(Pin_ledB,data[Datapacket_ledB ]);
  }
}
void fadeRGB2(int i){
  if(i==0){
    analogWrite(Pin_ledR,255);
  }
  else if(i<256){
    analogWrite(Pin_ledG,i);
  }
  else if(i>256&&i<512){
    analogWrite(Pin_ledR,512-i);
  }
  else if(i>512&&i<768){
    analogWrite(Pin_ledB,i-512);
  }
  else if(i>768&&i<1024){
    analogWrite(Pin_ledG,1024-i);
  }
  else if(i>1024&&i<1280){
    analogWrite(Pin_ledR,i-1024);
  }
  else if(i>1280&&i<1536){
    analogWrite(Pin_ledB,1536-i);
  }
}

int getInput(int passthrough){
  counterA=0;
   if(Serial.available()){
     do{
       while(!Serial.available());
         inputByte=Serial.read();
         inputBytes[counterA++]=inputByte;
         if(checkDelim(inputByte)||counterA==INPUT_DIGIT_LIMIT+1)
           break;
     }while(true);
     inputBytes[counterA]='\n';
     return atoi(inputBytes);
   }
   return passthrough;
}

boolean checkDelim(int code){
  if(code==10||code==13)
    return true;
  return false;
}