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;
    public ModeController mc;
    public Haptics haptics;

    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>();

    private bool isComparing;
    public bool endStepsPressed;

    private void Update()
    {
        if (!isComparing)
            return;

        if (SteamVR_Actions.default_GrabPinch.GetStateDown(SteamVR_Input_Sources.Any) || Input.GetKeyDown(KeyCode.Space))
        {
            isComparing = false;
            endStepsPressed = true;
        }
    }

    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;
        }

        isComparing = true;

        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)
                {
                    if (mc.feedback == ModeController.Feedback.ColorFeedback)
                    {
                        body.GetChild((int)jt).GetComponent<Renderer>().material.color = Color.red;
                    } else if (mc.feedback == ModeController.Feedback.HapticFeedback)
                    {
                        // TODO: Haptic feedback
                        if (jt == Kinect.JointType.WristLeft)
                        {
                            SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.LeftHand].Execute(0, 0.1f, 10, 1);
                        }
                        else if (jt == Kinect.JointType.WristRight)
                        {
                            SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.RightHand].Execute(0, 0.1f, 10, 1);
                        }
                        else if (jt == Kinect.JointType.AnkleLeft)
                        {
                            haptics.Vibrate(15, 1, 100);
                        }
                        else if (jt == Kinect.JointType.AnkleRight)
                        {
                            haptics.Vibrate(16, 1, 100);
                        }
                    }
                }
                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);

            // If pressed stop, end immediately and save last to csv
            if (!isComparing)
            {
                for (int j = i + 1; j < jointsDataDemo.Count; j++)
                {
                    List<Vector3> oneBodyDemo2 = 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[j];
                        Vector3 posJointDemo = new Vector3(jd.jointsPositionsX[(int)jt], jd.jointsPositionsY[(int)jt], jd.jointsPositionsZ[(int)jt]);

                        // Save positions demo and real body
                        oneBodyDemo2.Add(posJointDemo);
                    }
                    // Save it to csv later
                    posJointsDemos.Add(oneBodyDemo2);
                    posJointsBodys.Add(oneBodyReal);
                    csvTimes.Add(viveInput.csvTime);
                }

                // Make all joints white
                Transform bodyEnd2 = 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;

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

                yield break;
            }

            float waitTime = 0;
            if (isComparing && i < recordingTimesDemo.Count - 1)
            {
                waitTime = recordingTimesDemo[i + 1] - recordingTimesDemo[i];
            }

            yield return new WaitForSeconds(waitTime);
        }

        isComparing = false;

        // 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, mc);
    }
}