#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,
};

float CURRENT_POSITION = 0.0;
float CURRENT_ROTATION = 0.0;
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 write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
  if (rpm == 0) {rpm = 1;};
  float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
  motor->tune_vel_pid(kV, KP,KI,KD);
  delay(1);
  motor->write_rpm(rpm);
  delay(1);
}

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

    Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
    
    left_motor.write_rpm(0.0);
    delay(1);
    right_motor.write_rpm(0.0);
    delay(999);
    
    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;
  switch (robot_state) {
    case FORWARD:
    case RETURN:
      setpoint = trapezoidal_planner(&forward, time);
      break;
    case TURN:
      setpoint = trapezoidal_planner(&forward, time);
      break;
  }


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

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

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

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

  float left_velocity = setpoint.velocity + left_effort;
  float right_velocity = (turnsig * setpoint.velocity) + right_effort;
  
  // calculate feedforward from motion profile and position PI data
  float feedforward_left = 
    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
    left_velocity * CMS_RPM * FF_VEL + 
    ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
  float feedforward_right = 
    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
    right_velocity * CMS_RPM * FF_VEL + 
    ((right_velocity > 0.0) ? FF_STAT : -FF_STAT);

  // arbitrary feedforward with on-board velocity PID
  write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
  write_rpm_ff(&right_motor, right_velocity * CMS_RPM  , feedforward_right);

  // 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("ff:");
  Serial.print(right_effort);
  Serial.print(",time:");
  Serial.print(time);
  Serial.print(",distgoal:");
  Serial.print(setpoint.position);
  Serial.print(",dist:");
  Serial.print(pos * DEG_CM);
  Serial.print(",cms/s:");
  Serial.print(rpm / CMS_RPM);
  Serial.print(",setvel:");
  Serial.print(setpoint.velocity);
  Serial.print(",setacc:");
  Serial.print(setpoint.acceleration);
  Serial.print(",Err:");
  Serial.print(error / CMS_RPM);
  Serial.println("");

  // total wheel offness in degrees
  float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
  // move on if at setpoint TODO: give up after timeout
  if (setpoint.complete && total_pos_error < 10.0) {
    // rehome
    left_home += (setpoint.position / DEG_CM);
    right_home += (turnsig * setpoint.position / DEG_CM);
    // zero accumulators
    right_iaccum = 0.0; left_iaccum = 0.0;

    switch (robot_state) {
      case FORWARD:
        robot_state = TURN;
        phase_start = (float)millis() / 1000.0;
        break;
      case TURN:
        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;
    }
  }
}