#include <Arduino.h>
#include <Wire.h>
#include <smartmotor.h>
#include <SMC_gains.h>
#include <SMC_pid_directions.h>
#include "trapezoidal.h"
#include "constants.h"

SmartMotor left_motor(0x0A); 
SmartMotor right_motor(0x0B); 

enum ROBOT_STATE {
  FORWARD,
  TURN,
  RETURN,
  COMPLETE,
};

struct Setpoint setpoint; 
// start of current state
float phase_start;
enum ROBOT_STATE robot_state;

struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
struct Trapezoidal turn = {VEL_LIMIT, ACCEL_LIMIT, TURN_DISTANCE};

float left_iaccum = 0.0;
float right_iaccum = 0.0;
float time_p = 0.0;

float left_home = 0.0;
float right_home = 0.0;

void setup() {
    // INIT SERIAL
    Serial.begin(115200);
    while(!Serial);

    Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER

		left_motor.tune_pos_pid(0.4, 0.024, 0.008);
    delay(1);
		right_motor.tune_pos_pid(0.4, 0.024, 0.008);
    delay(1);
    left_motor.tune_vel_pid(Kv, KP,KI,KD);
    delay(1);
    right_motor.tune_vel_pid(Kv, KP,KI,KD);
    delay(1);

    delay(10);
    // runs to mend bad state
    left_motor.home();
    delay(1);
    left_motor.write_angle(0.0);
    delay(1);
    right_motor.home();
    delay(1);
    right_motor.write_angle(0.0);
    delay(3000);
    
    phase_start = (float)millis() / 1000.0;
    robot_state = FORWARD;
    left_motor.home();
    right_motor.home();
}

void loop() {
  float time = (float)millis() / 1000.0 - phase_start;
  float end_time;
  switch (robot_state) {
    case FORWARD:
    case RETURN:
      setpoint = trapezoidal_planner(&forward, time);
      end_time = trapezoidal_time(&forward);
      break;
    case TURN:
      setpoint = trapezoidal_planner(&turn, time);
      end_time = trapezoidal_time(&turn);
      break;
  }

  float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor

  // position PI controller
  float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle();
  float pos_err_left = ((turnsig * setpoint.position / DEG_CM) + left_home) - left_motor.read_angle();

  // total wheel offness in degrees
  float total_pos_error = abs(pos_err_left) + abs(pos_err_right);

  // delta should be ~4ms
  if (time > time_p) {
    float delta = time - time_p;
    left_iaccum += pos_err_left * delta;
    right_iaccum += pos_err_right * delta;
  }
  time_p = time;

  float right_effort = pos_err_right * KPP + right_iaccum * KPI;
  float left_effort = pos_err_left * KPP + left_iaccum * KPI;

  float right_velocity = setpoint.velocity + right_effort;
  float left_velocity = (turnsig * setpoint.velocity) + left_effort;
  
	if (setpoint.complete && robot_state == TURN) {
		right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
		left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
	} else {
    right_motor.write_rpm(right_velocity * CMS_RPM);
    delay(1);
    left_motor.write_rpm(left_velocity * CMS_RPM);
    delay(1);
	}

  // send telemetry
  int32_t rpm = right_motor.read_rpm();
  int32_t pos = right_motor.read_angle();
  int32_t error = setpoint.velocity * CMS_RPM - rpm;
  Serial.print("el:");
  Serial.print(pos_err_left);
  Serial.print(",er:");
  Serial.print(pos_err_right);
  Serial.print(",il:");
  Serial.print(left_iaccum);
  Serial.print(",ir:");
  Serial.print(right_iaccum);
  Serial.print(",effr:");
  Serial.print(right_effort);
  //Serial.print("ff:");
  //Serial.print(left_effort);
  //Serial.print(",time:");
  //Serial.print(time);
  //Serial.print(",distgoal:");
  //Serial.print(setpoint.position);
  //Serial.print(",dist:");
  //Serial.print(pos * DEG_CM);
  Serial.print(",vel:");
  Serial.print(rpm / CMS_RPM);
  Serial.print(",setvel:");
  Serial.print(setpoint.velocity);
  Serial.print(",rvel:");
  Serial.print(right_velocity);
  //Serial.print(",setacc:");
  //Serial.print(setpoint.acceleration);
  //Serial.print(",Err:");
  //Serial.print(error / CMS_RPM);
  Serial.println("");

  // move on if at setpoint or timeout
  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 5.0)) {
    // rehome
    right_home += (setpoint.position / DEG_CM);
    left_home += (turnsig * setpoint.position / DEG_CM);
    // zero accumulators
    right_iaccum = 0.0; left_iaccum = 0.0;

    switch (robot_state) {
      case FORWARD:
        // turn tuning (more aggressive)
        left_motor.tune_vel_pid(TURN_Kv, TURN_KP,TURN_KI,TURN_KD);
        delay(1);
        right_motor.tune_vel_pid(TURN_Kv,TURN_KP,TURN_KI,TURN_KD);

        robot_state = TURN;
				right_iaccum = left_iaccum = 60.0;
        phase_start = (float)millis() / 1000.0;
        break;
      case TURN:
        // straight line tuning (less aggressive)
        left_motor.tune_vel_pid(Kv, KP,KI,KD);
        delay(1);
        right_motor.tune_vel_pid(Kv, KP,KI,KD);

        robot_state = RETURN;
        phase_start = (float)millis() / 1000.0;

        break;
      case RETURN:

        // end
        left_motor.write_rpm(0.0);
        delay(1);
        right_motor.write_rpm(0.0);
        for (;;) {};
        break;
    }
  }
}