// === Pin Assignments ===
#define X_DIR_PIN 16
#define X_STEP_PIN 17
#define Z_DIR_PIN 18
#define Z_STEP_PIN 19
#define C_DIR_PIN 25
#define C_STEP_PIN 26

// === Start STOP===
#define START 32
#define STOP 33

// === UART Settings DWIN ===
#define UART_RX 4 // to review 4 as alternate
#define UART_TX 15 // to reivew 15 as alternate
#define BAUD_RATE 115200

// === I2C Settings LCD===
//Wire.begin(21, 22); // SDA, SCL in setup

// === SPINDLE & COOLANT ===
#define SPINDLE_S 27
#define COOLANT_S 14

// === HOME POSITION SENSORS ===
#define X_Home 34
#define Z_Home 35
#define C_Home 36

// === For LCD Display ===
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4); //LCD Address

// === Feed Rates ==
struct FeedrateSet { // Structure array of feedarate
  float X[5]; // JogFeed rate 0,1,3, Feed G01 and G00 mode
  float Z[5];
  float C[5];
};
FeedrateSet feedrates;

// === Library to Save and load in NVS for editiing
#include <Preferences.h> 
Preferences prefsF;        // Handle for NVS storage

// -- Function to load default feedrate --
// To be used for testing
void initDefaultFeedrates() {
  feedrates.X[0] =  20.0;  feedrates.X[1] = 50.0;  
  feedrates.X[2] = 100.0;  feedrates.X[3] = 30.0; feedrates.X[4] = 70.0; 
  feedrates.Z[0] =  20.0;  feedrates.Z[1] = 50.0;  
  feedrates.Z[2] = 100.0;  feedrates.Z[3] = 30.0; feedrates.Z[4] = 70.0; 
  feedrates.C[0] =  20.0;  feedrates.C[1] = 50.0; 
  feedrates.C[2] = 150.0;  feedrates.C[3] = 30.0; feedrates.C[4] = 70.0; 
}
// ---- Funsction Save to NVS ----
void saveFeedrates() {
  prefsF.begin("feedstore", false); // RW mode in feedstore folder
  prefsF.putBytes("feedrate", &feedrates, sizeof(feedrates));
  prefsF.end();
}
// ---- Function Load from NVS ----
void loadFeedrates() {
  prefsF.begin("feedstore", true); // folder to be read-only
  size_t size = prefsF.getBytes("feedrate", &feedrates, sizeof(feedrates));
  prefsF.end(); //

  if (size == 0) {
    Serial.println("⚠️ No feedrates found in NVS, using defaults.");
    initDefaultFeedrates();
    saveFeedrates();
  }
}
// ---- Function Print current values ----
void listFeedrates() {
  Serial.println("\n--- Feedrates ---");
  for (int i = 0; i < 5; i++) {
    Serial.printf("Index %d: X=%.3f  Z=%.3f  C=%.3f\n",
                  i, feedrates.X[i], feedrates.Z[i], feedrates.C[i]);
  }
}

// === for Spline Programs ==
struct SplineProgram {
  uint8_t   srNo;              // Program number 1–16
  char      name[11];          // Program name, up to 10 chars + null terminator
  float     length;            // Job length (mm, 3 decimals)
  float     depth;             // Job depth (mm, 3 decimals)
  uint8_t   numSplines;        // Number of splines (0–18)
};

SplineProgram splinestore[16];   // 16 programs in memory
Preferences prefsS;        // Handle for NVS storage

// -- Function to load default Spline Programs --
// To be used for testing
void initDefaultSplines() {
  for (int i = 0; i < 16; i++) {
    splinestore[i].srNo = i + 1;  // program numbers 1–16

    // Give each program a default name "Prog01", "Prog02", etc.
    snprintf(splinestore[i].name, sizeof(splinestore[i].name), "Prog%02d", i + 1);

    // Example defaults (you can change as needed)
    splinestore[i].length     = 100.0 + i;   // just for testing
    splinestore[i].depth      = 10.0 + i;    // just for testing
    splinestore[i].numSplines = i % 18;      // max 18
  }
}

// --- Function Save all 16 programs to NVS ---
void saveSplines() {
  prefsS.begin("splinestore", false);  // namespace "splinestore"
  prefsS.putBytes("splineprogram", &splinestore, sizeof(splinestore));
  prefsS.end();
}
// ---- Function Load from NVS ----
void loadSplines() {
  prefsS.begin("splinestore", true); // read-only
  size_t size = prefsS.getBytes("splineprogram", &splinestore, sizeof(splinestore));
  prefsS.end();

  if (size == 0) {
    Serial.println("⚠️ No Program found in NVS, using defaults.");
    initDefaultSplines();
    saveSplines();
  }
}

// ---- Function Print current values Spline programs ----
void listSplines() {
  for (int i = 0; i < 16; i++) {
    Serial.printf("Program %d | Name: %s | Length: %.3f | Depth: %.3f | Splines: %d\n",
                  splinestore[i].srNo,
                  splinestore[i].name,
                  splinestore[i].length,
                  splinestore[i].depth,
                  splinestore[i].numSplines);
  }
}

// === Dual Core Libraries ==
#include <Arduino.h>
//#include <FreeRTOS.h>

// === For Pause button control ===
volatile bool moveActive = false;   // true while executing a block
volatile bool movePaused = false;   // true if pause button pressed

// === Motion control part ISR ===
volatile long masterStepsRemaining = 0;  // Master steps of program
volatile long errX = 0, errZ = 0, errC = 0 ; // for step calulation in ISR
volatile long stepsX_forBlock = 0; // steps in master step block
volatile long stepsZ_forBlock = 0; // steps in master step block
volatile long stepsC_forBlock = 0; // steps in master step block

// === Timer Function for Steps ===

// --- Forward declaration of ISR ---
void IRAM_ATTR onStepTimer();

// Variable for timer
hw_timer_t *stepTimer = NULL; // Time for ISR
volatile uint32_t stepInterval_us = 1000;  // default step interval (1ms = 1 kHz)

// --- Timer Function --
void setupTimers(uint32_t stepInterval_us) {
    // Master step timer
    //uint32_t freq = 1000000;  // Hz from µs interval
    //stepTimer = timerBegin(freq);  // time tick
    //timerAttachInterrupt(stepTimer, &onStepTimer);
    timerStart(stepTimer); //enables when function is called
    timerAlarm(stepTimer, stepInterval_us, false, 0); // deploy timer

    Serial.println("Timer - Triggered");
}

volatile bool X_STEP = false; // for pulse high low
volatile bool Z_STEP = false; // for pulse high low
volatile bool C_STEP = false; // for pulse high low
volatile unsigned long X_STEPS = 0; // for count of steps
volatile unsigned long Z_STEPS = 0; // for count of steps
volatile unsigned long C_STEPS = 0; // for count of steps

// constants for Motion profile (to be linked)
const float accel_mm_s2 = 1.0f;   // acceleration (mm/s^2)
volatile float vmax_mm_s_X   = 8.0f;    // target velocity (mm/s)
volatile float vmax_mm_s_Z   = 2.0f;    // target velocity (mm/s)
volatile float vmax_deg_s_C   = 2.0f;    // target velocity (deg/s)
const float stepsPerMM_X  = 80.0f;    // steps per mm for X
const float stepsPerMM_Z  = 80.0f;    // steps per mm for Z
const float stepsPerDeg_C  = 80.0f;    // steps per Degree for C
volatile uint32_t accelRemaining = 0; //for steps in block to accel
volatile uint32_t cruiseRemaining = 0; // for steps to run at  const. speed
volatile uint32_t decelRemaining = 0; // for steps in blok to decel
volatile float v2_mm_s_X = 0.0f; // velocity [mm/s]
volatile float v2_mm_s_X_sq = 0.0f; // velocity squared [mm^2/s^2]
float accel_mm_s2_forBlock = 1.0f;  // mm/s², tune this
// step length in mm (1 / stepsPerMM)
float deltaS_mm_per_masterStep = 1.0f;
float v_mm_s ; // velocity
float steps_per_sec ;

// ---Motion block structure ---
struct MotionBlock {
  // geometric (mm)
  float deltaX = 0, deltaZ = 0, deltaC = 0;
  float length_mm = 0;       // Euclidean length
  // motion parameters
  float feed_mm_min = 0;     // requested feed (mm/min)
  float accel_mm_s2 = 0;     // accel (mm/s^2)
  // lookahead results (in mm/s)
  float cruise_mm_s = 0;
  float entry_mm_s  = 0;
  float exit_mm_s   = 0;
  // step counts
  long stepsX = 0, stepsZ = 0, stepsC = 0;
  long masterSteps = 0;      // highest of stepsX/stepsZ/stepsC
  // phases (in master-steps)
  long accelSteps = 0, cruiseSteps = 0, decelSteps = 0;
  // directions
  int dirX = 1, dirZ = 1, dirC = 1;
};

volatile MotionBlock currentBlock; // to be used in program

// === Setup ===
void setup() {
  // === Massage on Serial ===
  Serial.begin(115200);
  Serial.println("Hello, Welcome to CNC Controller");
  Serial2.begin(115200, SERIAL_8N1, UART_RX, UART_TX);

  // === LCD WELCOME NOTE ===
  Wire.begin(21, 22); // SDA, SCL
  lcd.begin(20,4);
  lcd.backlight();
  lcd.print("HELLO!");

  // === Load Feed Rate ===
  Serial.println("\n=== Feedrate Test ===");
  loadFeedrates();   // Load saved or defaults
  listFeedrates();

  // === Load Spline Program ===
  Serial.println("\n=== Spline Program Review ===");
  loadSplines();   // Load saved or defaults
  listSplines();

  // == Define Tasks ==
  /*xTaskCreatePinnedToCore(
    TaskFunction_t pvTaskCode,   // Function name
    const char * const pcName,   // Task name (string)
    const uint32_t usStackDepth, // Stack size in words (NOT bytes)
    void * const pvParameters,   // Task parameters
    UBaseType_t uxPriority,      // Priority (higher = more important)
    TaskHandle_t * const pxCreatedTask, // Task handle (or NULL if not needed)
    const BaseType_t xCoreID     // Core to pin: 0 or 1
    );*/
xTaskCreatePinnedToCore (Task_M,"Task_M",2000,NULL,3,NULL,0);
xTaskCreatePinnedToCore (Task_P,"Task_P",2000,NULL,2,NULL,0);
xTaskCreatePinnedToCore (Task_LCD,"Task_LCD",2000,NULL,1,NULL,1);
xTaskCreatePinnedToCore (Task_DWIN,"Task_DWIN",4000,NULL,2,NULL,1);
xTaskCreatePinnedToCore (Task_HTML,"Task_HTML",4000,NULL,1,NULL,1);

}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}

// === Task for LCD Display ===
void Task_LCD (void *pvParameters) {
  while (1) {
    //lcd.setCursor(0, 1); // LCD off set to Row 2
    //lcd.print(String("Steps X: ")+ X_STEPS);
    vTaskDelay(200 / portTICK_PERIOD_MS);  
  }
}

// === Task M ===
void Task_M(void *pvParameters) {
  pinMode(SPINDLE_S, OUTPUT);
  while (1) {
    digitalWrite(SPINDLE_S, HIGH);
    //Serial.println("LED ON");
    vTaskDelay(1000 / portTICK_PERIOD_MS);  
      //Serial.print(getCpuFrequencyMhz()); // to check CPU frequency
      //Serial.println(" MHz");
    digitalWrite(SPINDLE_S, LOW);
    //Serial.println("LED OFF");
    vTaskDelay(600 / portTICK_PERIOD_MS);  
  }
}

// === Tark to execute program ===
void Task_P (void* pvParameters) {
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(Z_STEP_PIN, OUTPUT);
  pinMode(C_STEP_PIN, OUTPUT);
  
    // --- Timer Function --
  // Master step timer
    stepTimer = timerBegin(1000000);  // use tiemr 0 and set prescale to 80 so 1 tick is 1 uSec
    timerAttachInterrupt(stepTimer, &onStepTimer); // point to the ISR
    timerStop(stepTimer); // stop as to start in program

  // Give system time to boot
  vTaskDelay(2000 / portTICK_PERIOD_MS);
  startTestMove();  // begin test
 
  while (1) {
    if (!moveActive) {
      Serial.println("✅ Move finished");
      vTaskDelay(2000 / portTICK_PERIOD_MS);
      startTestMove();  // start again
      }
    vTaskDelay(100 / portTICK_PERIOD_MS);
    //Serial.println(String("Steps Remaining ") + masterStepsRemaining);
    Serial.println(String("VTimer ") + stepInterval_us);
    Serial.println(String("Velocity ") + (v2_mm_s_X_sq)*10);
  }
}

// === Function for testing current block with fix values
void startTestMove() {
  currentBlock.stepsX = 10800;
  currentBlock.stepsZ = 1500;
  currentBlock.stepsC = 300;
  currentBlock.masterSteps = currentBlock.stepsX;

  masterStepsRemaining = currentBlock.masterSteps;
  errX = errZ = errC = 0;

  stepsX_forBlock = 10800;  // important!
  stepsZ_forBlock = 1500;
  stepsC_forBlock = 300;

  moveActive = true;
  movePaused = false;
  //Serial.println(String("Current block steps ") + currentBlock.masterSteps);
  //Serial.println(X_STEPS);

  // --- Motion profile calculation ---
  float totalSteps = currentBlock.masterSteps;
  float totalMM = totalSteps / stepsPerMM_X;

  // distance needed to accel to vmax
  float accelDist_mm = (vmax_mm_s_X * vmax_mm_s_X) / (2.0f * accel_mm_s2);
  float accelSteps = accelDist_mm * stepsPerMM_X;

  // symmetric accel/decel
  float decelSteps = accelSteps;

  if ((accelSteps + decelSteps) > totalSteps) {
    // triangle profile (can’t reach vmax)
    accelSteps = totalSteps / 2;
    decelSteps = totalSteps - accelSteps;
    cruiseRemaining = 0;
  } else {
    cruiseRemaining = totalSteps - accelSteps - decelSteps;
  }

  accelRemaining = (uint32_t)accelSteps;
  decelRemaining = (uint32_t)decelSteps;

  // initial velocity² (start from rest)
  v2_mm_s_X_sq = 0.0f;
  deltaS_mm_per_masterStep = ((vmax_mm_s_X * vmax_mm_s_X)-v2_mm_s_X_sq)/(2.0f * accel_mm_s2_forBlock * accelRemaining);

  /* Serial.printf("Accel: %u, Cruise: %u, Decel: %u\n", 
                accelRemaining, cruiseRemaining, decelRemaining);*/

  // --- Start ISR with initial slow step ---
  stepInterval_us = 2000;   
  setupTimers(stepInterval_us);
  // set step interval for ~1 kHz stepping
  // setupTimers(2000);   // slow initial step interval (500 Hz)

}


void Task_DWIN (void* pvParameters) {
  while (1) {
  vTaskDelay (600); 
  }
}
void Task_HTML (void* pvParameters) {
  while (1) {
  vTaskDelay (600); 
  }
}

// === ISR for Motor Pulse ===
void IRAM_ATTR onStepTimer() {
  // For checking pause button
  if (!moveActive || movePaused) {
    timerStop(stepTimer);   // stop stepping if paused or inactive
    return;
  }

  // For stopping ISR timer at program end
  if (masterStepsRemaining <= 0) {
    // Finished current block
    moveActive = false;
    timerStop(stepTimer);
    return;
  }

  // --- Bresenham stepping ---
  errX += stepsX_forBlock;
  if (errX >= currentBlock.masterSteps) {
      if(X_STEP){
      digitalWrite(X_STEP_PIN, LOW);
      X_STEP=false;
      X_STEPS ++;
      }
      else {digitalWrite(X_STEP_PIN, HIGH);
      X_STEP=true;
      X_STEPS ++;
      }
      errX -= currentBlock.masterSteps;
  }

  errZ += stepsZ_forBlock;
  if (errZ >= currentBlock.masterSteps) {
      if(Z_STEP){
      digitalWrite(Z_STEP_PIN, LOW);
      Z_STEP=false;
      Z_STEPS ++;
      }
      else {digitalWrite(Z_STEP_PIN, HIGH);
      Z_STEP=true;
      Z_STEPS ++;
      }
    errZ -= currentBlock.masterSteps;
  }

  errC += stepsC_forBlock;
  if (errC >= currentBlock.masterSteps) {
      if(C_STEP){
      digitalWrite(C_STEP_PIN, LOW);
      C_STEP=false;
      C_STEPS ++;
      }
      else {digitalWrite(C_STEP_PIN, HIGH);
      C_STEP=true;
      C_STEPS ++;
      }
    errC -= currentBlock.masterSteps;
  }
 
  //--- Velocity update ---
  if (accelRemaining > 0) {
    v2_mm_s_X_sq += 2.0f * accel_mm_s2_forBlock * deltaS_mm_per_masterStep;
    accelRemaining--;
  } else if (cruiseRemaining > 0) {
    cruiseRemaining--;
  } else if (decelRemaining > 0) {
    v2_mm_s_X_sq -= 2.0f * accel_mm_s2_forBlock * deltaS_mm_per_masterStep;
    if (v2_mm_s_X_sq < 0.0f) v2_mm_s_X_sq = 0.0f;
    decelRemaining--;
  }
  float v_mm_s = sqrtf(v2_mm_s_X_sq);          // current velocity (mm/s)
  float steps_per_sec = v_mm_s * stepsPerMM_X; // convert to steps/s
  if (steps_per_sec > 250.0f) {
    stepInterval_us = (uint32_t)(1000000.0f / steps_per_sec);
  } else {
    stepInterval_us = 4000; // fallback slow step
  }

  masterStepsRemaining--;
  timerAlarm(stepTimer, stepInterval_us, false, 0); // reset to new time & autoload disable
  timerWrite(stepTimer,0); // reset the counter value to 0 alarm
}