// based on  http://code.google.com/p/generate-ppm-signal/
// gplv2

//////////////////////CONFIGURATION///////////////////////////////
#define chanel_number 4  //set the number of chanels
#define default_servo_value 1500  //set the default servo value
#define PPM_FrLen 22500  //set the PPM frame length in microseconds (1ms = 1000µs)
#define PPM_PulseLen 280  //set the pulse length
#define onState 0  //set polarity of the pulses: 1 is positive, 0 is negative
#define sigPin 7  //set PPM signal output pin on the arduino
//////////////////////////////////////////////////////////////////

#define DEBUG

#include <Esplora.h>


int xTrim=-50; //works for my microcrawler
int yTrim=-100; //works for my microcrawler
int yinvert=1;
int xinvert=1;

/*this array holds the servo values for the ppm signal
 change theese values in your code (usually servo values move between 1000 and 2000)*/
int ppm[chanel_number];

void setup(){
#ifdef DEBUG  
  Serial.begin(9600);
#endif
  //initialize default ppm values
  for(int i=0; i<chanel_number; i++){
    ppm[i]= default_servo_value;
  }

  pinMode(sigPin, OUTPUT);
  digitalWrite(sigPin, !onState);  //set the PPM signal pin to the default state (off)
  
  if (!Esplora.readButton(3)) { yinvert=-1; }  // up
  if (!Esplora.readButton(2)) { xinvert=-1; }  // left
  
  cli();
  TCCR1A = 0; // set entire TCCR1 register to 0
  TCCR1B = 0;
  
  OCR1A = 100;  // compare match register, change this
  TCCR1B |= (1 << WGM12);  // turn on CTC mode
  TCCR1B |= (1 << CS11);  // 8 prescaler: 0,5 microseconds at 16mhz
  TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
  sei();
}

void loop(){
  
   int xAxis = map(Esplora.readJoystickX()*xinvert,-512,512,1900,1100);
   int yAxis = map(Esplora.readJoystickY()*yinvert,-512,512,1900,1100);
  
   if (Esplora.readButton(1)) { yTrim++; }
   if (Esplora.readButton(2)) { xTrim++; } 
   if (Esplora.readButton(3)) { yTrim--; }
   if (Esplora.readButton(4)) { xTrim--; }
  
    #ifdef DEBUG
      Serial.print ("xAxis:");
      Serial.print (xAxis);
      Serial.print (" yAxis:");
      Serial.print (yAxis);
      Serial.print (" xTrim:");
      Serial.print (xTrim);
      Serial.print (" yTrim:");
      Serial.print (yTrim);
      Serial.print ("\n");
    #endif  
  
  //put main code here
  
  ppm[0] = xAxis+xTrim;
  ppm[1] = yAxis+yTrim;
  
  delay(15);
}

ISR(TIMER1_COMPA_vect){  //leave this alone
  static boolean state = true;
  
  TCNT1 = 0;
  
  if(state) {  //start pulse
    digitalWrite(sigPin, onState);
    OCR1A = PPM_PulseLen * 2;
    state = false;
  }
  else{  //end pulse and calculate when to start the next pulse
    static byte cur_chan_numb;
    static unsigned int calc_rest;
  
    digitalWrite(sigPin, !onState);
    state = true;

    if(cur_chan_numb >= chanel_number){
      cur_chan_numb = 0;
      calc_rest = calc_rest + PPM_PulseLen;// 
      OCR1A = (PPM_FrLen - calc_rest) * 2;
      calc_rest = 0;
    }
    else{
      OCR1A = (ppm[cur_chan_numb] - PPM_PulseLen) * 2;
      calc_rest = calc_rest + ppm[cur_chan_numb];
      cur_chan_numb++;
    }     
  }
}