#include <Arduino.h>
#include <Wire.h>
#include <smartmotor.h>
#include <SMC_gains.h>

const float ACCEL_LIMIT = 4.0; // cm/s/s
const float VEL_LIMIT = 16.0; // cm/s
const float BACKLASH_RIGHT = 0.0; // degrees
const float BACKLASH_LEFT = 0.0; // degrees
const float WHEEL_DIAMETER = 6.37; // cm
const float WHEEL_TO_WHEEL = 0.0; // cm

const float FORWARD_DISTANCE = 100.0; // cm
const float TURN_AMOUNT = 180.0; // degrees
const float RETURN_DISTANCE = 100.0; // cm

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

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

    Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
    
    // TUNE POSITION PID
    left_motor.tune_vel_pid(9.0, 0.0,0.0,0.0); 
    right_motor.tune_vel_pid(1.0, 0.65,0.060,0.065); 
    delay(1000);

    // SET MOTOR POSITION
    //int32_t angle=360;
    //uint8_t status= left_motor.write_angle(angle);
    //delay(1000);

    // READ MOTOR POSITION
    int32_t pos = left_motor.read_angle();
    Serial.print("Pos: ");
    Serial.print(pos);
    Serial.println(" deg");
}

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

enum ROBOT_STATE robot_state = FORWARD;

float CURRENT_POSITION = 0.0;
float CURRENT_ROTATION = 0.0;
float velocity = 40.0;

struct Setpoint {
  float velocity; // unitless
  float position; // unitless
  bool complete; // the setpoint will no longer change
};

// returns the position and velocity at the given time on a trapezoidal motion plan
// this could be baked if too computationally expensive
// not fully fuzzed
struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, float time) {
  struct Setpoint setpoint = {0.0, 0.0};

  float time_accelerating = max_acc / max_vel;
  float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;

  if (2.0 * distance_while_accelerating > dist) {
    // triangular
    float peak_velocity_time = sqrt(distance/max_acc);
    float peak_velocity = max_acc * peak_velocity_time;

    if (time < peak_velocity_time) {
      // accelerating
      setpoint.velocity = max_acc * time;
      setpoint.position = 0.5 * max_acc * time * time;
    } else if (time < peak_velocity_time * 2.0) {
      // slowing down
      float time_decay = time - peak_velocity_time;
      setpoint.velocity = peak_velocity - (time_decay * max_acc);
      setpoint.position = (0.5 * A * peak_velocity_time * peak_velocity_time) // acceleration phase
        + peak_velocity * time_decay 
        - 0.5 * max_acc * time_decay * time_decay;
    } else {
      setpoint.velocity = 0;
      setpoint.position = dist;
      setpoint.complete = true;
    }
  } else {
    // trapezoidal
    float cruise_distance = distance - 2.0 * distance_while_accelerating;
    float cruise_time = cruise_distance / max_vel;
    float total_time = 2 * time_accelerating + cruise_time;
    if (time < time_accelerating) {
      // still accelerating
      setpoint.velocity = time * max_acc;
      setpoint.position = 0.5 * max_acc * time * time;
    } else if (time < time_accelerating + cruise_time) {
      // cruising
      setpoint.velocity = max_vel;
      setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
    } else if (time < time_total) {
      // slowing down
      float time_decay = time - (time_accelerating + cruise_time);
      setpoint.velocity = max_vel - time_decay * max_acc;
      setpoint.position = distance_while_accelerating + cruise_distance
        + (max_vel * time_decay)
        - 0.5 * max_acc * time_decay * time_decay;
    } else {
      //done
      setpoint.velocity = 0.0;
      setpoint.position = dest;
      setpoint.complete = true;
    }
  }

  return setpoint;
}

void loop() {
  switch (robot_state) {
    case FORWARD:
      break;
    case TURN:
      break;
    case RETURN:
      break;
  }



  left_motor.write_rpm(velocity);
  //right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
  delay(20);

  // READ MOTOR POSITION
  int32_t rpm = left_motor.read_rpm();
  int32_t error = velocity - rpm;
  Serial.print("RPM: ");
  Serial.print(rpm);
  Serial.print(" Setpoint: ");
  Serial.print(velocity);
  Serial.print(" Err: ");
  Serial.print(error);
  Serial.println("");
}