using System;
using System.Collections;
using System.Collections.Generic;
using System.IO.Ports;
using System.Threading;
using UnityEngine;
using LitJson;

public class MyUnitySerialPort : MonoBehaviour {
    private SerialPort serialPort;
    private Thread readPortData;
    //public List<byte> listReceive = new List<byte>();
    public string jsonData = "";
    public Transform MpuBoardTrans;


    // Use this for initialization
    void Start () {
        //Application.targetFrameRate = 120;
        try
        {
            serialPort = new SerialPort("COM4", 115200);
            serialPort.ReadTimeout = 500;
            if (!serialPort.IsOpen)
            {
                serialPort.Open();
            }
            readPortData = new Thread(new ThreadStart(readData));
            readPortData.IsBackground = true;
            readPortData.Start();
        }
        catch (Exception)
        {
            throw;
        }
    }
	
	// Update is called once per frame
	void Update () {
        if (jsonData != "") {
            MPUQuaternion mpudata_ = JsonMapper.ToObject<MPUQuaternion>(jsonData);
            MpuBoardTrans.transform.localEulerAngles = TransQuaternionToEularAngles((float)mpudata_.q0, (float)mpudata_.q1, (float)mpudata_.q2, (float)mpudata_.q3);
        }
	}




    
    //结束后关闭串口
    private void OnDestroy()
    {
        if (serialPort.IsOpen)
            serialPort.Close();
        readPortData.Abort();
    }

    //弧度转角度
    private float TransRadToAngle(double rad)
    {
        //return -(float)rad * 180 / Mathf.PI;
        return (float)rad * 90;
    }
    //Double转Float
    private float TransDoubleToFloat(double angle)
    {
        //return -(float)rad * 180 / Mathf.PI;
        return (float)angle;
    }
    /// <summary>
    /// 读取数据
    /// </summary>
    private void readData()
    {
        try
        {
            while (serialPort != null && serialPort.IsOpen)
            {
                byte addr = Convert.ToByte(serialPort.ReadByte());
                serialPort.DiscardInBuffer();
                //listReceive.Add(addr);
                //print("{"+serialPort.ReadLine());
                jsonData = "{" + serialPort.ReadLine();
            }
        }
        catch (Exception)
        {
            throw;
        }
    }

    //将四元数转换为欧拉角(pitch,yaw取负数的情况)
    public Vector3 TransQuaternionToEularAngles(float q0, float q1, float q2, float q3)
    {
        float pitch = -Mathf.Asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3f;
        float roll = Mathf.Atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3f;
        float yaw = -Mathf.Atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3f;
        Vector3 eularAngles = new Vector3(pitch, yaw, roll);
        return eularAngles;
    }
}