using System.Collections;
using System.Collections.Generic;
using UnityEngine;

using System.IO.Ports;

public class espwrite : MonoBehaviour
{
    public GameObject Thumb;
    public GameObject Index;

    

    SerialPort ESP32_port = new SerialPort("COM15", 115200);
    bool toggle = true;
    bool periodicEnabled = false;

    double time;

    [Header("Interaction Values")]

    public double thumb_val;
    public double index_val;
    public bool invert_finger = false;

    [Range(0.035f, 1f)]
    public double timeDelay = 0.045f;

    [Header("Index thimble management")]

    [Range(36, 255)]
    public int idx_lim = 255;

    [Range(0, 100)]
    public int idx_offset = 0;

    [Header("Thumb thimble management")]

    [Range(36, 255)]
    public int plx_lim = 255;

    [Range(0, 100)]
    public int plx_offset = 0;

    [Header("Objects stiffness management")]
    [Range(0.001f, 1.0f)] 
    public double stiffness = 0.3f;

    byte[] outBuffer = new byte[2];
    // Start is called before the first frame update
    void Start()
    {
        ESP32_port.Open();
        time = 0f;
        timeDelay = 0.045f;
        //InvokeRepeating("Serial_Data_Write", 0f, 0.05f);
    }

    // Update is called once per frame
    void Update()
    {
        time = time + 1f * Time.deltaTime;
        if(time >= timeDelay)
        {
            time = 0f;

            thumb_val = Thumb.GetComponent<fingertip_interaction_manager>().getDisplacement();
            index_val = Index.GetComponent<fingertip_interaction_manager>().getDisplacement();

            if (thumb_val > this.transform.localScale.x)
                thumb_val = this.transform.localScale.x;

            if (index_val > this.transform.localScale.x)
                index_val = this.transform.localScale.x;




            thumb_val = map(thumb_val , 0, this.transform.localScale.x, 36 + plx_offset, plx_lim);
            index_val = map(index_val , 0, this.transform.localScale.x, 36 + idx_offset, idx_lim);

            //thumb_val = stiffness * thumb_val;
            //index_val = stiffness * index_val;
            int thumb_val_int = System.Convert.ToInt32(System.Math.Round(thumb_val));
            int index_val_int = System.Convert.ToInt32(System.Math.Round(index_val));

            if (invert_finger)
            {
                Serial_Data_Write(thumb_val_int, index_val_int);
            }
            else
            {
                Serial_Data_Write(index_val_int, thumb_val_int);
            }
            

            

        }

     
        
        
    }

    // Function called repeatedly
    void Serial_Data_Write(int motor_idx_val, int motor_plx_val)
    {
        if (ESP32_port.IsOpen)
        {
            outBuffer[0] = System.Convert.ToByte(motor_idx_val);
            outBuffer[1] = System.Convert.ToByte(motor_plx_val);
            ESP32_port.Write(outBuffer, 0, outBuffer.Length);
        }
    }

    // c#
    double map(double s, double a1, double a2, double b1, double b2)
    {
        return b1 + (s - a1) * (b2 - b1) / (a2 - a1);
    }
}
