#include <Arduino.h>
#include <Wire.h>
#include <smartmotor.h>
#include <SMC_gains.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 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");

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

void loop() {
  float time = (float)millis() / 1000.0;
  switch (robot_state) {
    case FORWARD:
      setpoint = trapezoidal_planner(&forward, time);
      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("");

  // 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;
        break;
      case TURN:
        break;
      case RETURN:
        break;
    }
  }
}