//{"motor_position_degrees": 180, "led_color": "#FF0000"}


#include "A4988_driver.h"

#include <ArduinoJson.h>

// state machine codes
enum StateCode {
  S_IDLE,
  S_MOTOR_MOVE_POS,
  S_MOTOR_MOVE_NEG,
};
StateCode curr_state = S_IDLE;

// pin definitions
#define BTN_PIN 3

// constants
#define DEBOUNCE_DELAY_MS 10

// State machine function definitions
void Idle_Entry(void);
void Idle_Update(void);
void MotorMovePos_Entry(void);
void MotorMovePos_Update(void);
void MotorMoveNeg_Entry(void);
void MotorMoveNeg_Update(void);

// helper functions
bool shortest_direction(void);
void check_serial(void);

// button debouncing variables
volatile unsigned long last_debounce = 0;
volatile bool btn_state = 0;

// global variables
uint16_t desired_position = 0;
float actual_position     = 0;
bool direction;

// ArduinoJson variables
String json;
JsonDocument packet;

// driver objects
A4988_Stepper stepper(2, 5, 8, 7, 4);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(BTN_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BTN_PIN), ISR_Btn_Press, RISING);
  stepper.init();

  TIMSK0 |= _BV(OCIE0A);  //Enable output compare interrupt on STEP pin (5)
  Idle_Entry();
}

void loop() {
  // put your main code here, to run repeatedly:
  check_serial();
  switch (curr_state)
  {
    case S_IDLE:
      Idle_Update();
      break;
    case S_MOTOR_MOVE_POS:
      MotorMovePos_Update();
      break;
    case S_MOTOR_MOVE_NEG:
      MotorMoveNeg_Update();
      break;
    default:
      curr_state = S_IDLE;
      break;
  }
}


/*
  State machine functions
*/
void Idle_Entry(void)
{
  curr_state = S_IDLE;
}
void Idle_Update(void)
{
  actual_position = stepper.get_position();
  if(abs(desired_position - actual_position) >= STEP_SIZE)
  {
    if (shortest_direction() == CW_DIR)
    {
      // Faster to move CW
      MotorMovePos_Entry();
      return;
    }
    else
    {
      // Faster to move CCW
      MotorMoveNeg_Entry();
      return;
    }
  }
}
void MotorMovePos_Entry(void)
{
  curr_state = S_MOTOR_MOVE_POS;
  direction = CW_DIR;
  stepper.set_direction(direction);
  stepper.start();
}
void MotorMovePos_Update(void)
{
  actual_position = stepper.get_position();
  if(!stepper.is_moving())
  {
    Idle_Entry();
    return;
  }
  else if (shortest_direction() == CCW_DIR)
  {
    // Faster to move CCW now
    stepper.stop();
    MotorMoveNeg_Entry();
    return;
  }
}
void MotorMoveNeg_Entry(void)
{
  curr_state = S_MOTOR_MOVE_POS;
  direction = CCW_DIR;
  stepper.set_direction(direction);
  stepper.start();
}
void MotorMoveNeg_Update(void)
{
  actual_position = stepper.get_position();
  if(!stepper.is_moving())
  {
    Idle_Entry();
    return;
  }
  else if (shortest_direction() == CW_DIR)
  {
    // Faster to move CW now
    stepper.stop();
    MotorMovePos_Entry();
    return;
  }
}

/*
  ISRs
*/
void ISR_Btn_Press(void)
{
  
  if (millis() - last_debounce > DEBOUNCE_DELAY_MS)
  {
    last_debounce = millis();
    if(!btn_state)
    {
      // rising edge
      btn_state = 1;
      desired_position = (desired_position + 10) % 360;
    }
    else
    {
      // falling edge
      btn_state = 0;
    }
  }
}

ISR(TIMER0_COMPA_vect)
{
  // output compare ISR for STEP pin (5)
  stepper.step_callback();
  if(abs(desired_position - stepper.get_position()) < STEP_SIZE)
  {
    stepper.stop();
  }
}

/*
  Helper functions
*/
bool shortest_direction(void)
{
  float delta = abs(desired_position - actual_position);
  if(desired_position > actual_position)
  {
    if(delta > 180)
    {
      return CCW_DIR;
    }
    else
    {
      return CW_DIR;
    }
  }
  else
  {
    if(delta > 180)
    {
      return CW_DIR;
    }
    else
    {
      return CCW_DIR;
    }    
  }
}

void check_serial(void)
{
  if(Serial.available())
  {
    json = Serial.readString();
    deserializeJson(packet, json);
    desired_position = packet["motor_position_degrees"];
  }
}