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

public class QuadcoperClient : MonoBehaviour {
    private SerialPort port;
    public string portName = "COM5";
    private bool _stream = false;

    private int[] motor_speeds = new int[4];

    // Use this for initialization
    void Start() {
        Connect();
    }

    // Update is called once per frame
    void Update() {
        if (port != null && port.IsOpen)
        {
           // UpdateOrientation();
        }
    }

    void Connect() {
        port = new SerialPort(portName);
        port.BaudRate = 1500000;
        port.Open();
        port.ReadTimeout = SerialPort.InfiniteTimeout;
    }

    void Disconnect() {
        port.Close();
    }

    static Quaternion AccelerationToOrientation(Vector3 acceleration)
    {
        acceleration.x = -acceleration.x;
        acceleration.z = -acceleration.z;
        if (Vector3.Dot(acceleration.normalized, Vector3.up) > 0.9999f){
            return Quaternion.identity;
        }
        return Quaternion.FromToRotation(Vector3.up, acceleration);
    }

    public void SetMotorSpeed(byte motor, byte speed) {
        port.ExecuteCommand(UartCommand.Create(uart_commands.motor_write).Write<byte>(motor).Write<byte>(speed));
    }

    public Vector3 ReadAccelerometer() {
        var response = port.ExecuteCommand(UartCommand.Create(uart_commands.acc_read));
        var result = response.CreateReader().Read<Vector3>();
        return new Vector3(result.x, result.z, result.y);
    }

    void SyncTransform()
    {
        transform.localRotation = AccelerationToOrientation(ReadAccelerometer());
    }

    void OnGUI() {

        for (int i = 0; i < 4; ++i)
        {
            var new_speed = (int)GUI.HorizontalSlider(new Rect(10, 10 + 30 * i, 300, 30), motor_speeds[i], 0, 100);
            if (new_speed != motor_speeds[i]) {
                motor_speeds[i] = new_speed;
                SetMotorSpeed((byte)i, (byte)new_speed);
            }
        }

        if (GUILayout.Button("Play sound"))
        {
            port.ExecuteCommand(UartCommand.Create(uart_commands.play_sound));
        }

        if (GUI.Button(new Rect(400, 10, 150, 30), "Read accelerometer"))
        {
            Debug.Log(ReadAccelerometer());
            SyncTransform();
        }

        if (_stream)
        {
            if (GUI.Button(new Rect(10, 130, 150, 30), "Stop stream"))
            {
                _stream = false;
            }
        } else {
            if (GUI.Button(new Rect(10, 130, 150, 30), "Start stream")) {
                _stream = true;
            }
        }

        if (_stream)
        {
            SyncTransform();
        }

    }

    void OnDestroy() {
        Disconnect();
    }
}