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


public class PointCloudAggregator : MonoBehaviour {

	public float accuracy = 1000f;
	public float maxDistance = 15f;

	public ParticleSystem _particleSystem;

	private ParticleSystem.Particle[] particles;

	// Kinectsensor foo
	private MultiSourceManager _MultiManager;
	private KinectSensor _Sensor;
	private CoordinateMapper _Mapper;
	private int depthWidth;
	private int depthHeight;

	private FrameDescription depthFrameDesc;

	private CameraSpacePoint[] cameraSpacePoints;

	private ColorSpacePoint[] colorSpacePoints;
	private Texture2D cameraTexture;
	private Color[] cameraColor;

	private Vector2 maxVals;
	private Vector2 minVals;

	Dictionary<Vector2Int, ColorHeight> data = new Dictionary<Vector2Int, ColorHeight>();

	Dictionary<Vector3, Color> data2 = new Dictionary<Vector3, Color>();

	public struct ColorHeight
	{
		public float height;
		public Color color;
		float avgAbout;

		public ColorHeight(float inHeight, Color inColor)
		{
			height = inHeight;
			color = inColor;
			avgAbout = 1;

		}

		public void UpdateInfo(float newHeight, Color newColor)
		{
			color = (color * avgAbout / (avgAbout + 1)) + newColor / avgAbout;
			height = (height * avgAbout / (avgAbout + 1)) + newHeight / avgAbout;
			avgAbout++;
		}
	}



	void Start()
	{
		_Sensor = KinectSensor.GetDefault();
		if (_Sensor != null)
		{
			_MultiManager = GetComponent<MultiSourceManager>();
			_Mapper = _Sensor.CoordinateMapper;
			
			depthFrameDesc = _Sensor.DepthFrameSource.FrameDescription;
			depthWidth = depthFrameDesc.Width;
			depthHeight = depthFrameDesc.Height;

			if (!_Sensor.IsOpen)
			{
				_Sensor.Open();
			}


			cameraSpacePoints = new CameraSpacePoint[depthWidth * depthHeight];

			colorSpacePoints = new ColorSpacePoint[depthWidth * depthHeight];

			cameraColor = new Color[depthWidth * depthHeight];
		}
		maxVals = Vector2.negativeInfinity;
		minVals = Vector2.positiveInfinity;
	}
	
	// Update is called once per frame
	void Update () {
		if (Input.GetKeyDown(KeyCode.Space))
		{
			Debug.Log("Snapshot");
			NewSnapshot(true);
			Debug.Log("Data set not has: " + data.Count + " entries");
		}
        if (Input.GetKeyDown(KeyCode.Y))
        {
            Debug.Log("Snapshot");
            NewSnapshot(false);
            Debug.Log("Data set not has: " + data.Count + " entries");
        }

        if (Input.GetKeyDown(KeyCode.C))
		{
			Debug.Log("Cloud!");
			Debug.Log(data.Count);
			ShowAsPointCloud();
		}

        if (Input.GetKeyDown(KeyCode.A))
        {
            Debug.Log("Cloud2!");
            Debug.Log(data.Count);
            ShowAsPointCloud2();
        }
    }

	void NewSnapshot(bool normal)
	{
		Vector3 currentPoint;
		Vector2Int floorPosition;
		ushort[] rawdata = _MultiManager.GetDepthData();

		cameraTexture = _MultiManager.GetColorTexture();
		_Mapper.MapDepthFrameToCameraSpace(rawdata, cameraSpacePoints);
		_Mapper.MapDepthFrameToColorSpace(rawdata, colorSpacePoints);
		for(int i = 0; i < cameraSpacePoints.Length; i++)
		{
			
			currentPoint = KinectPointToVector3(cameraSpacePoints[i]);
			currentPoint = transform.InverseTransformPoint(currentPoint);

			float x = (Mathf.Clamp(currentPoint.x, -maxDistance, maxDistance ));
			float y = (Mathf.Clamp(currentPoint.z, -maxDistance, maxDistance ));

			

			

			x *= accuracy;
			y *= accuracy;

			if(float.IsNaN(x) || float.IsNaN(y))
			{
				continue;
			}
            
			floorPosition = new Vector2Int(Mathf.RoundToInt(x),Mathf.RoundToInt(y));

			minVals = Vector2.Min(floorPosition, minVals);
			maxVals = Vector2.Max(floorPosition, maxVals);
            if (!normal)
            {
                data2[currentPoint] = GetColorAtIndex(i);
            }
            else
            {

                if (data.ContainsKey(floorPosition))
                {
                    ColorHeight values = data[floorPosition];
                    values.UpdateInfo(currentPoint.y, GetColorAtIndex(i));
                    data[floorPosition] = values;
                }
                else
                {

                    data[floorPosition] = new ColorHeight(currentPoint.y, GetColorAtIndex(i));
                }
            }
		}

	}

	Vector3 KinectPointToVector3(CameraSpacePoint point)
	{
		return new Vector3(point.X, point.Y, point.Z);
	}

	Color GetColorAtIndex(int index)
	{
		Color result = cameraTexture.GetPixel(Mathf.RoundToInt(colorSpacePoints[index].X), Mathf.RoundToInt(colorSpacePoints[index].Y));
		return result;
	}

	void ShowAsPointCloud()
	{
		ColorHeight colorHeight;
		particles = new ParticleSystem.Particle[data.Count];    
		
		int i = 0;
		foreach(Vector2Int floorPos in data.Keys)
		{
			colorHeight = data[floorPos];

			particles[i].position = new Vector3((float)floorPos.x/accuracy, (float)colorHeight.height, (float)floorPos.y/ accuracy);
			particles[i].startColor = colorHeight.color;
			particles[i].startSize = 0.02f;
			i++;
		}
		_particleSystem.SetParticles(particles, particles.Length);
	}


    void ShowAsPointCloud2()
    {
        particles = new ParticleSystem.Particle[data2.Count];

        int i = 0;
        foreach (Vector3 pos in data2.Keys)
        {
            particles[i].position = pos;
            particles[i].startColor = data2[pos];
            particles[i].startSize = 0.02f;
            i++;
        }
        _particleSystem.SetParticles(particles, particles.Length);
    }

}