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

public class SimpleCalibrator : MonoBehaviour {

    private static Dictionary<ulong, SkeletonData> _Skeletons = new Dictionary<ulong, SkeletonData>();
    public static Dictionary<ulong, SkeletonData> Skeletons { get{ return _Skeletons ; } }

    public SimpleVisualizer visualizor;

    [SerializeField]
    private BodySourceManager BodyManager;

    [SerializeField]
    private GameObject visualizer_root;

    public bool enableBones;

    public static bool EnableBones;
    void Update()
    {
        EnableBones = enableBones;
        if (Input.GetKeyDown(KeyCode.Escape))
        {
            BodyManager = null;
        }

        if (BodyManager == null)
        {
            return;
        }

        Kinect.Body[] data = BodyManager.GetData();
        if (data == null)
        {
            return;
        }

        List<ulong> trackedIds = new List<ulong>();
        foreach (var body in data)
        {
            if (body == null)
            {
                continue;
            }

            if (body.IsTracked)
            {
                trackedIds.Add(body.TrackingId);
            }
        }

        List<ulong> knownIds = new List<ulong>(_Skeletons.Keys);

        // First delete untracked bodies
        foreach (ulong trackingId in knownIds)
        {
            if (!trackedIds.Contains(trackingId))
            {
                _Skeletons[trackingId].isDead = true;
                _Skeletons.Remove(trackingId);
            }
        }

        foreach (var body in data)
        {
            if (body == null)
            {
                continue;
            }

            if (body.IsTracked)
            {
                if (!_Skeletons.ContainsKey(body.TrackingId))
                {
                    _Skeletons[body.TrackingId] = new SkeletonData();

                    //TODO spawn new SimpleVisualizer into visualizer_root
                    for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
                    {
                        _Skeletons[body.TrackingId].joints.Add(jt, new SkeletonData.JointData());
                    }

                    GameObject obj = Instantiate(visualizor.gameObject , visualizer_root.transform) as GameObject;
                    obj.name = "Body:" + body.TrackingId;
                    obj.GetComponent<SimpleVisualizer>().skeletonData = _Skeletons[body.TrackingId];
                }

                RefreshBodyObject(body, _Skeletons[body.TrackingId]);
            }
        }
    }

    private void RefreshBodyObject(Kinect.Body body, SkeletonData bodyObject)
    {
        //TODO
        
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];

            Kinect.JointOrientation oo = body.JointOrientations[jt];
            
            bodyObject.joints[jt].position = (transform.rotation * GetVector3FromJoint(sourceJoint) + transform.position);

            bodyObject.joints[jt].rotation = transform.rotation * new Quaternion(oo.Orientation.X, oo.Orientation.Y, oo.Orientation.Z, oo.Orientation.W);

        }
    }

    private static Vector3 GetVector3FromJoint(Kinect.Joint joint)
    {
        return new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
    }
}