#include <Servo.h>

#define DEBUG true
#define STATE_KEYBOARD 0
#define STATE_RF 1

#define DEFAULT_STATE STATE_KEYBOARD

#define PIN_SERVO_TAIL 9
#define PIN_MOTOR_TAIL 5
#define PIN_MOTOR_CENTER 6

#define SERVO_TAIL_STARTING_ANGLE 90
#define SERVO_TAIL_MIN_ANGLE 0
#define SERVO_TAIL_MAX_ANGLE 190

#define MOTOR_TAIL_MIN_POWER 90
#define MOTOR_TAIL MAX_POWER 180
#define MOTOR_CENTER_MIN_POWER 90
#define MOTOR_CENTER_MAX_POWER 180

#define BAUD_RATE 57600
#define CHAR_NEWLINE 10
#define BUFFER_SIZE 16

#define SWITCH_STATES 'c'
#define TYPE_MOTOR 'm'
#define TYPE_SERVO 's'

#define TYPE_INDEX 0
#define NUMBER_INDEX 1
#define VALUE_INDEX 3

// type[# value]

// examples:
// m1 90 (motor 1, set power to 90)
// s1 90 (servo 1, set angle to 90)
// c (switch states)

Servo tailServo;
Servo tailMotor;
Servo centerMotor;

int state;
char serialInput[BUFFER_SIZE];
int serialIndex; 
char tmpChar;

/**
 * Initialize
 **/
void setup() {
  Serial.begin(BAUD_RATE);
 
  tailServo.attach(PIN_SERVO_TAIL);
  tailMotor.attach(PIN_MOTOR_TAIL);
  centerMotor.attach(PIN_MOTOR_CENTER);

  // make sure the engines are off
  tailMotor.write(MOTOR_TAIL_MIN_POWER);
  centerMotor.write(MOTOR_CENTER_MIN_POWER);
 
  // set default starting angle for the tail
  tailServo.write(SERVO_TAIL_STARTING_ANGLE);
  
  // set default values
  state = DEFAULT_STATE;
  serialIndex = 0;
  
  if (DEBUG) {
    Serial.println("initialization complete");
    Serial.println("");
    Serial.println("Protocol:");
    Serial.println("type[# value]");
    Serial.println("");
    Serial.println("examples:");
    Serial.println("m1 90 (motor 1, set power to 90)");
    Serial.println("s1 90 (servo 1, set angle to 90)");
    Serial.println("c (switch states)");
  }
}

/**
 * Main loop
 **/
void loop() {
  switch (state) {
    case STATE_KEYBOARD:
      handleKeyboard();
      break;
      
    case STATE_RF:
      handleRF();
      break;
  }
}

/**
 * Handle incoming commands
 *
 * @param *input
 * @param size The length of input
 */
void handleInputCommand(char *input, int size) {
  if (size == 0) {
    return;
  }
  
  char type = input[TYPE_INDEX];
  if (DEBUG) {
    Serial.print("Type: ");
    Serial.println(type);
  }
  
  if (type == SWITCH_STATES) {
    // change states
    state = STATE_RF;
    if (DEBUG) {
      Serial.println("-- switching to RF state");
    } 
  } else if (size > VALUE_INDEX) {
    Servo outputDevice; 
    // select output device
    if (type == TYPE_MOTOR) {
      if (input[NUMBER_INDEX] == '1') {
        outputDevice = tailMotor;
        if (DEBUG) {
          Serial.println("-- tail motor selected");
        }
      } else {
        outputDevice = centerMotor;
        if (DEBUG) {
          Serial.println("-- center motor selected");
        }
      }
    } else {
      outputDevice = tailServo;
      if (DEBUG) {
        Serial.println("-- tail servo selected");
      }
    }
    
    // set output device value
    int value = atoi(input + VALUE_INDEX);
    if (DEBUG) {
      Serial.print("value: ");
      Serial.println(value);
    }
    
    outputDevice.write(value);
  }  
}

/**
 * Parse serial input and pass it forward to the handling module
 **/
void handleKeyboard() {  
  if (Serial.available() > 0) {      
      tmpChar = Serial.read();
      if (tmpChar == CHAR_NEWLINE) {
        handleInputCommand(serialInput, serialIndex);
        
        // clear saved input
        serialIndex = 0;
        serialInput[serialIndex] = '\0';
      } else {
        serialInput[serialIndex++] = tmpChar;
        serialInput[serialIndex] = '\0'; // null terminate the string
        
        if (serialIndex > BUFFER_SIZE - 2) {
          if (DEBUG) {
            if (Serial.available() > 0) {
              Serial.println("exceeded buffer size, ignoring some input!");
            }  
          }
          
          // clear buffer and saved input
          while (Serial.read() != -1);
          serialIndex = 0;
          serialInput[serialIndex] = '\0';  
        }
      }
  }
}


/**
 * Parse radio input and pass it forward to the handling module
 **/
void handleRF() {  
  // TODO  
  Serial.println("RF state not implemented, switching back to keyboard");
  state = STATE_KEYBOARD;
}