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


public class PointCloudAggregator : MonoBehaviour {

    public float accuracy = 1000f;

	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;

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

	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];
		}
	}
	
	// Update is called once per frame
	void Update () {
		if (Input.GetKeyDown(KeyCode.Space))
		{
            Debug.Log("Snapshot");
			NewSnapshot();
		}

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

	}

	void NewSnapshot()
	{
		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);
			floorPosition = new Vector2Int(Mathf.RoundToInt(currentPoint.x * (int)accuracy), Mathf.RoundToInt(currentPoint.z * (int)accuracy));



            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);
    }

}