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

public static class HelperScript
{
    static Vector3[] playerJoints;

    public static 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 static Vector3[] ConvertJointsDataToVector3Array(JointsData jd)
    {
        Vector3[] joints = new Vector3[jd.jointsPositionsX.Count];
        for (int i = 0; i < joints.Length; i++)
        {
            joints[i] = new Vector3(jd.jointsPositionsX[i], jd.jointsPositionsY[i], jd.jointsPositionsZ[i]);
        }
        return joints;
    }

    public static Vector3[] RescaleJoints(Vector3[] initJoints)
    {
        Vector3[] rescaledJoints = new Vector3[25];

        // Player's each bone length
        float lengthA = (playerJoints[(int)Kinect.JointType.SpineBase] - playerJoints[(int)Kinect.JointType.HipLeft]).magnitude;
        float lengthB = (playerJoints[(int)Kinect.JointType.HipLeft] - playerJoints[(int)Kinect.JointType.KneeLeft]).magnitude;
        float lengthC = (playerJoints[(int)Kinect.JointType.KneeLeft] - playerJoints[(int)Kinect.JointType.AnkleLeft]).magnitude;
        float lengthD = (playerJoints[(int)Kinect.JointType.AnkleLeft] - playerJoints[(int)Kinect.JointType.FootLeft]).magnitude;

        float lengthE = (playerJoints[(int)Kinect.JointType.SpineBase] - playerJoints[(int)Kinect.JointType.SpineMid]).magnitude;
        float lengthF = (playerJoints[(int)Kinect.JointType.SpineMid] - playerJoints[(int)Kinect.JointType.SpineShoulder]).magnitude;
        float lengthG = (playerJoints[(int)Kinect.JointType.SpineShoulder] - playerJoints[(int)Kinect.JointType.ShoulderLeft]).magnitude;
        float lengthH = (playerJoints[(int)Kinect.JointType.ShoulderLeft] - playerJoints[(int)Kinect.JointType.ElbowLeft]).magnitude;
        float lengthI = (playerJoints[(int)Kinect.JointType.ElbowLeft] - playerJoints[(int)Kinect.JointType.WristLeft]).magnitude;
        float lengthJ = (playerJoints[(int)Kinect.JointType.SpineShoulder] - playerJoints[(int)Kinect.JointType.Neck]).magnitude;

        rescaledJoints[(int)Kinect.JointType.SpineBase] = Vector3.zero;

        // Lower part
        rescaledJoints[(int)Kinect.JointType.HipLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipLeft], lengthA) + rescaledJoints[(int)Kinect.JointType.SpineBase];
        rescaledJoints[(int)Kinect.JointType.HipRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipRight], lengthA) + rescaledJoints[(int)Kinect.JointType.SpineBase];

        rescaledJoints[(int)Kinect.JointType.KneeLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.HipLeft], initJoints[(int)Kinect.JointType.KneeLeft], lengthB) + rescaledJoints[(int)Kinect.JointType.HipLeft];
        rescaledJoints[(int)Kinect.JointType.KneeRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.HipRight], initJoints[(int)Kinect.JointType.KneeRight], lengthB) + rescaledJoints[(int)Kinect.JointType.HipRight];

        rescaledJoints[(int)Kinect.JointType.AnkleLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.KneeLeft], initJoints[(int)Kinect.JointType.AnkleLeft], lengthC) + rescaledJoints[(int)Kinect.JointType.KneeLeft];
        rescaledJoints[(int)Kinect.JointType.AnkleRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.KneeRight], initJoints[(int)Kinect.JointType.AnkleRight], lengthC) + rescaledJoints[(int)Kinect.JointType.KneeRight];

        rescaledJoints[(int)Kinect.JointType.FootLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.AnkleLeft], initJoints[(int)Kinect.JointType.FootLeft], lengthD) + rescaledJoints[(int)Kinect.JointType.AnkleLeft] + new Vector3(0, 0.03f, 0);
        //initJoints[(int)Kinect.JointType.AnkleLeft], initJoints[(int)Kinect.JointType.FootLeft], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleLeft];
        rescaledJoints[(int)Kinect.JointType.FootRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.AnkleRight], initJoints[(int)Kinect.JointType.FootRight], lengthD) + rescaledJoints[(int)Kinect.JointType.AnkleRight] + new Vector3(0, 0.03f, 0);
        //initJoints[(int)Kinect.JointType.AnkleRight], initJoints[(int)Kinect.JointType.FootRight], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleRight];

        // Upper part
        rescaledJoints[(int)Kinect.JointType.SpineMid] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.SpineMid], lengthE) + rescaledJoints[(int)Kinect.JointType.SpineBase];

        rescaledJoints[(int)Kinect.JointType.SpineShoulder] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineMid], initJoints[(int)Kinect.JointType.SpineShoulder], lengthF) + rescaledJoints[(int)Kinect.JointType.SpineMid];

        rescaledJoints[(int)Kinect.JointType.ShoulderLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderLeft], lengthG) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];
        rescaledJoints[(int)Kinect.JointType.ShoulderRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderRight], lengthG) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];

        rescaledJoints[(int)Kinect.JointType.ElbowLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ShoulderLeft], initJoints[(int)Kinect.JointType.ElbowLeft], lengthH) + rescaledJoints[(int)Kinect.JointType.ShoulderLeft];
        rescaledJoints[(int)Kinect.JointType.ElbowRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ShoulderRight], initJoints[(int)Kinect.JointType.ElbowRight], lengthH) + rescaledJoints[(int)Kinect.JointType.ShoulderRight];

        rescaledJoints[(int)Kinect.JointType.WristLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ElbowLeft], initJoints[(int)Kinect.JointType.WristLeft], lengthI) + rescaledJoints[(int)Kinect.JointType.ElbowLeft];
        rescaledJoints[(int)Kinect.JointType.WristRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ElbowRight], initJoints[(int)Kinect.JointType.WristRight], lengthI) + rescaledJoints[(int)Kinect.JointType.ElbowRight];

        rescaledJoints[(int)Kinect.JointType.Neck] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.Neck], lengthJ) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];

        return rescaledJoints;
    }

    private static Vector3 GetTargetJointWithDesiredLength(Vector3 source, Vector3 target, float desiredLength)
    {
        Vector3 sourceToTarget = target - source;
        float length = sourceToTarget.magnitude;
        return sourceToTarget * desiredLength / length;
    }

    public static void SetPlayerJoints(Vector3[] playerJoints)
    {
        HelperScript.playerJoints = playerJoints;
    }
}