using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace UltraCombos
{
    public class KinectOpticalFlowMath : MonoBehaviour
    {
        [SerializeField]
        KinectOpticalFlow kinect;

        [SerializeField]
        int fps = 15;
        float step;
        float stamp = 0.0f;

        int home_count;
        Vector4[] samples = new Vector4[512 * 424];
        
        public Vector3 averagePositinon = Vector3.zero;
        public float triggerRadius = 1.5f;
        public float standardDeviation = -1.0f;

        [SerializeField]
        string debug;
        public string DebugString { get { return debug; } }

        private void Start()
        {
            step = 1.0f / fps;
        }

        private void Update()
        {
            if (Time.time - stamp < step)
                return;
            stamp = Time.time;

            var data = new int[kinect.HomeCountBuffer.count];
            kinect.HomeCountBuffer.GetData(data);
            home_count = data[0];
            kinect.HomePositionBuffer.GetData(samples, 0, 0, home_count);

            if (home_count > 0)
            {
                float div = 1.0f / home_count;
                float avg_dist = 0.0f;
                averagePositinon = Vector3.zero;
                standardDeviation = 0.0f;
                for (int i = 0; i < home_count; i++)
                {
                    var smp = new Vector3(samples[i].x, samples[i].y, samples[i].z);
                    averagePositinon += smp;
                    float dist = new Vector2(smp.x - transform.position.x, smp.z - transform.position.z).magnitude;
                    avg_dist += dist;
                    standardDeviation += Mathf.Pow(dist, 2.0f);
                }
                averagePositinon.Scale(Vector4.one * div);
                avg_dist *= div;
                standardDeviation = Mathf.Sqrt(standardDeviation * div - avg_dist * avg_dist);

                debug = string.Format("{0}({1}): {2}", averagePositinon, avg_dist, standardDeviation);
            }
            else
            {
                standardDeviation = -1.0f;
                debug = "null";
            }
        }

        private void OnDrawGizmosSelected()
        {
            Gizmos.DrawWireSphere(averagePositinon, 0.5f);
            if (new Vector2(averagePositinon.x - transform.position.x, averagePositinon.z - transform.position.z).magnitude < triggerRadius)
            {
                Gizmos.color = Color.red;
            }
            Gizmos.DrawWireSphere(transform.position, triggerRadius);

            Gizmos.color = Color.white;
        }
    }
}