#include <AccelStepper.h>
#include "HX711.h"

// Pin settings
const int INTERFACE_TYPE = 1; 
const int STEP_PIN = D5; 
const int DIR_PIN = D4; 
const int LOADCELL_DOUT_PIN = D1;
const int LOADCELL_SCK_PIN = D0;
const int SW_PIN = D10;
const int analogInPin = A2;

// Variables
long HX711_reading = 0;
int sensorValue = 0;
int switch_status = 1;
int machine_status = 0; // 0:Idle, 1:Forward(Down), 2:Reverse(Up)

// Custom settings
const long THRESHOLD = 250000;    
const float MOVE_SPEED = 10000;   // Forward speed
const float ESCAPE_SPEED = 5000;  // Reverse speed

HX711 scale;
AccelStepper Xaxis(INTERFACE_TYPE, STEP_PIN, DIR_PIN);

void setup() {
  pinMode(SW_PIN, INPUT_PULLUP);
  Xaxis.setMaxSpeed(10000);
  Serial.begin(115200);
}

void loop() {
  int current_switch = digitalRead(SW_PIN);

  // --- 1. Switch Control ---
  if (current_switch != switch_status && current_switch == LOW) {
    if (machine_status == 0) {
      machine_status = 1; // Start movement
      Xaxis.setSpeed(MOVE_SPEED);
      Serial.println("START: FORWARD");
    } else {
      machine_status = 0; // Stop movement
      Xaxis.stop();
      Xaxis.disableOutputs();
      Serial.println("STOP: BY SWITCH");
    }
    delay(200); // Debounce
  }
  switch_status = current_switch;

  // --- 2. Motor Logic ---
  if (machine_status == 1) {
    // Forward mode: check for overload
    if (HX711_reading < -THRESHOLD) {
      machine_status = 2; // Auto-reverse
      Xaxis.setSpeed(-ESCAPE_SPEED); 
      Serial.println("!!! OVERLOAD: REVERSING !!!");
    } else {
      Xaxis.runSpeed();
    }
  } 
  else if (machine_status == 2) {
    // Reverse mode: keep moving back
    Xaxis.runSpeed();
  } 
  else {
    // Stop mode
    Xaxis.disableOutputs();
  }
}

// --- Data Acquisition (Core 1) ---
void setup1() {
  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
}

void loop1() {
  if (scale.is_ready()) {
    HX711_reading = scale.read();
    sensorValue = analogRead(analogInPin);
    
    // Print data for Serial Plotter
    Serial.print(600);
    Serial.print(",");
    Serial.print(sensorValue);
    Serial.print(",");
    Serial.println(HX711_reading);
  }
  delay(100);
}