#include <ESP32Servo.h>
#include <elapsedMillis.h>

// reload servo pin assignment
const int reloadServoPinM1 = 5;
const int reloadServoPinM2 = 7;
const int reloadServoPinM3 = 16;
const int reloadServoPinM4 = 18;

// dispense servo pin assignment
const int dispenseServoPinM1 = 14;
const int dispenseServoPinM2 = 12;
const int dispenseServoPinM3 = 10;
const int dispenseServoPinM4 = 46;

// reload sensor pin assingment
const int reloadSensorPinM1 = 4;
const int reloadSensorPinM2 = 6;
const int reloadSensorPinM3 = 15;
const int reloadSensorPinM4 = 17;

// dispense sensor pin assingment
const int dispenseSensorPinM1 = 13;
const int dispenseSensorPinM2 = 11;
const int dispenseSensorPinM3 = 9;
const int dispenseSensorPinM4 = 3;


const int posServoOpened = 180;
const int posServoClosed = 90;

Servo reloadServo1;
Servo reloadServo2;
Servo reloadServo3;
Servo reloadServo4;

Servo dispenseServo1;
Servo dispenseServo2;
Servo dispenseServo3;
Servo dispenseServo4;


const int clockPin = 21;  // SHCP
const int latchPin = 20;  // STCP
const int dataPin = 19;   // DSs

//function to open reload hatch



enum StepperDirection {
  CounterClockWise, ClockWise
};


struct Stepper
{
  uint8_t step_pin;
  uint8_t dir_pin;
  StepperDirection direction;
  long target_steps;
  long cur_steps;
  bool enabled;
  elapsedMillis last_step_t;
};


class MultipleSteppersController {
    // Uses a 74HC595 IC to control a maximum of 4 steppers.
    // Each stepper uses 2 wires each.

  private:
    // IC mask is 0b00_00_00_00 , lsb first
    // 4 Steppers total, each having 2 pins
    Stepper steppers[4] = {};
    uint8_t mask = 0b00000000;

    // SPI pins
    int clockPin;
    int dataPin;
    int latchPin;

    void updateStepperMasks() {
      this->mask = 0b00000000;
      for (int i = 0; i < 4; i++) {
        if (
          !steppers[i].enabled ||
          steppers[i].target_steps - steppers[i].cur_steps == 0
        ) continue;

        bitWrite(mask, steppers[i].step_pin, true);
        steppers[i].cur_steps += 1;

        switch (steppers[i].direction) {
          case StepperDirection::CounterClockWise:
            bitWrite(this->mask, steppers[i].dir_pin, false);
            break;
          case StepperDirection::ClockWise:
            bitWrite(this->mask, steppers[i].dir_pin, true);
            break;
          default:
            break;
        }
      }
    }

    void triggerLatch() {
      digitalWrite(latchPin, HIGH);
      digitalWrite(latchPin, LOW);
      shiftOut(dataPin, clockPin, MSBFIRST, 0);
      digitalWrite(latchPin, HIGH);
      digitalWrite(latchPin, LOW);
    }

    void stepSteppers() {
      updateStepperMasks();
      shiftOut(dataPin, clockPin, MSBFIRST, mask);
      triggerLatch();
    }

  public:
    MultipleSteppersController(bool enabled_steppers_p[4], int spi[3]) {
      for (int i = 0; i < 4; i++) {
        int pin_selct = i * 2;
        if (enabled_steppers_p[i]) {
          steppers[i] = Stepper {
            // IC_MASK ^= (1 << stepper.step_pin); -> to step the mottor
            // IC_MASK ^= (1 << stepper.dir_pin); -> to direct the mottor;
            // all of this only if the stepper is enabled
            pin_selct + 1, pin_selct,
            StepperDirection::ClockWise,
            0, 0,
            true,
            0,
          };
        } else {
          steppers[i] = Stepper {
            pin_selct + 1, pin_selct,
            StepperDirection::ClockWise,
            0, 0,
            false,
            0
          };
        }
      }

      dataPin  = spi[0];
      clockPin = spi[1];
      latchPin = spi[2];
    }

    void enable() {
      pinMode(dataPin, OUTPUT);
      pinMode(clockPin, OUTPUT);
      pinMode(latchPin, OUTPUT);
    }

    void stepToN(uint8_t stepper, long steps) {
      // Invalid stepper indicies
      if (stepper > 3) return;
      // Check if stepper is enabled
      if (!steppers[stepper].enabled) return;

      if (steps < 0) {
        steps *= -1;
        steppers[stepper].direction = StepperDirection::CounterClockWise;
      } else {
        steppers[stepper].direction = StepperDirection::ClockWise;
      }

      steppers[stepper].target_steps = steps;

      if (steppers[stepper].last_step_t > 35) {
        steppers[stepper].last_step_t = 0;
        stepSteppers();
      }
    }

    void stepToNDegrees(uint8_t stepper, int degrees) {
      stepToN(stepper, degreesToSteps(degrees));
    }

    int degreesToSteps(float degrees_p) {
      float degs = 360.0 / 200;
      return degrees_p / degs;
    }
};

bool enabled[4] = {true, true, true, true};
int spi[4] = {dataPin, clockPin, latchPin};

int reloadAngles[9] = {0, 40, 80, 120, 160, 200, 240, 280, 320};
int dispenseAngles[9] = {80, 220, 260, 320, 340, 20, 60, 100, 140};

MultipleSteppersController msc(enabled, spi);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S3!");
  msc.enable();
  reloadServo1.attach(reloadServoPinM1);

  reloadServo1.write(0);
  /*
    reloadServo2.attach(reloadServoPinM2, 500, 2400);
    reloadServo3.attach(reloadServoPinM3, 500, 2400);
    reloadServo4.attach(reloadServoPinM4, 500, 2400);

    // Configurar as posições iniciais dos servos
    reloadServo2.write(0);
    reloadServo3.write(0);
    reloadServo4.write(0);
  */
}
int i = 0;
void loop() {

  // Usar stepToNDegrees - para poder usar angulos expressos em nºs inteiros
  msc.stepToNDegrees(0, reloadAngles[3]);
  msc.stepToN(1, 100);
  msc.stepToN(2, 150);
  msc.stepToN(3, 200);

  // reloadServo1.write(++i);
  // Serial.println(i);
  /*
    reloadServo1.write(10);
    delay(1000);
    reloadServo1.write(20);
    delay(1000);
    reloadServo1.write(30);
    delay(1000);*/

}