//Sequential PPM read-in, Serial out

#define INPIN 2
#define NUM_OF_CHANNELS 6
#define TIMEOUT 9000
#define OFFSET 600

//5pcb
#define CENTER 127

int channels[NUM_OF_CHANNELS];
byte bytechannels[NUM_OF_CHANNELS];
boolean is_high_waiting = false;
boolean go_again = false;
byte ch = 0;

void setup() {
  pinMode(INPIN,INPUT);
//  Serial.begin(38400);
  Serial.begin(57600);
}

//Provides PPM values in an array channels[]. Reads high period of PPM only
// __|¯1¯¯¯¯|__|¯2¯¯|_|¯3¯¯¯¯¯¯¯¯|_...|¯¯¯¯¯¯¯¯¯¯¯¯¯9ms¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯|__|¯1¯¯¯|__
//Low periods are constant (400us), high periods between 600 and 1600 uS with center of 1100
//Long refresh pulse of > 9ms after channels
//Outputs raw values only - a refresh pulse may be captured at the very first instant before
//syncinc. This should be handled appropriately by the receiver
void loop() {
  if(!is_high_waiting) {
    //Loop initial condition: Start taking in pulses indiscrimately
    for(ch = 0; ch < NUM_OF_CHANNELS; ch++) {
      channels[ch] = pulseIn(INPIN,HIGH,20000);
      //If an abormally long pulse is found, sync to it
      if(channels[ch] > TIMEOUT) {
        go_again = true; //The last pulse captured was the high idle period, so bail and try again
        break;
      } else {
        go_again = false;
      }
    } //Once the NUM_OF_CHANNELS channels have finished collecting, automatically bail
    if(go_again) {
      is_high_waiting = false;
    } else {
      is_high_waiting = true;
    }
  }
  //Do other shit here that takes longer than 300uS but less than 9ms, like a bunch of Serial writes
  //Skipped if "go_again" is set, inhibiting the start of the wait period
  if(is_high_waiting) {
    
    //Pack the channel data into 1 byte (0-255) for easy Serial.write. What needs more than
    //255 positions anyway?
    for(ch = 0; ch < NUM_OF_CHANNELS ; ch++) {
      bytechannels[ch] = (channels[ch] - OFFSET) / 4;
//      Serial.print(bytechannels[ch]);
//      Serial.print('\t');
    } 
//    Serial.print('\n');
    
    //5pcb
    //Data packet form: 255 THR ELE AIL RUD
    //Mode 2: 
    //0 -> AIL  1 -> ELE  2 -> THR  3 -> RUD
    //Mode 4:
    //0 -> RUD  1 -> ELE  2 -> THR  3 -> AIL
    Serial.write(255);
    Serial.write(bytechannels[2]); //THR
    Serial.write(bytechannels[1]); //ELE
    Serial.write(bytechannels[3]); //AIL
    Serial.write(bytechannels[0]); //RUD

    
    //After everything, wait a litle bit
    delayMicroseconds(400);
    is_high_waiting = false;
    go_again = false;
  }
}