const int CONTROLLER_1_STEP = 38;
const int CONTROLLER_1_DIR = 37;

enum StepperDirection {
  CounterClockWise, ClockWise
};

class StepperController {
  private:
    // Stepper motor operation state
    long cur_steps;
    StepperDirection direction;

    // Stepper motor controller pins
    int step_pin;
    int dir_pin;

    // Stepper mechanical operation settings
    int stepper_mstep_count;


    void triggerLatch() {
      digitalWrite(step_pin, HIGH);
      digitalWrite(step_pin, LOW);
    }

    void changeDirection(StepperDirection direction_p) {
      if (direction == direction_p) return;
      direction = direction_p;
      switch (direction) {
        case StepperDirection::ClockWise:
          digitalWrite(dir_pin, HIGH);
          break;
        case StepperDirection::CounterClockWise:
          digitalWrite(dir_pin, LOW);
          break;
        default:
          digitalWrite(dir_pin, HIGH);
      }
    }

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

  public:
    StepperController(long pin, long step_dir_pin, int mstep_count) {
      cur_steps = 0;
      stepper_mstep_count = mstep_count;
      step_pin = pin;
      dir_pin = step_dir_pin;
      direction = StepperDirection::ClockWise;
    }

    void enable() {
      pinMode(step_pin, OUTPUT);
      pinMode(dir_pin, OUTPUT);

      switch (direction) {
        case StepperDirection::ClockWise:
          digitalWrite(dir_pin, HIGH);
          break;
        case StepperDirection::CounterClockWise:
          digitalWrite(dir_pin, LOW);
          break;
        default:
          digitalWrite(dir_pin, HIGH);
      }
    }

    void stepToN(long steps) {
      if (steps < 0) {
        steps *= -1;
        changeDirection(StepperDirection::CounterClockWise);
      } else {
        changeDirection(StepperDirection::ClockWise);
      }

      int diff = (steps - (cur_steps + 1) + 1);

      if (diff != 0) {
        triggerLatch();
        ++cur_steps;
      }
    }

    void stepToNDegrees(float degrees) {
      stepToN(degreesToSteps(degrees));
    }
};



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

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[16] = {};
    int mask = 0b0000000000000000;

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

    void updateStepperMasks() {
      mask = 0b0000000000000000;
      for (int i = 0; i < 16; i++) {
        if (steppers[i].enabled && steppers[i].target_steps - steppers[i].cur_steps != 0) {
          bitWrite(mask, steppers[i].step_pin, true);
          steppers[i].cur_steps += 1;
        }

        switch (steppers[i].direction) {
          case StepperDirection::CounterClockWise:
            bitWrite(mask, steppers[i].dir_pin, false);
            break;
          case StepperDirection::ClockWise:
            bitWrite(mask, steppers[i].dir_pin, true);
            break;
          default:
            break;
        }
      }
      Serial.println("MASK");
      for (int i = 0; i < 8; i++) {
        Serial.print(bitRead((mask << 8) >> 8, i));
      }
      Serial.println("");
      for (int i = 0; i < 8; i++) {
        Serial.print(bitRead(mask >> 8, i));
      }
      Serial.println("");
    }

    void triggerLatch() {
      for (int i = 0; i < 8; i++) {
        digitalWrite(clockPin, LOW);
        digitalWrite(dataPin, bitRead(mask >> 8, i));
        digitalWrite(clockPin, HIGH);
      }

      for (int i = 0; i < 8; i++) {
        digitalWrite(clockPin, LOW);
        digitalWrite(dataPin, bitRead((mask << 8) >> 8, i));
        digitalWrite(clockPin, HIGH);
      }

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

    void stepSteppers() {
      updateStepperMasks();
      triggerLatch();
    }

  public:
    MultipleSteppersController(bool enabled_steppers_p[16], int spi[3]) {
      for (int i = 0; i < 16; i++) {
        int pin_selct = i * 2;
        if (enabled_steppers_p[i]) {
          Serial.print("Stepper state:");
          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,
          };
        } else {
          steppers[i] = Stepper {
            pin_selct + 1, pin_selct,
            StepperDirection::ClockWise,
            0, 0,
            false,
          };
        }
      }

      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 > 15) 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;

      stepSteppers();
    }

    // void stepToNDegrees(float degrees) {
    //   stepToN(degreesToSteps(degrees));
    // }
};


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


StepperController stepper(CONTROLLER_1_STEP, CONTROLLER_1_DIR, 200);
StepperController stepper2(40, 39, 200);

bool enabled[16] = {
  true, true, true, true, true,
  false, false, false, false,
  false, false, false, false,
  false, false, false
};

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

MultipleSteppersController msc(enabled, spi);

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

  stepper.enable();
  stepper2.enable();

  msc.enable();
}

/*
  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, LSBFIRST, pattern);
  digitalWrite(latchPin, HIGH);
  pattern = ~pattern;
*/

void loop() {
  Serial.println("Hello");

  msc.stepToN(0, 100);
  // msc.stepToN(1, 100);
  // msc.stepToN(2, 150);
  msc.stepToN(4, 200);
  delay(50);
}