using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using System.Drawing;

using Emgu.CV;
using Emgu.CV.Structure;

using bbiwarg.Utility;
using bbiwarg.Detectors.Touch;

namespace bbiwarg.Graphics
{
    class KalmanDemo
    {
        private List<Point> points, kalmanPoints;
        private Kalman2DPositionFilter kalman;

        public KalmanDemo()
        {
            points = new List<Point>();
            kalmanPoints = new List<Point>();
        }

        public void addPoint(Point p)
        {
            if (points.Count == 0)
                kalman = new Kalman2DPositionFilter(new Vector2D(p));

            Vector2D v = kalman.getCorrectedPosition(new Vector2D(p));

            points.Add(p);
            kalmanPoints.Add(new Point(v.IntX, v.IntY));
        }

        public void reset()
        {
            points.Clear();
            kalmanPoints.Clear();
        }

        public Int16[] getTextureData(int width, int height)
        {
            Image<Bgr, short> image = new Image<Bgr, short>(width, height);
            image.DrawPolyline(points.ToArray(), false, new Bgr(0, Int16.MaxValue, 0), 1);
            image.DrawPolyline(kalmanPoints.ToArray(), false, new Bgr(0, 0, Int16.MaxValue), 1);

            Int16[] textureData = new Int16[3 * width * height];
            int index = 0;
            for (int y = 0; y < height; ++y) {
                for (int x = 0; x < width; ++x) {
                    textureData[index + 0] = image.Data[y, x, 2];
                    textureData[index + 1] = image.Data[y, x, 1];
                    textureData[index + 2] = image.Data[y, x, 0];

                    index += 3;
                }
            }

            return textureData;
        }
    }
}