using System;
using System.IO.Ports;
using UnityEngine;

public class ArduinoSerialManager : MonoBehaviour
{
    SerialPort portConnection;

    [Header("Index Joints")]
    public Transform index;
    public Transform index2;
    public Transform index3;

    [Header("Middle Joints")]
    public Transform middle;
    public Transform middle2;
    public Transform middle3;

    [Header("Ring Joints")]
    public Transform ring;
    public Transform ring2;
    public Transform ring3;

    [Header("Pinky Joints")]
    public Transform pinky;
    public Transform pinky2;
    public Transform pinky3;

    [Header("Object to Move/Rotate with Gyro")]
    public Transform objectToMove;

    private int[] flexSensorValues = new int[4];
    private float[] imuValues = new float[6];

    [Header("Per-Finger Calibration (min)")]
    public int[] flexMin = new int[4] { 700, 700, 700, 700 };

    [Header("Per-Finger Calibration (max)")]
    public int[] flexMax = new int[4] { 800, 800, 800, 800 };

    [Header("Angle and Smoothing")]
    public float maxAngle = 60f;
    public float smoothSpeed = 12f;

    private float[] currentAngles = new float[4];
    private Vector3 currentPosition = Vector3.zero;
    private Quaternion currentRotation = Quaternion.identity;

    [Header("Serial Settings")]
    public string portName = "COM3";
    public int baudRate = 115200;

    [Header("Accel Calibration")]
    public bool calibrateRestingAccel = false;
    private Vector3 accelRestOffset = Vector3.zero;
    private bool accelCalibrated = false;

    private const float accelScale = 16384f;

    void Start()
    {
        Debug.Log("Available serial ports: " + string.Join(", ", SerialPort.GetPortNames()));

        try
        {
            portConnection = new SerialPort(portName, baudRate);
            portConnection.ReadTimeout = 100;
            portConnection.Open();
            Debug.Log("Serial port opened on " + portName);
        }
        catch (Exception e)
        {
            Debug.LogError("Error opening serial port: " + e.Message);
        }

        // Optional: Set initial position higher to avoid "hand on floor"
        currentPosition = new Vector3(0, 0.2f, 0);
    }

    void Update()
    {
        if (calibrateRestingAccel && !accelCalibrated)
        {
            CalibrateRestingAccel();
            calibrateRestingAccel = false;
        }

        if (portConnection != null && portConnection.IsOpen)
        {
            try
            {
                while (portConnection.BytesToRead > 0)
                {
                    string line = portConnection.ReadLine();
                    Debug.Log("Received line: " + line);
                    ProcessSerialData(line);
                }
            }
            catch (TimeoutException) { }
            catch (Exception e)
            {
                Debug.LogError("Serial read error: " + e.Message);
            }
        }

        UpdateFingerJoints();
        UpdatePositionAndRotation();
    }

    public void CalibrateRestingAccel()
    {
        accelRestOffset = new Vector3(imuValues[0], imuValues[1], imuValues[2]) / accelScale;
        accelCalibrated = true;
        Debug.Log($"Calibrated resting accel offset (g units): {accelRestOffset}");
    }

    void ProcessSerialData(string line)
    {
        string[] parts = line.Split(',');

        if (parts.Length != 10)
        {
            Debug.LogWarning($"Unexpected data length: {parts.Length} (expected 10)");
            return;
        }

        for (int i = 0; i < 6; i++)
        {
            if (!float.TryParse(parts[i], out imuValues[i]))
            {
                Debug.LogWarning($"Failed to parse IMU value at index {i}: '{parts[i]}'");
                return;
            }
        }

        for (int i = 0; i < 4; i++)
        {
            if (!int.TryParse(parts[6 + i], out flexSensorValues[i]))
            {
                Debug.LogWarning($"Failed to parse flex sensor value at index {i}: '{parts[6 + i]}'");
                return;
            }
        }
    }

    float MapFlexToAngle(int fingerIndex, int flexValue)
    {
        if (fingerIndex < 0 || fingerIndex >= flexMin.Length)
        {
            Debug.LogWarning("Invalid fingerIndex in MapFlexToAngle: " + fingerIndex);
            return 0f;
        }

        int min = flexMin[fingerIndex];
        int max = flexMax[fingerIndex];
        int clamped = Mathf.Clamp(flexValue, min, max);

        float normalized = max > min ? (float)(clamped - min) / (max - min) : 0f;
        return normalized * maxAngle;
    }

    void RotateFinger(Transform joint1, Transform joint2, Transform joint3, float targetAngle)
    {
        if (joint1 != null)
            joint1.localRotation = Quaternion.Euler(Mathf.LerpAngle(joint1.localRotation.eulerAngles.x, targetAngle, Time.deltaTime * smoothSpeed), 0, 0);
        if (joint2 != null)
            joint2.localRotation = Quaternion.Euler(Mathf.LerpAngle(joint2.localRotation.eulerAngles.x, targetAngle * 0.2f, Time.deltaTime * smoothSpeed), 0, 0);
        if (joint3 != null)
            joint3.localRotation = Quaternion.Euler(Mathf.LerpAngle(joint3.localRotation.eulerAngles.x, targetAngle * 0.1f, Time.deltaTime * smoothSpeed), 0, 0);
    }

    void UpdateFingerJoints()
    {
        for (int i = 0; i < 4; i++)
        {
            float target = MapFlexToAngle(i, flexSensorValues[i]);
            currentAngles[i] = Mathf.Lerp(currentAngles[i], target, Time.deltaTime * smoothSpeed);
        }

        RotateFinger(index, index2, index3, currentAngles[0]);
        RotateFinger(middle, middle2, middle3, currentAngles[1]);
        RotateFinger(ring, ring2, ring3, currentAngles[2]);
        RotateFinger(pinky, pinky2, pinky3, currentAngles[3]);
    }

    void UpdatePositionAndRotation()
    {
        float dt = Time.deltaTime;

        // Remap gyro axes to match your physical hand movement
        // imuValues[3] = Gyro X (pitch), imuValues[4] = Gyro Y (yaw), imuValues[5] = Gyro Z (roll)
        Vector3 gyro = new Vector3(
            -imuValues[3], // Invert X: forward tilt = upward in-game
             imuValues[4], // Yaw unchanged
            -imuValues[5]  // Invert Z: sideways tilt matches roll
        );

        Quaternion deltaRotation = Quaternion.Euler(gyro * dt);
        currentRotation = Quaternion.Slerp(currentRotation, currentRotation * deltaRotation, dt * smoothSpeed);

        if (objectToMove != null)
        {
            objectToMove.localRotation = currentRotation;

            Vector3 accelG = new Vector3(imuValues[0], imuValues[1], imuValues[2]) / accelScale;
            if (accelCalibrated)
                accelG -= accelRestOffset;

            accelG.y = Mathf.Clamp(accelG.y, -0.5f, 0.5f);
            Vector3 targetPosition = accelG * 0.1f;
            currentPosition = Vector3.Lerp(currentPosition, targetPosition, dt * smoothSpeed);
            objectToMove.localPosition = currentPosition;
        }
    }

    void OnApplicationQuit()
    {
        if (portConnection != null && portConnection.IsOpen)
        {
            portConnection.Close();
            Debug.Log("Serial port closed.");
        }
    }
}