#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; 
float velocity = 40.0;
// start of current state
float phase_start;
enum ROBOT_STATE robot_state;

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

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

    Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
    
    // TUNE VELOCITY PID
    left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
    delay(10);
    right_motor.tune_vel_pid(0.0, 3.7,0.3,0.0); 
    delay(10);
    right_motor.set_direction(PIDDirection::DIRECT);
    
    uint16_t ratio = right_motor.get_gear_ratio();
    Serial.print("ratio: ");
    Serial.println(ratio);
    ratio = left_motor.get_gear_ratio();
    Serial.print("ratio: ");
    Serial.println(ratio);


    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");

    phase_start = (float)millis() / 1000.0;
    robot_state = FORWARD;
}

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



  left_motor.write_rpm(setpoint.velocity * CMS_RPM);
  delay(10);
  right_motor.write_rpm(robot_state == TURN ? -velocity : velocity);
  delay(10);

  // READ MOTOR POSITION
  int32_t rpm = right_motor.read_rpm();
  int32_t pos = right_motor.read_angle();
  int32_t error = setpoint.velocity * CMS_RPM - rpm;
  Serial.print("time:");
  Serial.print(time);
  Serial.print(",dist goal:");
  Serial.print(setpoint.position);
  Serial.print(",dist:");
  Serial.print(pos * DEG_CM);
  Serial.print(",cms/s:");
  Serial.print(rpm / CMS_RPM);
  Serial.print(",Setpoint:");
  Serial.print(setpoint.velocity);
  Serial.print(",Err:");
  Serial.print(error / CMS_RPM);
  Serial.println("");

  // move on if at setpoint TODO: check that the error is low as well
  if (setpoint.complete) {
    switch (robot_state) {
      case FORWARD:
        robot_state = TURN;
        phase_start = (float)millis() / 1000.0;

        // end (temp)
        left_motor.write_rpm(0.0);
        delay(10);
        right_motor.write_rpm(0.0);
        for (;;) {};
        break;
      case TURN:
        break;
      case RETURN:
        break;
    }
  }
}