#include <ESP32Servo.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

#define SERVO_BASE_PIN     14
#define SERVO_SHOULDER_PIN 12
#define SERVO_ELBOW_PIN    13
#define SERVO_GRIPPER_PIN   4

const float L1 = 10.0;
const float L2 = 8.0;

Servo servoBase, servoShoulder, servoElbow, servoGripper;

float baseAngle = 90;
float shoulderAngle = 90;
float elbowAngle = 90;
float gripperAngle = 90;

float currentBase = 90;
float currentShoulder = 90;
float currentElbow = 90;
float currentGripper = 90;

typedef struct {
  float x, y, z, gripper;
} TargetPosition;

QueueHandle_t targetQueue;
SemaphoreHandle_t servoMutex;

void moveSmooth(float& current, float target, Servo& servo) {
  float step = 1.0;
  if (fabs(target - current) > step) {
    current += (target > current) ? step : -step;
    servo.write(current);
  } else {
    current = target;
    servo.write(current);
  }
}

bool calculateInverseKinematics(float x, float y, float z) {
  baseAngle = atan2(y, x) * RAD_TO_DEG;
  float r = sqrt(x * x + y * y);
  float d = sqrt(r * r + z * z);

  if (d > (L1 + L2) || d < fabs(L1 - L2)) return false;

  float cosElbow = (d * d - L1 * L1 - L2 * L2) / (2 * L1 * L2);
  cosElbow = constrain(cosElbow, -1.0, 1.0);
  elbowAngle = 180 - acos(cosElbow) * RAD_TO_DEG;

  float alpha = atan2(z, r);
  float beta = acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d));
  shoulderAngle = (alpha + beta) * RAD_TO_DEG;

  baseAngle = constrain(baseAngle, 0, 180);
  shoulderAngle = constrain(shoulderAngle, 0, 180);
  elbowAngle = constrain(elbowAngle, 0, 180);

  return true;
}

void inverseKinematicsTask(void *pvParameters) {
  TargetPosition newTarget;

  for (;;) {
    if (xQueueReceive(targetQueue, &newTarget, portMAX_DELAY)) {
      if (xSemaphoreTake(servoMutex, portMAX_DELAY)) {
        if (calculateInverseKinematics(newTarget.x, newTarget.y, newTarget.z)) {
          gripperAngle = constrain(newTarget.gripper, 0, 180);
          Serial.println("Target berhasil dihitung.");
        } else {
          Serial.println("Posisi di luar jangkauan.");
        }
        xSemaphoreGive(servoMutex);
      }
    }
    vTaskDelay(pdMS_TO_TICKS(10));
  }
}

void servoControlTask(void *pvParameters) {
  for (;;) {
    if (xSemaphoreTake(servoMutex, portMAX_DELAY)) {
      moveSmooth(currentBase, baseAngle, servoBase);
      moveSmooth(currentShoulder, shoulderAngle, servoShoulder);
      moveSmooth(currentElbow, elbowAngle, servoElbow);
      moveSmooth(currentGripper, gripperAngle, servoGripper);
      xSemaphoreGive(servoMutex);
    }
    vTaskDelay(pdMS_TO_TICKS(20));  // 50 Hz update rate
  }
}

void serialCommunicationTask(void *pvParameters) {
  for (;;) {
    if (Serial.available()) {
      String input = Serial.readStringUntil('\n');
      int i1 = input.indexOf(',');
      int i2 = input.indexOf(',', i1 + 1);
      int i3 = input.indexOf(',', i2 + 1);

      if (i1 != -1 && i2 != -1 && i3 != -1) {
        TargetPosition target;
        target.x = input.substring(0, i1).toFloat();
        target.y = input.substring(i1 + 1, i2).toFloat();
        target.z = input.substring(i2 + 1, i3).toFloat();
        target.gripper = input.substring(i3 + 1).toFloat();

        xQueueSend(targetQueue, &target, portMAX_DELAY);
      } else {
        Serial.println("Format salah. Gunakan: X,Y,Z,G");
      }
    }
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

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

  servoBase.attach(SERVO_BASE_PIN);
  servoShoulder.attach(SERVO_SHOULDER_PIN);
  servoElbow.attach(SERVO_ELBOW_PIN);
  servoGripper.attach(SERVO_GRIPPER_PIN);

  targetQueue = xQueueCreate(5, sizeof(TargetPosition));
  servoMutex = xSemaphoreCreateMutex();

  xTaskCreatePinnedToCore(inverseKinematicsTask, "IK", 4096, NULL, 2, NULL, 1);
  xTaskCreatePinnedToCore(servoControlTask, "Servo", 2048, NULL, 3, NULL, 1);
  xTaskCreatePinnedToCore(serialCommunicationTask, "Serial", 2048, NULL, 1, NULL, 0);

  Serial.println("Masukkan koordinat: X,Y,Z,G");
}

void loop() {
  vTaskDelay(portMAX_DELAY); // Kosong karena pakai RTOS
}