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

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


    // 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 != "")
        {
            MPU9250Data mpudata_ = JsonMapper.ToObject<MPU9250Data>(jsonData);
            ahrsObj.Update((float)mpudata_.GyroX, (float)mpudata_.GyroY, (float)mpudata_.GyroZ, (float)mpudata_.AccelX, (float)mpudata_.AccelY, (float)mpudata_.AccelZ, (float)mpudata_.MagneticX, (float)mpudata_.MagneticY, (float)mpudata_.MagneticZ);
            MpuBoardTrans.transform.localEulerAngles = TransQuaternionToEularAngles(ahrsObj.Quaternion[0], ahrsObj.Quaternion[1], ahrsObj.Quaternion[2], ahrsObj.Quaternion[3]);
        }
    }





    //结束后关闭串口
    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;
    }
}