// === Part 1: Constants, Pins, Shared Variables ===
#include <Arduino.h>
#include <EEPROM.h>

// Pins
#define X_STEP_PIN  17
#define X_DIR_PIN   16
#define Z_STEP_PIN  19
#define Z_DIR_PIN   18
#define C_STEP_PIN  22
#define C_DIR_PIN   21
#define COOLANT_PIN 25
#define SPINDLE_PIN 26

#define RXD2 16 // to review
#define TXD2 17 // to review

// Settings
float stepsPerMM_X = 10.0; //Stepper motor 
float stepsPerMM_Z = 10.0; //Stepper motor 
float stepsPerMM_C = 10.0; //Stepper motor 
float feedrate = 100.0; // mm/min
float jogFeedrate = 50.0; // mm/min

// EEPROM addresses
#define EEPROM_SIZE 12
#define EEPROM_ADDR_X 0
#define EEPROM_ADDR_Z 4
#define EEPROM_ADDR_C 8

// Motion state
volatile float xPos = 0.0;
volatile float zPos = 0.0;
volatile float cPos = 0.0;

float jobRefX = 0.0, jobRefZ = 0.0, jobRefC = 0.0;

bool useJobCoordinates = false;

enum Axis { AXIS_X, AXIS_Z, AXIS_C };
enum MotionMode { MODE_IDLE, MODE_JOG, MODE_MOVE };

//===Motion Command structure ===
struct MotionCommand {
  Axis axis;
  float targetPos;
  float speed;
  MotionMode mode;
};

QueueHandle_t motionQueue;

//=== Declare Functions , LOOP ===
void loopMotionCore(void *pvParameters);
void loopUARTCore(void *pvParameters);

void moveStepper(Axis axis, float distanceMM, float speedMM);
void updatePosition(Axis axis, float delta);

//===Jog Mode Logic ===
volatile bool jogActiveX = false;
volatile bool jogActiveZ = false;
volatile bool jogActiveC = false;

unsigned long lastJogCommandTimeX = 0;
unsigned long lastJogCommandTimeZ = 0;
unsigned long lastJogCommandTimeC = 0;

// === Part 2: Setup ===

void setup() {
  Serial.begin(115200);
  //Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);

  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(Z_STEP_PIN, OUTPUT);
  pinMode(Z_DIR_PIN, OUTPUT);
  pinMode(C_STEP_PIN, OUTPUT);
  pinMode(C_DIR_PIN, OUTPUT);
  pinMode(COOLANT_PIN, OUTPUT);
  pinMode(SPINDLE_PIN, OUTPUT);

  digitalWrite(COOLANT_PIN, LOW);
  digitalWrite(SPINDLE_PIN, LOW);

  EEPROM.begin(EEPROM_SIZE);
  EEPROM.get(EEPROM_ADDR_X, jobRefX);
  EEPROM.get(EEPROM_ADDR_Z, jobRefZ);
  EEPROM.get(EEPROM_ADDR_C, jobRefC);
  EEPROM.end();

  motionQueue = xQueueCreate(10, sizeof(MotionCommand));

  xTaskCreatePinnedToCore(loopMotionCore, "MotionCore", 8192, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(loopUARTCore, "UARTCore", 8192, NULL, 1, NULL, 0);
}

void loop() {
  // Main loop is unused in dual-core setup
}

// === Part 3: Loop 1 ===

void loopMotionCore(void *pvParameters) {
  unsigned long lastUpdateTime = millis();

  while (true) {
    checkJogTimeout();

    if (jogActiveX) moveStepper(AXIS_X, 1.0, jogFeedrate);
    if (jogActiveZ) moveStepper(AXIS_Z, 1.0, jogFeedrate);
    if (jogActiveC) moveStepper(AXIS_C, 1.0, jogFeedrate);

    MotionCommand cmd;
    if (xQueueReceive(motionQueue, &cmd, 0) == pdTRUE) {
      float currentPos = (cmd.axis == AXIS_X) ? xPos : (cmd.axis == AXIS_Z) ? zPos : cPos;
      float delta = cmd.targetPos - currentPos;
      moveStepper(cmd.axis, delta, cmd.speed);
      uint8_t busyMsg[] = {0x5A, 0xA5, 0x03, 0x82, 0x20, 0x20, 0x00};
      Serial.write(busyMsg, sizeof(busyMsg));
      //Serial2.write((uint8_t[]){0x5A, 0xA5, 0x03, 0x82, 0x20, 0x20, 0x00}, 7);
    }

    if (millis() - lastUpdateTime >= 10) {
      lastUpdateTime = millis();
    }
    vTaskDelay(1);
  }
}

// === Part 4: UART Communication Core ===
void loopUARTCore(void *pvParameters) {
  while (true) {
    if (Serial.available() >= 7 && Serial.peek() == 0x5A) {
      Serial.read(); // consume 0x5A
      Serial.println("Received: Msg");
      if (Serial.read() == 0xA5) {
        uint8_t len = Serial.read();
        uint8_t cmd = Serial.read();
        uint8_t vpHi = Serial.read();
        uint8_t vpLo = Serial.read();
        uint16_t vp = (vpHi << 8) | vpLo;

        uint8_t floatBytes[4];
        if (Serial.readBytes(floatBytes, 4) == 4) {
          float val;
          memcpy(&val, floatBytes, sizeof(val));

          Serial.write(cmd);  // Echo for debug

          switch (vp) {
            case 0x2001: addMotionCommand(AXIS_X, val, MODE_MOVE); break;
            case 0x2002: addMotionCommand(AXIS_Z, val, MODE_MOVE); break;
            case 0x2003: addMotionCommand(AXIS_C, val, MODE_MOVE); break;
            case 0x2009: feedrate = val; break;
            case 0x2019: jogFeedrate = val; break;
            case 0x2011: jogActiveX = (val == 1); lastJogCommandTimeX = millis(); break;
            case 0x2012: jogActiveZ = (val == 1); lastJogCommandTimeZ = millis(); break;
            case 0x2013: jogActiveC = (val == 1); lastJogCommandTimeC = millis(); break;
            case 0x2044: useJobCoordinates = (val == 2); break;
            case 0x2041: EEPROM.put(EEPROM_ADDR_X, val); EEPROM.commit(); jobRefX = val; break;
            case 0x2042: EEPROM.put(EEPROM_ADDR_Z, val); EEPROM.commit(); jobRefZ = val; break;
            case 0x2043: EEPROM.put(EEPROM_ADDR_C, val); EEPROM.commit(); jobRefC = val; break;
            default: Serial.println("Unknown VP"); break;
          }
        }
      }
    }
    vTaskDelay(1); // FreeRTOS-friendly delay
  }
}

// Test this with "5A A5 08 82 20 01 00 00 C8 42"

// === Part 5: Functions ==== Motion Part 1 ===

void checkJogTimeout() {
  unsigned long now = millis();
  if (jogActiveX && now - lastJogCommandTimeX > 120) jogActiveX = false;
  if (jogActiveZ && now - lastJogCommandTimeZ > 120) jogActiveZ = false;
  if (jogActiveC && now - lastJogCommandTimeC > 120) jogActiveC = false;
}

void updatePosition(Axis axis, float delta) {
  if (axis == AXIS_X) xPos += delta;
  if (axis == AXIS_Z) zPos += delta;
  if (axis == AXIS_C) cPos += delta;
}

void addMotionCommand(Axis axis, float target, MotionMode mode) {
  MotionCommand cmd = { axis, target, feedrate, mode };
  xQueueSend(motionQueue, &cmd, portMAX_DELAY);
}


// === Stepper Control Functions Oart 2 ===
void moveStepper(Axis axis, float distanceMM, float speedMM) {
  int stepsPerMM = (axis == AXIS_X) ? stepsPerMM_X : (axis == AXIS_Z) ? stepsPerMM_Z : stepsPerMM_C;
  int stepPin = (axis == AXIS_X) ? X_STEP_PIN : (axis == AXIS_Z) ? Z_STEP_PIN : C_STEP_PIN;
  int dirPin = (axis == AXIS_X) ? X_DIR_PIN : (axis == AXIS_Z) ? Z_DIR_PIN : C_DIR_PIN;

  int totalSteps = abs(distanceMM * stepsPerMM);
  digitalWrite(dirPin, (distanceMM > 0) ? HIGH : LOW);

  for (int i = 0; i < totalSteps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(100);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(100);

    float stepDist = (distanceMM > 0) ? (1.0 / stepsPerMM) : -(1.0 / stepsPerMM);
    updatePosition(axis, stepDist);
  }
}