/* ============================================
  code is placed under the MIT license
  Copyright (c) 2025 J-M-L (https://forum.arduino.cc/u/j-m-l)
  For the Arduino Forum : https://forum.arduino.cc/t/arduino-scaraarm/1414033?u=j-m-l

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
  ===============================================
*/

#include <SPI.h>
#include <SD.h>

const byte CS_PIN = 5;
const char *fileName = "/circle.txt";

constexpr double L1 = 290.0f;
constexpr double L2 = 290.0f;
const double STEP_MM = 1;

const double DEG_HALF_REV = 180.0;

#define MOTOR_A_STEP 14
#define MOTOR_A_DIR 32
#define MOTORS_EN 12 // pin is connected to both motors EN pin.

#define MOTOR_B_STEP 27
#define MOTOR_B_DIR 15

constexpr double MICROSTEPPING = 16.0;
constexpr double STEPS_PER_REV = 200.0;
constexpr double STEPS_PER_DEG_A = (STEPS_PER_REV * MICROSTEPPING) / 360.0;
constexpr double STEPS_PER_DEG_B = (STEPS_PER_REV * MICROSTEPPING) / 360.0;

const unsigned long stepDelayUs = 500;  // to be adjusted based on your hardware


double lastTheta1 = 0;
double lastTheta2 = 0;
double lastX = 0;
double lastY = 0;
bool haveLast = false;


bool xyToScara(double x, double y, double &t1, double &t2, bool dbg) {
  double r2 = x * x + y * y;
  double c2 = (r2 - L1 * L1 - L2 * L2) / (2.0f * L1 * L2);
  if (c2 < -1.0f || c2 > 1.0f) return false;
  double s2 = sqrt(1.0f - c2 * c2);
  t2 = atan2(s2, c2) * DEG_HALF_REV / M_PI;
  double k1 = L1 + L2 * c2;
  double k2 = L2 * s2;
  t1 = (atan2(y, x) - atan2(k2, k1)) * DEG_HALF_REV / M_PI;
  return true;
}


void moveMotorsTo(double targetTheta1, double targetTheta2) {

  double diffA = targetTheta1 - lastTheta1;
  double diffB = targetTheta2 - lastTheta2;

  digitalWrite(MOTOR_A_DIR, diffA >= 0 ? HIGH : LOW);
  digitalWrite(MOTOR_B_DIR, diffB >= 0 ? HIGH : LOW);

  long stepsA = abs(diffA * STEPS_PER_DEG_A);
  long stepsB = abs(diffB * STEPS_PER_DEG_B);
  long maxSteps = max(stepsA, stepsB);
  if (maxSteps == 0) return;

  long counterA = 0;
  long counterB = 0;

  for (long i = 0; i < maxSteps; i++) {
    if ((i * stepsA) / maxSteps > counterA) {
      digitalWrite(MOTOR_A_STEP, HIGH);
      counterA++;
    }
    if ((i * stepsB) / maxSteps > counterB) {
      digitalWrite(MOTOR_B_STEP, HIGH);
      counterB++;
    }

    delayMicroseconds(stepDelayUs);

    digitalWrite(MOTOR_A_STEP, LOW);
    digitalWrite(MOTOR_B_STEP, LOW);

    delayMicroseconds(stepDelayUs);
  }

  lastTheta1 = targetTheta1;
  lastTheta2 = targetTheta2;
}




void interpolateAndMove(double x0, double y0, double x1, double y1) {
  double dx = x1 - x0;
  double dy = y1 - y0;
  double dist = sqrt(dx * dx + dy * dy);
  int steps = dist / STEP_MM;
  if (steps < 1) steps = 1;

  for (int i = 1; i <= steps; i++) {
    double u = double(i) / double(steps);
    double xi = x0 + u * dx;
    double yi = y0 + u * dy;
    double t1, t2;
    if (xyToScara(xi, yi, t1, t2, false)) {
      Serial.print("θ1=");
      Serial.print(t1);
      Serial.print(" θ2=");
      Serial.println(t2);
      moveMotorsTo(t1, t2);
    } else {
      Serial.println("unreachable point");
      return;
    }
  }
}



void executeGcodeLine(String &line) {
  if (line.length() == 0) return;
  if (line.startsWith(";")) return;

  if (line.startsWith("G0") || line.startsWith("G1")) {
    double x, y, t1, t2;
    if (sscanf(line.c_str(), "%*s X%lf Y%lf", &x, &y) == 2) {
      if (!haveLast) {
        haveLast = true;
        lastX = x;
        lastY = y;
      } else {
        interpolateAndMove(lastX, lastY, x, y);
        lastX = x;
        lastY = y;
      }
    } else {
      Serial.println("invalid G0/G1 format");
    }
  } else if (line.startsWith("G28")) {
    Serial.println("homeMotors");
  } else if (line.startsWith("M0")) {
    Serial.println("PAUSE");
  } else if (line.startsWith("M2")) {
    Serial.println("STOP");
  } else {
    Serial.print("could not decode: ");
    Serial.println(line);
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(MOTOR_A_STEP, OUTPUT);
  pinMode(MOTOR_A_DIR, OUTPUT);

  pinMode(MOTOR_B_STEP, OUTPUT);
  pinMode(MOTOR_B_DIR, OUTPUT);

  pinMode(MOTORS_EN, OUTPUT);
  digitalWrite(MOTORS_EN, LOW);


  SD.begin(CS_PIN);
  File f = SD.open(fileName);
  if (!f) {
    Serial.println("file open error");
    return;
  }
  Serial.println("STARTING");
  while (f.available()) {
    String line = f.readStringUntil('\n');
    line.trim();
    executeGcodeLine(line);
  }
  f.close();
  Serial.println("DONE");

}

void loop() {}