#include <Arduino.h>

// ----- Rotary Encoder Pins -----
#define CLK 34
#define DT  35

// ----- Stepper Pins -----
#define DIR 32
#define STEP 33

// ----- Microstep Pins -----
#define MS1 16
#define MS2 17
#define MS3 18

// ----- Steering Config -----
const int PPR = 24;       // Pulses per rotation
const int ROTATIONS = 2;  // 2 full rotations
const float MAX_ENCODER_ANGLE = 720.0;  // ±720° steering wheel
const float MAX_WHEEL_ANGLE   = 45.0;   // ±45° front wheels

int lastCLK;
long encoderTicks = 0;

// Stepper control
long stepperPos = 0;
float STEPPER_STEP_ANGLE = 1.8; // will change dynamically
float stepsPerDegree = 1.0 / STEPPER_STEP_ANGLE; // updated in setup

// ----- Microstep Mode -----
enum MicrostepMode { FULL=1, HALF=2, QUARTER=4, EIGHTH=8 };
MicrostepMode mode = QUARTER; // default mode

// ----- Functions -----
void setMicrostepMode(MicrostepMode m) {
  mode = m;
  switch(m) {
    case FULL:   digitalWrite(MS1, LOW);  digitalWrite(MS2, LOW);  digitalWrite(MS3, LOW);  STEPPER_STEP_ANGLE = 1.8; break;
    case HALF:   digitalWrite(MS1, HIGH); digitalWrite(MS2, LOW);  digitalWrite(MS3, LOW);  STEPPER_STEP_ANGLE = 0.9; break;
    case QUARTER: digitalWrite(MS1, LOW);  digitalWrite(MS2, HIGH); digitalWrite(MS3, LOW);  STEPPER_STEP_ANGLE = 0.45; break;
    case EIGHTH: digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, LOW);  STEPPER_STEP_ANGLE = 0.225; break;
  }
  stepsPerDegree = 1.0 / STEPPER_STEP_ANGLE;
}

void stepMotor(bool dir) {
  digitalWrite(DIR, dir);
  digitalWrite(STEP, HIGH);
  delayMicroseconds(800);
  digitalWrite(STEP, LOW);
  delayMicroseconds(800);
}

void setup() {
  pinMode(CLK, INPUT);
  pinMode(DT, INPUT);
  pinMode(DIR, OUTPUT);
  pinMode(STEP, OUTPUT);
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);

  lastCLK = digitalRead(CLK);
  Serial.begin(115200);

  setMicrostepMode(mode); // initialize microstep pins
}

void loop() {
  int currentCLK = digitalRead(CLK);
  if (currentCLK != lastCLK) {
    if (digitalRead(DT) != currentCLK) encoderTicks++;
    else encoderTicks--;

    long maxTicks = PPR * ROTATIONS;
    if (encoderTicks > maxTicks) encoderTicks = maxTicks;
    if (encoderTicks < -maxTicks) encoderTicks = -maxTicks;

    // --- Stepper Control ---
    float steeringAngle = encoderTicks * (MAX_ENCODER_ANGLE / (PPR * ROTATIONS));
    float frontWheelAngle = steeringAngle * (MAX_WHEEL_ANGLE / MAX_ENCODER_ANGLE);
    long targetSteps = frontWheelAngle * stepsPerDegree;

    if (targetSteps > stepperPos) { stepMotor(true); stepperPos++; }
    else if (targetSteps < stepperPos) { stepMotor(false); stepperPos--; }

    lastCLK = currentCLK;

    // --- Display ---
    Serial.print("Mode: "); Serial.print(mode);
    Serial.print(" | Steering Wheel Angle: "); Serial.print(steeringAngle); Serial.print("° | ");
    Serial.print("Front Wheel Angle: "); Serial.println(frontWheelAngle);
  }
}