#define BAUDRATE     57600
#define MAX_PWM        255

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "commands.h"
#include "motor_driver.h"
#include "encoder_driver.h"
#include "diff_controller.h"
#include <Wire.h>
#include "I2Cdev.h"
#include "AK8975.h"
#include "MPU6050.h"
#include "Kalman.h"


Kalman filter;
AK8975 mag(0x0C);
MPU6050 accelgyro;
float batt_lvl;
int16_t gx;
int16_t mx, my, mz;
int dt;


#define PID_RATE           30     // Hz
const int PID_INTERVAL = 1000 / PID_RATE;
unsigned long nextPID = PID_INTERVAL;
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;
int arg = 0;
int index = 0;
char chr;
char cmd;
char argv1[16];
char argv2[16];
long arg1;
long arg2;
float heading = 0.0;

void resetCommand() {
  cmd = NULL;
  memset(argv1, 0, sizeof(argv1));
  memset(argv2, 0, sizeof(argv2));
  arg1 = 0;
  arg2 = 0;
  arg = 0;
  index = 0;
}

int runCommand() {
  int i = 0;
  char *p = argv1;
  char *str;
  int pid_args[4];
  arg1 = atoi(argv1);
  arg2 = atoi(argv2);

  switch(cmd) {
  case GET_BAUDRATE:
    Serial.println(BAUDRATE);
    break;

  case READ_ENCODERS:
    Serial.print(readEncoder(LEFT));
    Serial.print(" ");
    Serial.println(readEncoder(RIGHT));
    break;

   case RESET_ENCODERS:
    resetEncoders();
    resetPID();
    Serial.println("OK");
    break;

  case MOTOR_SPEEDS:
    /* Reset the auto stop timer */
    lastMotorCommand = millis();
    if (arg1 == 0 && arg2 == 0) {
      setMotorSpeeds(0, 0);
      resetPID();
      moving = 0;
    }
    else moving = 1;
    leftPID.TargetTicksPerFrame = arg1;
    rightPID.TargetTicksPerFrame = arg2;
    Serial.println("OK");
    break;

  case UPDATE_PID_L:
    while ((str = strtok_r(p, ":", &p)) != '\0') {
       pid_args[i] = atoi(str);
       i++;
    }
    leftPID.Kp = pid_args[0];
    leftPID.Kd = pid_args[1];
    leftPID.Ki = pid_args[2];
    leftPID.Ko = pid_args[3];
    Serial.println("OK");
    break;

  case UPDATE_PID_R:
    while ((str = strtok_r(p, ":", &p)) != '\0') {
       pid_args[i] = atoi(str);
       i++;
    }
    rightPID.Kp = pid_args[0];
    rightPID.Kd = pid_args[1];
    rightPID.Ki = pid_args[2];
    rightPID.Ko = pid_args[3];
    Serial.println("OK");
    break;

  case DISP_PID_P:
    Serial.println(leftPID.Kp);
    Serial.println(leftPID.Kd);
    Serial.println(leftPID.Ki);
    Serial.println(leftPID.Ko);
    Serial.println(rightPID.Kp);
    Serial.println(rightPID.Kd);
    Serial.println(rightPID.Ki);
    Serial.println(rightPID.Ko);
    Serial.println("OK");
    break;
  case BATTERY_LVL:
	  Serial.println((int)(batt_lvl * 100));
	  break;
  case IMU:
    Serial.println((int)(heading * 100));
    break;
  default:
    Serial.println("Invalid Command");
    break;
  }
}

/* Setup function--runs once at startup. */
void setup() {
  Wire.setClock(400000);
  Wire.begin();
  Serial.begin(BAUDRATE);
  
  accelgyro.initialize();
  accelgyro.setI2CBypassEnabled(true);
  mag.initialize();
  mag.testConnection();
  delay(100);
  mag.getHeading(&my, &mx, &mz);
  delay(10);
  mag.getHeading(&my, &mx, &mz);
  
  heading = atan2(-mz, my) * RAD_TO_DEG;
  
  filter.setQangle(0.0001);
  filter.setQbias(0.003);
  filter.setRmeasure(0.05);
  filter.setAngle(heading);

    pinMode(A0,INPUT);
    digitalWrite(A0,1);
    batt_lvl = analogRead(A0);
    DDRC &= ~(1<<LEFT_ENC_PIN_A);
    DDRC &= ~(1<<LEFT_ENC_PIN_B);
    DDRD &= ~(1<<RIGHT_ENC_PIN_A);
    DDRD &= ~(1<<RIGHT_ENC_PIN_B);

    //enable pull up resistors
    PORTC |= (1<<LEFT_ENC_PIN_A);
    PORTC |= (1<<LEFT_ENC_PIN_B);
    PORTD |= (1<<RIGHT_ENC_PIN_A);
    PORTD |= (1<<RIGHT_ENC_PIN_B);

    // tell pin change mask to listen to left encoder pins
    PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
    // tell pin change mask to listen to right encoder pins
    PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);

    // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
    PCICR |= (1 << PCIE1) | (1 << PCIE2);

  initMotorController();
  resetPID();

}
            
uint32_t timer;
void loop() {

  gx = accelgyro.getRotationX();
  
  mag.getHeading(&my, &mx, &mz);
  batt_lvl = ( analogRead(A0) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
  
  double dt = (micros() - timer) / 1000000.0;
  timer = micros();

  Serial.println(heading);
  
  float tempY = atan2(-mz, my) * RAD_TO_DEG;

  if ((tempY < -90 && heading > 90) || (tempY > 90 && heading < -90)) {
    filter.setAngle(tempY);
    heading = tempY;
    }

  else

    heading = filter.getAngle(tempY, gx / 131.0, dt);

  //Serial.println(heading);


  while (Serial.available() > 0) {

    // Read the next character
    chr = Serial.read();

    // Terminate a command with a CR
    if (chr == 13) {
      if (arg == 1) argv1[index] = NULL;
      else if (arg == 2) argv2[index] = NULL;
      runCommand();
      resetCommand();
    }
    // Use spaces to delimit parts of the command
    else if (chr == ' ') {
      // Step through the arguments
      if (arg == 0) arg = 1;
      else if (arg == 1)  {
        argv1[index] = NULL;
        arg = 2;
        index = 0;
      }
      continue;
    }
    else {
      if (arg == 0) {
        // The first arg is the single-letter command
        cmd = chr;
      }
      else if (arg == 1) {
        // Subsequent arguments can be more than one character
        argv1[index] = chr;
        index++;
      }
      else if (arg == 2) {
        argv2[index] = chr;
        index++;
      }
    }
  }

  if (millis() > nextPID) {
    updatePID();
    nextPID += PID_INTERVAL;
  }

  // Check to see if we have exceeded the auto-stop interval
  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
    setMotorSpeeds(0, 0);
    moving = 0;
    resetPID();
  }

}