using System.Collections;
using System;
using UnityEngine;
using System.IO.Ports; // allows serial communication & changes .NET settings

public class step_response : MonoBehaviour
{ 
// declare variables

#region parameters
// port name
public string port = "/dev/cu.usbserial-A1085XH9";  
// baud rate
public int baudrate = 9600;
// declare serial port
private SerialPort sp;
// boolean variable for reading values
bool isStreaming = false;
// geometry to modify
public GameObject sph;

    bool seq = true;
    #endregion
    int byte1;
    int byte2;
    int byte3;

    int byte4;
    float filt = 0f;

    private float startTime;

    public float speed = 1000.0f;

    // Total distance between the markers.
    private float journeyLength;

    public Vector3 startMarker;

    public Vector3 endMarker;


    // Start is called before the first frame update
    void Start()
    {
         byte2=0;
         byte3=0;
         byte4=0;

        startTime = Time.time;

        journeyLength = Vector3.Distance(startMarker, endMarker);

    }

    void Update()
    {
        if (isStreaming)
        {
            #region python2csharp
            //while (1){
            byte1 = byte2;
            byte2 = byte3;
            byte3 = byte4;
            byte4 = (char)sp.ReadChar();

            //if ((byte1 == 1) && (byte2 == 2) && (byte3 == 3) && (byte4 == 4))
            //{
            //seq = false;
            ////break;


            int up_low = sp.ReadChar();
            int up_high = sp.ReadChar();
            int down_low = sp.ReadChar();
            int down_high = sp.ReadChar();
            int up_value = (256 * down_high + down_low);
            int down_value = (256 * up_high + up_low);
            int value = up_value - down_low;
            filt = (1 - 0.75f) * filt + 0.75f * (float)value;
            if (filt > 500)
            {
                // Distance moved equals elapsed time times speed..
                float distCovered = (Time.time - startTime) * speed;

                // Fraction of journey completed equals current distance divided by total distance.
                float fractionOfJourney = distCovered / journeyLength;

                // Set our position as a fraction of the distance between the markers.
                sph.transform.position = Vector3.Lerp(startMarker, endMarker, fractionOfJourney);
            }
            else
            {
                // Distance moved equals elapsed time times speed..
                float distCovered = (Time.time - startTime) * speed;
                // Fraction of journey completed equals current distance divided by total distance.
                float fractionOfJourney = distCovered / journeyLength;

                //transform.position = Vector3.Lerp(endMarker, startMarker, fractionOfJourney);
                sph.transform.localPosition = new Vector3(0,0, 0);

            }

            //else 

            //{
            Debug.Log(filt);
            ////sph.transform.localScale = new Vector3(value,value,value);
            //   //sph.transform.localPosition = new Vector3(sph.transform.position.x, sph.transform.position.y, sph.transform.position.z);
            //}


            //}
            //}
            #endregion
        }

    }
    // opens serial port & set program to read values from it
    public void Open()
    {
        isStreaming = true;

        sp = new SerialPort(port, baudrate);

        sp.ReadTimeout = 100;

        sp.Open(); // this opens the serial port

        Debug.Log("port was opened succesfully");
    }


    public int ReadSerialPort(int timeout = 50)
    {
        int message;

        sp.ReadTimeout = timeout;
        // try to read values from serial port
        try
        {
            message = sp.ReadByte();
            return message;
        }
        catch(TimeoutException)
        {
            return 0;
        }

    }


    public void Close()
    {
        sp.Close(); // closes the serial port
    }
    // Update is called once per frame
  
}