using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Kinect = Windows.Kinect;
using Valve.VR;

public class BodyComparer : MonoBehaviour
{
    public BodySourceView bsv;
    public ViveInput viveInput;

    private List<JointsData> jointsDataDemo = new List<JointsData>();
    private List<float> recordingTimesDemo = new List<float>();

    // For saving to CSV
    private List<List<Vector3>> posJointsDemos = new List<List<Vector3>>();
    private List<List<Vector3>> posJointsBodys = new List<List<Vector3>>();
    private List<float> csvTimes = new List<float>();

    public void SetDataDemo(List<JointsData> jointsDataDemo, List<float> recordingTimesDemo)
    {
        this.jointsDataDemo = jointsDataDemo;
        this.recordingTimesDemo = recordingTimesDemo;
    }

    public IEnumerator StartCompare()
    {
        if (bsv.body == null)
        {
            yield return new WaitForSeconds(3);
            yield break;
        }

        for (int i = 0; i < jointsDataDemo.Count; i++)
        {
            Transform body = bsv.body.transform;
            List<Vector3> oneBodyDemo = new List<Vector3>();
            List<Vector3> oneBodyReal = new List<Vector3>();

            for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
            {
                // Skip these joints
                if (jt == Kinect.JointType.Head || jt == Kinect.JointType.ThumbLeft || jt == Kinect.JointType.ThumbRight
                        || jt == Kinect.JointType.HandLeft || jt == Kinect.JointType.HandRight
                        || jt == Kinect.JointType.HandTipLeft || jt == Kinect.JointType.HandTipRight)
                    continue;

                JointsData jd = jointsDataDemo[i];
                Vector3 posJointDemo = new Vector3(jd.jointsPositionsX[(int)jt], jd.jointsPositionsY[(int)jt], jd.jointsPositionsZ[(int)jt]);
                Vector3 posJointBody = body.GetChild((int)jt).position;

                // Save positions demo and real body
                oneBodyDemo.Add(posJointDemo);
                oneBodyReal.Add(posJointBody);

                float distance = (posJointBody - posJointDemo).magnitude;

                if (distance > 0.15f)
                {
                    body.GetChild((int)jt).GetComponent<Renderer>().material.color = Color.red;

                    // TODO: Haptic feedback
                    if (jt == Kinect.JointType.WristLeft)
                    {
                        SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.LeftHand].Execute(0, 0.01f, 10, 1);
                    }
                    else if (jt == Kinect.JointType.WristRight)
                    {
                        SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.RightHand].Execute(0, 0.01f, 10, 1);
                    }
                    else if (jt == Kinect.JointType.AnkleLeft)
                    {
                        SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.LeftFoot].Execute(0, 0.01f, 10, 1);
                    }
                    else if (jt == Kinect.JointType.AnkleRight)
                    {
                        SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.RightFoot].Execute(0, 0.01f, 10, 1);
                    }
                }
                else
                {
                    body.GetChild((int)jt).GetComponent<Renderer>().material.color = Color.white;
                }
            }

            // Save it to csv later
            posJointsDemos.Add(oneBodyDemo);
            posJointsBodys.Add(oneBodyReal);
            csvTimes.Add(viveInput.csvTime);

            float waitTime = 0.01f;
            if (i < recordingTimesDemo.Count - 1)
            {
                waitTime = recordingTimesDemo[i + 1] - recordingTimesDemo[i];
            }

            yield return new WaitForSeconds(waitTime);
        }

        // Make all joints white
        Transform bodyEnd = bsv.body.transform;
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            // Skip these joints
            if (jt == Kinect.JointType.Head || jt == Kinect.JointType.ThumbLeft || jt == Kinect.JointType.ThumbRight
                    || jt == Kinect.JointType.HandLeft || jt == Kinect.JointType.HandRight
                    || jt == Kinect.JointType.HandTipLeft || jt == Kinect.JointType.HandTipRight)
                continue;

            bodyEnd.GetChild((int)jt).GetComponent<Renderer>().material.color = Color.white;
        }
    }

    public void WriteCSV()
    {
        Debug.Log("Save to CSV");
        CSVWriter.WriteCSV(posJointsDemos, posJointsBodys, csvTimes);
    }
}