#include <GP2D02.h>
 
//comment this out to see the demodulated waveform
//it is useful for debugging purpose.
#define MODULATED 1
 
//Pins
const int IR_PIN = 3; //Infrared LED pin
const int vIn = 8; //Green Wire of GP2D02
const int vOut = 9; //Yellow Wire of GP2D02
 
//For signal generation. Lengths are in microseconds.
const int HEADER_DURATION = 2000; //Length of header pulse
const int HIGH_DURATION = 380; //Length of high part of 0/1-pulse
const int ZERO_LOW_DURATION = 220; //Length of low part of 0-pulse
const int ONE_LOW_DURATION = 600; //Length of low part of 1-pulse
const byte ROTATION_STATIONARY = 60; // Trim
const byte CAL_BYTE = 52; //Trailing pulse signature
 
//For fitness calculation
int heightCM = 12; //Input the height you want the program to attempt to reach
int heightIRvalue = 447.54*pow(heightCM,-0.407); // Conver CM to the scale that the GP2D02 outputs at
int bestFitness = 1000; // starting best fitness is far from zero
int currentFitness = 1000; // starting fitness is far from zero
int runNum = 0; //Tracks number of runs
int currentIRValue = 0; //Store GP2D02 IRValue
int minIR = 0; //Minimum position IR Value
int maxIR = 0; //Maximum position IR Value
int lastHighThrottle = 255; // Starts last fit high throttle at max range
int lastLowThrottle = 0; // Starts last low high throttle at min range
int lastThrottle = 0; // The value of the throttle on the last run
 
//For timed loop of one run
int starttime = 0; 
int endtime = 0; 
 
//For intial SYMA 107 signal parameters
int Throttle = 0;
int LeftRight = 0;//-64 to 63
int FwdBack = 0;//-128 to 127
 
//Initialize the IR sensor
GP2D02 sensor(vIn,vOut);
 
void sendHeader() //Sends the header pulse that singals the begining of a command
{
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HEADER_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
 
  delayMicroseconds(HEADER_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HIGH_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendZero() //Sends one 0 pulse
{
  delayMicroseconds(ZERO_LOW_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else 
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HIGH_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendOne() //Sends one 1 pulse
{
  delayMicroseconds(ONE_LOW_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HIGH_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendCommand(int throttle, int leftRight, int forwardBackward)
{ //Converts SYMA Parameters to a signal
  //For loops convert decimal numbers to binary numbers
  //Sends a pulse for each 0 or 1.
  byte b;
 
  sendHeader();
 
  for (int i = 7; i >=0; i--) 
  { // Yaw part of signal
    b = ((ROTATION_STATIONARY + leftRight) & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
 
  for (int i = 7; i >=0; i--)
  { //Pitch part of signal
    b = ((63 + forwardBackward) & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
 
  for (int i = 7; i >=0; i--)
  { //Throttle part of signal
    b = (throttle & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
 
  for (int i = 7; i >=0; i--)
  { //Trailing Pulses
    b = (CAL_BYTE & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
}
 
void setup()
{ //Initializes the program
  Serial.begin(9600); //Baud rate for command line
  pinMode(IR_PIN, OUTPUT); //Initializes the LED pin
  digitalWrite(IR_PIN, LOW);
 
  //setup PWM: f=38Khz PWM=0.5 
  byte v = 8000 / 38;
  TCCR2A = _BV(WGM20);
  TCCR2B = _BV(WGM22) | _BV(CS20);
  OCR2A = v;
  OCR2B = v / 2;
  minIR = sensor.getRawRange(); //get lowest point IR reading
  maxIR = 3000000*pow(minIR,-2.409)+11; //Convert to centimeters and add range of height of boom in cm
  maxIR = 447.54*pow(maxIR,-0.407); //Convert to scaled IR value
  Serial.println("Program Start");
  Serial.print("Height to reach: ");
  Serial.println(heightIRvalue);
}
 
void loop() //main program loop
{ 
  randomSeed(millis());
  runNum = runNum + 1;
  if(runNum>1){ //Runs after first
  Serial.println("runNum>1");
    if(bestFitness>5){ //if last fitness was high then new random is a smaller number
      Throttle = random(lastThrottle,lastHighThrottle);//0 to 255
      lastLowThrottle = lastThrottle;
      Serial.println("bF>5");
    }
    else if(bestFitness<-5){ //if last fitness was low then new random is larger number
      Throttle = random(lastLowThrottle,lastThrottle);
      lastHighThrottle = lastThrottle;
       Serial.println("bF>5");
    }
    else{ // 0 is the best fitness, this is the final part of the program
        Serial.println("\nFinal Values");
        Serial.print("Throttle: ");
        Serial.println(lastThrottle);
        Serial.print("Number of Runs: ");
        Serial.println(runNum);
      while(1){
        sendCommand(lastThrottle, LeftRight, FwdBack); //generates IR signal
        //end of program
        //hangs indefinately
      }
    }
  }
  else{ //Initial Run
  Serial.println("runNum1");
    Throttle = random(255);
  }
  Serial.print("\nNumber of Runs: ");
  Serial.println(runNum);
  Serial.print("Throttle: ");
  Serial.println(Throttle);
  
  lastThrottle = Throttle;
  starttime = millis();
  endtime = starttime;
  // do this loop for up to 5000mS
  while ((endtime - starttime) <=5000) 
  { //Runs take place over a period of 5 seconds
    sendCommand(Throttle, LeftRight, FwdBack); //generates IR signal
    endtime = millis();
    currentIRValue = sensor.getRawRange();
    currentFitness = currentIRValue-heightIRvalue;
  }
  if(abs(currentFitness)<abs(bestFitness)){
    bestFitness = currentFitness; //If new fitness is closer to zero, replace old fitness
  }
  Serial.print("Current IR Value: ");
  Serial.println(currentIRValue);
  Serial.print("Current Fitness: ");
  Serial.println(bestFitness);
  delay(3000); //wait to lower to lowest position.
}