//Title: servo_interrupt_test
//
//Programmer: Joe Morrow
//Contact: jmmorrow@mit.edu
//Date: Nov 28, 2011
//
//Description: Testing out the Servo library to control JX-1

#include <Servo.h>

//Transmitter Stuff
#include <string.h>
#define MAXTOKENS 5 //characters between commas
#define MAXSTRING 70 //total characters in transmission X2
#define MAXVALUES 35 //total characters after ";"
char *tokens[MAXTOKENS];
char buffer[MAXSTRING];
char values[MAXVALUES];
char *pch;
char *last;
int flagA;
int flagB;
float pwThrottle;
float rollRad;
float pitchRad;
float yawRad;
float ctrlSetting1;
float ctrlSetting2;

Servo servo1;
Servo servo2;
Servo servo3;

void setup() {
  servo1.attach(13);
  servo2.attach(7);
  servo3.attach(6);
  Serial.begin(9600);
}

void loop() {


/////RECEIVE COMMAND FROM PILOT/////
    flagA = 0;
    flagB = 0; 
    if(Serial.available() >= MAXSTRING) {
      for(int i = 0; i < MAXSTRING; i++) {
        buffer[i] = Serial.read();
        if(i == (MAXSTRING - 1)) {
          flagA = 1;
          goto PARSE;
        }
      }
    } 
PARSE: 
    if(flagA == 1) {
      for(int i = 0; i < MAXSTRING; i++) {
        if (buffer[i] == ';') {
          for(int j = 0; j < MAXVALUES; j++) {
            values[j] = buffer[i+1+j];
          }
          flagB = 1;
          goto CONVERT;
        }
      }
    }
CONVERT:
    if(flagB == 1)  {
      int i = 0;
      for((pch = strtok_r(values,",",&last)); pch; (pch = strtok_r(NULL, ",", &last)), i++) {
        if(i < MAXTOKENS) {
          tokens[i] = pch;
        }
      }
      tokens[i] = NULL;
      pwThrottle = atof(tokens[0])*100.0;
      rollRad = atof(tokens[1]);
      pitchRad = atof(tokens[2]);
      yawRad = atof(tokens[3]);
      ctrlSetting1 = atof(tokens[4]);
      ctrlSetting2 = atof(tokens[5]);
      Serial.flush();
    }

//  Serial.print(pwRoll);
//  Serial.print(" ");
//  Serial.print(pwPitch);
//  Serial.print(" ");
//  Serial.println(pwYaw);

  float pwPitch = pitchRad*500 + 1500;
  float pwRoll = rollRad*500 + 1500;
  float pwYaw = yawRad*500 + 1500;
  float pwThrottle = constrain(pwThrottle,1000,2000);
  pwPitch = constrain(pwPitch,1000,2000);
  pwRoll = constrain(pwRoll,1000,2000);
  pwYaw = constrain(pwYaw,1000,2000);

//  if (pwPitch > 1600) {
//    digitalWrite(13,HIGH);
//  } else {
//    digitalWrite(13,LOW);
//  }
//Serial.println(pwPitch);
  
//  int val = analogRead(innerPot);
//  val = map(val,0,1023,1000,2000);
//  val = constrain(val,1000,2000);
  servo1.writeMicroseconds(pwPitch);
  servo2.writeMicroseconds(pwRoll);
//  servo3.writeMicroseconds(val);
//  delay(10);

}