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;

        [SerializeField]
        int countThreshold = 30;

        int home_count;
        Vector4[] position_samples = new Vector4[512 * 424 * 2];
        //Vector4[] velocity_samples = new Vector4[512 * 424 * 2];

        public Vector3 averagePositinon = Vector3.zero;
        public float standardDeviation = -1.0f;

        //public Vector3 averageVelocity = Vector3.zero;

        [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(position_samples, 0, 0, home_count);
            //kinect.HomeVelocityBuffer.GetData(velocity_samples, 0, 0, home_count);

            averagePositinon = Vector3.zero;
            //averageVelocity = Vector3.zero;
            if (home_count > countThreshold)
            {
                float div = 1.0f / home_count;
                float avg_dist = 0.0f;
                
                standardDeviation = 0.0f;
                for (int i = 0; i < home_count; i++)
                {
                    var pos_smp = new Vector3(position_samples[i].x, position_samples[i].y, position_samples[i].z);
                    averagePositinon += pos_smp;
                    float dist = new Vector2(pos_smp.x - transform.position.x, pos_smp.z - transform.position.z).magnitude;
                    avg_dist += dist;
                    standardDeviation += Mathf.Pow(dist, 2.0f);

                    //var vel_smp = new Vector3(velocity_samples[i].x, velocity_samples[i].y, velocity_samples[i].z);
                    //averageVelocity += vel_smp;
                }

                averagePositinon.Scale(Vector4.one * div);
                avg_dist *= div;
                standardDeviation = Mathf.Sqrt(standardDeviation * div - avg_dist * avg_dist);

                //averageVelocity.Scale(Vector4.one * div);

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