thesis-motion-guidance/Assets/Scripts/PlayerReplay.cs

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

public class PlayerReplay : MonoBehaviour
{
    // Joints already given the values from BodySourceView.cs that keeps updating
    public Transform[] joints = new Transform[25];

    public List<Vector3[]> jointsSequence = new List<Vector3[]>();
    public List<float> recordingTimes = new List<float>();
    public Material transparentMat;

    public Visualizer_FadeIn vfi;
    public Visualizer_FadeInSeries vfis;
    public BodyComparer bc;
    public StartStepPreview startStepPreview;
    public ModeController mc;

    public GameObject textFinish;

    private Dictionary<Kinect.JointType, Kinect.JointType> _BoneMap = new Dictionary<Kinect.JointType, Kinect.JointType>()
    {
        { Kinect.JointType.FootLeft, Kinect.JointType.AnkleLeft },
        { Kinect.JointType.AnkleLeft, Kinect.JointType.KneeLeft },
        { Kinect.JointType.KneeLeft, Kinect.JointType.HipLeft },
        { Kinect.JointType.HipLeft, Kinect.JointType.SpineBase },

        { Kinect.JointType.FootRight, Kinect.JointType.AnkleRight },
        { Kinect.JointType.AnkleRight, Kinect.JointType.KneeRight },
        { Kinect.JointType.KneeRight, Kinect.JointType.HipRight },
        { Kinect.JointType.HipRight, Kinect.JointType.SpineBase },

        { Kinect.JointType.HandTipLeft, Kinect.JointType.HandLeft },
        { Kinect.JointType.ThumbLeft, Kinect.JointType.HandLeft },
        { Kinect.JointType.HandLeft, Kinect.JointType.WristLeft },
        { Kinect.JointType.WristLeft, Kinect.JointType.ElbowLeft },
        { Kinect.JointType.ElbowLeft, Kinect.JointType.ShoulderLeft },
        { Kinect.JointType.ShoulderLeft, Kinect.JointType.SpineShoulder },

        { Kinect.JointType.HandTipRight, Kinect.JointType.HandRight },
        { Kinect.JointType.ThumbRight, Kinect.JointType.HandRight },
        { Kinect.JointType.HandRight, Kinect.JointType.WristRight },
        { Kinect.JointType.WristRight, Kinect.JointType.ElbowRight },
        { Kinect.JointType.ElbowRight, Kinect.JointType.ShoulderRight },
        { Kinect.JointType.ShoulderRight, Kinect.JointType.SpineShoulder },

        { Kinect.JointType.SpineBase, Kinect.JointType.SpineMid },
        { Kinect.JointType.SpineMid, Kinect.JointType.SpineShoulder },
        { Kinect.JointType.SpineShoulder, Kinect.JointType.Neck },
        { Kinect.JointType.Neck, Kinect.JointType.Head },
    };

    public void AddJoints(float recordingTime)
    {
        Vector3[] positions = new Vector3[25];

        for (int i = 0; i < joints.Length; i++)
        {
            positions[i] = joints[i].position;
        }

        jointsSequence.Add(positions);
        recordingTimes.Add(recordingTime);
    }

    public void ResetRecording()
    {
        jointsSequence.Clear();
        recordingTimes.Clear();
    }

    public void Save()
    {
        if (jointsSequence.Count == 0)
        {
            Debug.Log("jointsSequence is empty");
            return;
        }
        string path = Application.dataPath + "/Recordings/" + mc.complexity.ToString() + "_" + mc.direction.ToString() + ".sav";
        SaveSystem.Save(path, jointsSequence, recordingTimes);
        Debug.Log("Save success");
    }

    public IEnumerator Load()
    {
        if (mc.testing)
        {
            string path = Application.dataPath + "/Recordings/" + mc.complexity.ToString() + "_" + mc.direction.ToString() + ".sav";
            JointsDataSequence data = SaveSystem.Load(path);
            if (data == null)
            {
                Debug.Log("Load failed");
                //return;
                yield break;
            }
            yield return StartCoroutine(ShowJoints(data));
        }
        else
        {
            string[] complexity = { "OneArm", "TwoArms", "TwoArmsPlusLeg" };
            string[] direction = { "Sideways", "Forward", "Backward" };
            string[] feedback = { "None", "Color", "Haptic" };
            string[] speed = { "Slow", "Fast" };
            List<string> config = new List<string>();

            for (int i = 0; i < complexity.Length; i++)
            {
                for (int j = 0; j < direction.Length; j++)
                {
                    for (int k = 0; k < feedback.Length; k++)
                    {
                        for (int l = 0; l < speed.Length; l++)
                        {
                            config.Add(complexity[i] + ";" + direction[j] + ";" + feedback[k] + ";" + speed[l]);
                        }
                    }
                }
            }

            // Shuffle config list
            System.Random rand = new System.Random();
            int n = config.Count;
            while (n > 1)
            {
                n--;
                int k = rand.Next(n + 1);
                string value = config[k];
                config[k] = config[n];
                config[n] = value;
            }

            foreach (string s in config)
            {
                string[] subs = s.Split(';');
                string path = Application.dataPath + "/Recordings/" + subs[0] + "_" + subs[1] + ".sav";

                if (subs[0] == "OneArm")
                    mc.complexity = ModeController.Complexity.OneArm;
                else if (subs[0] == "TwoArms")
                    mc.complexity = ModeController.Complexity.TwoArms;
                else
                    mc.complexity = ModeController.Complexity.TwoArmsPlusLeg;

                if (subs[1] == "Sideways")
                    mc.direction = ModeController.Direction.Sideways;
                else if (subs[1] == "Forward")
                    mc.direction = ModeController.Direction.Forward;
                else
                    mc.direction = ModeController.Direction.Backward;

                if (subs[2] == "None")
                    mc.feedback = ModeController.Feedback.None;
                else if (subs[2] == "Color")
                    mc.feedback = ModeController.Feedback.ColorFeedback;
                else
                    mc.feedback = ModeController.Feedback.HapticFeedback;

                mc.speed = subs[3] == "Slow" ? ModeController.Speed.Slow : ModeController.Speed.Fast;

                JointsDataSequence data = SaveSystem.Load(path);
                if (data == null)
                {
                    Debug.Log("Load failed");
                    //return;
                    yield break;
                }
                yield return StartCoroutine(ShowJoints(data));
            }
        }
        yield return StartCoroutine(End());
        //textFinish.SetActive(true);
        //Debug.Log("Load finish");
        //ViveInput.StopPlaying();
    }

    private IEnumerator ShowJoints(JointsDataSequence data)
    {
        List<JointsData> jointsData = data.jointsDataSequence;
        List<float> recordingTimes = data.recordingTimes;
        yield return StartCoroutine(VisualizeFadeInSeries(jointsData, recordingTimes));
    }

    private IEnumerator Visualize(List<JointsData> newJointsData, List<float> newRecordingTimes)
    {
        for (int i = 0; i < newJointsData.Count; i++)
        {
            // Create GameObject body
            GameObject body = new GameObject("Recorded Body Demo " + i);
            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;

                // Create GameObject cubes for joints
                GameObject jointObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
                LineRenderer lr = jointObj.AddComponent<LineRenderer>();
                lr.positionCount = 2;
                lr.material = new Material(Shader.Find("Sprites/Default"))
                {
                    color = new Color(1, 0.8f, 0.6f)
                };
                lr.startWidth = 0.05f;
                lr.endWidth = 0.05f;

                jointObj.transform.localScale = new Vector3(0.05f, 0.05f, 0.05f);
                jointObj.name = jt.ToString();
                JointsData jd = newJointsData[i];
                jointObj.transform.position = new Vector3(jd.jointsPositionsX[(int)jt], jd.jointsPositionsY[(int)jt], jd.jointsPositionsZ[(int)jt]);
                jointObj.transform.parent = body.transform;

                // Remove LineRenderer component from neck
                if (jt == Kinect.JointType.Neck)
                {
                    Destroy(jointObj.GetComponent<LineRenderer>());
                }
            }

            // Connect the joints with LineRenderer
            for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
            {
                // Skip if dictionary not contains the joint or other these joints
                if (!_BoneMap.ContainsKey(jt) || jt == Kinect.JointType.Neck
                    || 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;

                Transform jointObj = body.transform.Find(jt.ToString());
                Transform targetJoint = body.transform.Find(_BoneMap[jt].ToString());
                LineRenderer lr = jointObj.GetComponent<LineRenderer>();

                lr.SetPosition(0, jointObj.localPosition);
                lr.SetPosition(1, targetJoint.localPosition);
            }

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

            if (mc.speed == ModeController.Speed.Slow)
                waitTime *= 2;

            yield return new WaitForSeconds(waitTime);
            Destroy(body);
        }
    }

    private IEnumerator VisualizeFadeIn(List<JointsData> newJointsData, List<float> newRecordingTimes)
    {
        for (int i = 0; i < newJointsData.Count; i++)
        {
            float waitTime = 1;
            if (i < newRecordingTimes.Count - 1)
            {
                waitTime = newRecordingTimes[i + 1] - newRecordingTimes[i];
            }
            WaitForSeconds wait = new WaitForSeconds(waitTime);

            Visualizer_FadeIn body = Instantiate(vfi) as Visualizer_FadeIn;
            body.SetData(newJointsData[i]);
            body.ShowBody();

            yield return wait;
        }
    }

    private IEnumerator VisualizeFadeInSeries(List<JointsData> jointsData, List<float> recordingTimes)
    {
        // Visualization demo
        yield return Visualize(jointsData, recordingTimes);

        bc.SetDataDemo(jointsData, recordingTimes);

        // Visualization steps/series

        // Filtered datas according to distance
        List<JointsData> filteredJointsData = new List<JointsData>();
        List<float> filteredRecordingTimes = new List<float>();

        JointsData prevJd = jointsData[0];

        // Add first pose
        filteredJointsData.Add(jointsData[0]);
        filteredRecordingTimes.Add(recordingTimes[0]);

        for (int i = 1; i < jointsData.Count; i++)
        {
            JointsData jd = jointsData[i];

            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;

                Vector3 prevJointPosition = new Vector3(prevJd.jointsPositionsX[(int)jt], prevJd.jointsPositionsY[(int)jt], prevJd.jointsPositionsZ[(int)jt]);
                Vector3 jointPosition = new Vector3(jd.jointsPositionsX[(int)jt], jd.jointsPositionsY[(int)jt], jd.jointsPositionsZ[(int)jt]);
                float distance = Vector3.Distance(prevJointPosition, jointPosition);

                // If a joint is bigger than a certain distance, add it to the replay, else ignore it
                if (distance >= 0.15f)
                {
                    filteredJointsData.Add(jd);
                    filteredRecordingTimes.Add(recordingTimes[i]);
                    prevJd = jd;
                    continue;
                }
            }
        }

        // Add last step of demo
        filteredJointsData.Add(jointsData[jointsData.Count - 1]);

        // Show start position of steps
        startStepPreview.SetData(filteredJointsData[0]);
        startStepPreview.ShowBody();

        // Wait for input
        yield return ViveInput.WaitForControllerPress();

        // Delete start position of steps
        startStepPreview.DeleteBody();

        Visualizer_FadeInSeries body = Instantiate(vfis) as Visualizer_FadeInSeries;
        body.transform.parent = gameObject.transform;
        body.SetData(filteredJointsData, filteredRecordingTimes);
        body.ShowBody();

        // Compare visualization demo with body
        yield return bc.StartCompare();

        // Input for end steps. Wait for input if finish comparing. Don't wait for input if comparing is interupted.
        if (!bc.endStepsPressed)
        {
            // Unknown reason, need 2 times (not working if only 1)
            yield return ViveInput.WaitForControllerPress();
            yield return ViveInput.WaitForControllerPress();
        }
        else
        {
            // Reset
            bc.endStepsPressed = false;
        }

        Destroy(body);

        // Write positions to csv
        bc.WriteCSV();

        // Wait for input
        // Unknown reason, need 2 times (not working if only 1)
        yield return ViveInput.WaitForControllerPress();
        yield return ViveInput.WaitForControllerPress();
    }

    private IEnumerator End()
    {
        textFinish.SetActive(true);
        yield return new WaitForSeconds(3);
        textFinish.SetActive(false);
        Debug.Log("Load finish");
        ViveInput.StopPlaying();
    }
}