using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using System.Drawing;
using System.Windows.Forms;

namespace FFTDDR
{
    class SkeletonManager
    {
        public Skeleton[] skeletons;
        public bool[] calibrated, calibrating, currentlyTracked;
        public Vector4[] centre;
        public Vector4[] leftFoot;
        public Vector4[] rightFoot;
        public Vector4[] deadSpot;
        public float   deadSpotRadius = 0.2f;
        public bool previousLeftHandRaised = false;
        public float previousRightHandY = 0;


        public SkeletonManager(int numSkeletons)
        {
            skeletons = new Skeleton[numSkeletons];
            calibrated = new bool[numSkeletons];
            calibrating = new bool[numSkeletons];
            currentlyTracked = new bool[numSkeletons];
            centre = new Vector4[numSkeletons];
            leftFoot = new Vector4[numSkeletons];
            rightFoot = new Vector4[numSkeletons];
            deadSpot = new Vector4[numSkeletons];

            
            for (int i = 0; i < numSkeletons; i++)
            {
                calibrated[i] = false;
                calibrating[i] = false;
                currentlyTracked[i] = false;

            }
        }
        public void Update(Skeleton[] newSkeletons)
        {
            skeletons = newSkeletons;

            for (int i = 0; i < skeletons.Length; i++)
            {
                if(calibrated[i] == true){
                    leftFoot[i].X = skeletons[i].Joints[JointType.AnkleLeft].Position.X;
                    leftFoot[i].Y = skeletons[i].Joints[JointType.AnkleLeft].Position.Y;
                    leftFoot[i].Z = skeletons[i].Joints[JointType.AnkleLeft].Position.Z;
                    rightFoot[i].X = skeletons[i].Joints[JointType.AnkleRight].Position.X;
                    rightFoot[i].Y = skeletons[i].Joints[JointType.AnkleRight].Position.Y;
                    rightFoot[i].Z = skeletons[i].Joints[JointType.AnkleRight].Position.Z;


                }
            }

        }

        public int temp = 0;

        public string Calibrate(int skeletonNo)
        {

                if (calibrating[skeletonNo] == false &&
                    skeletons[skeletonNo].Joints[JointType.HandLeft].Position.Y > skeletons[skeletonNo].Joints[JointType.Head].Position.Y
                    && skeletons[skeletonNo].Joints[JointType.HandRight].Position.Y > skeletons[skeletonNo].Joints[JointType.Head].Position.Y)
                {
                    return "menu";
                }

                if (calibrating[skeletonNo] == false &&
                    skeletons[skeletonNo].Joints[JointType.HandLeft].Position.X > skeletons[skeletonNo].Joints[JointType.HandRight].Position.X)
                {

                    calibrating[skeletonNo] = true;
                    centre[skeletonNo] = calculateCentre(skeletons[skeletonNo]);

                    deadSpot[skeletonNo].X = centre[skeletonNo].X - deadSpotRadius / 2;
                    deadSpot[skeletonNo].Z = centre[skeletonNo].Z - deadSpotRadius / 2;

                    calibrating[skeletonNo] = false;
                    calibrated[skeletonNo] = true;

                    return "continue";
                }

                // If right hand is above collarbone, only if noncalibrated.

                if (calibrating[skeletonNo] == false &&
                    skeletons[skeletonNo].Joints[JointType.HandLeft].Position.Y > skeletons[skeletonNo].Joints[JointType.ShoulderCenter].Position.Y)
                {
                    if (previousLeftHandRaised == false)
                    {
                        previousLeftHandRaised = true;
                        previousRightHandY = skeletons[skeletonNo].Joints[JointType.HandRight].Position.Y;
                    }
                    float heightChange = previousRightHandY - skeletons[skeletonNo].Joints[JointType.HandRight].Position.Y;
                    if (heightChange < 0)
                    {
                        return heightChange.ToString();

                    }
                    return heightChange.ToString();

                }
                else if (calibrated[skeletonNo] == false && calibrating[skeletonNo] == false &&
                    skeletons[skeletonNo].Joints[JointType.HandLeft].Position.Y < skeletons[skeletonNo].Joints[JointType.ShoulderCenter].Position.Y)
                {
                    previousLeftHandRaised = false;
                }



                return "";
            

        }

        private Vector4 calculateCentre(Skeleton skel)
        {
            Vector4 temp = new Vector4();
            temp.X = (skel.Joints[JointType.FootLeft].Position.X + skel.Joints[JointType.FootRight].Position.X) / 2;
            temp.Y = (skel.Joints[JointType.FootLeft].Position.Y + skel.Joints[JointType.FootRight].Position.Y) / 2;
            temp.Z = (skel.Joints[JointType.FootLeft].Position.Z + skel.Joints[JointType.FootRight].Position.Z) / 2;

            return temp;
        }

        public int FindQuadrant(float Z, float X, Vector4 center)
        {
            if (Z < (center.Z + 0.8) && Z > (center.Z - 0.8) && X < (center.X + 0.8) && X > (center.X - 0.8))
            {
                if (Z > center.Z)
                {
                    if (X < center.X)
                    {
                        return 4;
                    }
                    else
                    {
                        return 3;
                    }
                }
                else
                {
                    if (X < center.X)
                    {
                        return 1;
                    }
                    else
                    {
                        return 2;
                    }
                }
            }
            else return 0;
        }
    }
}