#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 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);
  motor->write_rpm(rpm);
}

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.9, 3.7,0.3,0.0); 
    delay(10);
    //right_motor.set_direction(PIDDirection::DIRECT);
    
    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;
  }


  float feedforward = 
    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
    setpoint.velocity * CMS_RPM * FF_VEL + 
    FF_STAT;

  left_motor.write_rpm_ff(setpoint.velocity * CMS_RPM, feedforward);
  delay(1);
  right_motor.write_rpm_ff((robot_state == TURN ? velocity : -velocity) * CMS_RPM, feedforward);
  delay(19);

  // 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;
    }
  }
}