// DRIVER : 4 MICROSTEPS FOR BENCH

// include the library code:
#include "rgb_lcd.h"
#include <Adafruit_NeoPixel.h>

// Joystick to motor integer
int SpeedDelay;
int motorHigh = 190;
int motorLow = 900;
int steps = 10;
int farleft = 240;
int centerleft = 485;
int farright = 775;
int centerright = 525;


// --- GLOBAL STRUCTS ---
// Represent a pin and associated state
struct pin_state
{
  int pin;
  int state;
};

// --- COMMANDS and INPUTS ---
// Struct definition
// Struct containing all commands buttons, inputs and states
struct commands
{
  // joystick position, x axis
  pin_state joystick_x;
  // joystick position, y axis
  pin_state joystick_y;
};

// Create command struct
struct commands cmd = {{A1, 0}, {A0, 0}};

// --- MOTORS ---
// Structs definitions
// Struct containing input pullups attached to motor
struct pullups
{
  // home pullup
  pin_state home;
  // end pullup
  pin_state end;
};

// Struct containing all pins and delay associated with a motor
struct motor
{
  // pins
  int dir;
  int step;
  int enable;
  // delay
  int delay;
  // pullups associated
  pullups pups;
  // in position or not (mainly used by burner motor)
  bool in_pos;
};

// Structs creation
// Create struct containing bench motor stuff
struct motor bench_motor = {33, 35, 31, motorHigh, {2, 0, 4, 0}, false};

// Array containing signals instructions
// forward move
int forward[4] = {HIGH, HIGH, HIGH, LOW};
// backward move
int backward[4] = {LOW, HIGH, HIGH, LOW};
// freeMotor
int freeMotor[4] = {LOW, LOW, LOW, LOW};

// move_motor is used to move motor forward or backward
// arg motor: motor to activate is passed a parameter
// arg signals: array of int used to specify backward or forward mode
void move_motor(motor motor, int *signals)
{
  digitalWrite(motor.dir, signals[0]);
  digitalWrite(motor.enable, signals[1]);
  digitalWrite(motor.step, signals[2]);
  delayMicroseconds(motor.delay);
  digitalWrite(motor.step, signals[3]);
  delayMicroseconds(motor.delay);
}

// home_motors is used to get all motors to home position
// This function is disabled since no more end course

void home_motors()
{
//   bench_motor.delay = motorHigh+30;
//   Remove End course
//   while (bench_motor.pups.home.state == LOW)
//   {
//     move_motor(bench_motor, forward);
//     bench_motor.pups.home.state = digitalRead(bench_motor.pups.home.pin);
//   }
  refresh_motors_states();
}

// refresh all motors pullups states
// This function is disabled since no more end course
void refresh_motors_states()
{
//   bench_motor.pups.home.state = digitalRead(bench_motor.pups.home.pin);
//   bench_motor.pups.end.state = digitalRead(bench_motor.pups.end.pin);
}

// move_using_joystick is used to move motors depending on joystick position
void move_using_joystick()
{
  // fetch joystick position
  cmd.joystick_x.state = analogRead(cmd.joystick_x.pin);
  cmd.joystick_y.state = analogRead(cmd.joystick_y.pin);
  // Check if Joystick button is pressed - Remove end course
//   if (cmd.joystick_y.state > 1000 && bench_motor.pups.end.state == LOW)
  if (cmd.joystick_y.state > 1000)
  {
    home_motors();
  }
  // if forward, go forward  - Remove end course
//   while (cmd.joystick_x.state > centerright && bench_motor.pups.end.state == LOW)
  while (cmd.joystick_x.state > centerright)
  {
    SpeedDelay = map(cmd.joystick_x.state , centerright, farright, motorLow, motorHigh);
    bench_motor.delay = SpeedDelay;
    for (int i = 0; i < steps; i++)
      move_motor(bench_motor, forward);
//       Remove End course
//     bench_motor.pups.end.state = digitalRead(bench_motor.pups.end.pin);
    cmd.joystick_x.state = analogRead(cmd.joystick_x.pin);
  }

  // if backward, go backward - Remove end course
//   while (cmd.joystick_x.state < centerleft && bench_motor.pups.home.state == LOW)
  while (cmd.joystick_x.state < centerleft)
  {
    SpeedDelay = map(cmd.joystick_x.state , centerleft, farleft, motorLow, motorHigh);
    bench_motor.delay = SpeedDelay;
    for (int i = 0; i < steps; i++)
      move_motor(bench_motor, backward);
//       Remove end course
//     bench_motor.pups.home.state = digitalRead(bench_motor.pups.home.pin);
    cmd.joystick_x.state = analogRead(cmd.joystick_x.pin);
  }
  move_motor(bench_motor, freeMotor);
}
// setup_motors assign all pin to output and input for motors
void setup_motors()
{
  // bench motor setup
  pinMode(bench_motor.step, OUTPUT);
  pinMode(bench_motor.dir, OUTPUT);
  pinMode(bench_motor.enable, OUTPUT);
//   pinMode(bench_motor.pups.home.pin, INPUT_PULLUP);
//   pinMode(bench_motor.pups.end.pin, INPUT_PULLUP);
}

// -- Setup function, needed by Arduino
void setup()
{
  setup_motors();
}

// --- MAIN ---

// refresh_state is a wrapup off all refresh function
void refresh_state()
{
  // motors states
  refresh_motors_states();

}

// --- MAIN LOOP ---
void loop()
{

  // refresh states from all inputs
// This function is disabled since no more end course
//   refresh_state();

  // move joystick if needed
  move_using_joystick();

}