using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Drawing;
using System.Drawing.Imaging;
using Microsoft.Win32;
using System.Windows;

namespace AstraTestWpf
{
    public class Translate
    {
        public static string CameraParamsFileName = @"camera_params.ini";


        private float fxl, fyl, cxl, cyl;
        private float fxr, fyr, cxr, cyr;
        private float[] rot;
        private float[] t;
        private float[,] mat;
        private float[,] matC2D;

        public Translate()
        {
            while(!File.Exists(CameraParamsFileName))
            {
                MessageBox.Show("Did not find camera parameters file. Please provide the location of the file.", "Camera parameters file not found", MessageBoxButton.OK, MessageBoxImage.Warning);
                OpenFileDialog dlg = new OpenFileDialog();
                dlg.Title = "Please provide camera_params.ini file location";
                bool? b = dlg.ShowDialog();
                if (b == true)
                    CameraParamsFileName = dlg.FileName;
            }
            ReadCameraParameters();
            CalcAllMat();
            CalcAllMat();
        }


        private float[] GetValues(string s)
        {
            string[] a = s.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
            float[] values = new float[a.Length];
            for (int i = 0; i < a.Length; i++)
                values[i] = float.Parse(a[i]);
            return values;
        }
        private float[] GetValues(StreamReader sr)
        {
            string s = sr.ReadLine();
            string[] a = s.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
            float[] values = new float[a.Length];
            for (int i = 0; i < a.Length; i++)
                values[i] = float.Parse(a[i]);
            return values;
        }

        private void ReadCameraParameters()
        {
            float cx, cy, fx, fy, zero;
            float tx, ty, tz;
            StreamReader sr = new StreamReader(CameraParamsFileName);
            string sLine = "";
            float[] values = null;
            string sTitle1 = sr.ReadLine();
            values = GetValues(sr);
            fxl = values[0];
            cxl = values[2];
            values = GetValues(sr);
            fyl = values[1];
            cyl = values[2];
            values = GetValues(sr);


            string sTitle2 = sr.ReadLine();
            values = GetValues(sr);
            fxr = values[0];
            cxr = values[2];
            values = GetValues(sr);
            fyr = values[1];
            cyr = values[2];
            values = GetValues(sr);


            string sTitle3 = sr.ReadLine();
            rot = new float[9];
            values = GetValues(sr);
            rot[0] = values[0];
            rot[1] = values[1];
            rot[2] = values[2];
            values = GetValues(sr);
            rot[3] = values[0];
            rot[4] = values[1];
            rot[5] = values[2];
            values = GetValues(sr);
            rot[6] = values[0];
            rot[7] = values[1];
            rot[8] = values[2];

            string sTitle4 = sr.ReadLine();
            values = GetValues(sr);
            t = new float[3];
            t[0] = values[0];
            t[1] = values[1];
            t[2] = values[2];

        }

        private void CalcAllMat()
        {
            Matrix<double> RK = DenseMatrix.OfArray(new double[,] {
                { fxr, 0.0, cxr, 0.0 },
                { 0.0, fyr, cyr, 0.0 },
                { 0.0, 0.0, 1.0, 0.0 },
                { 0.0, 0.0, 0.0, 1.0 }
            });

            Matrix<double> R2L = DenseMatrix.OfArray(new double[,] {
                { rot[0], rot[1], rot[2], t[0] },
                { rot[3], rot[4], rot[5], t[1] },
                { rot[6], rot[7], rot[8], t[2] },
                { 0.0, 0.0, 0.0, 1.0 }
            });

            Matrix<double> LK = DenseMatrix.OfArray(new double[,] {
                { fxl, 0.0, cxl, 0.0 },
                { 0.0, fyl, cyl, 0.0 },
                { 0.0, 0.0, 1.0, 0.0 },
                { 0.0, 0.0, 0.0, 1.0 }
            });

            Matrix<double> all = RK * R2L * LK.Inverse();
            mat = new float[4, 4];
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    mat[i, j] = (float)all.At(i, j);

            Matrix<double> inverse = all.Inverse();
            matC2D = new float[4, 4];
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    matC2D[i, j] = (float)inverse.At(i, j);
        }


        public void MappingDepthToColor(int x, int y, int z, out int newX, out int newY)
        {
            //z * 8 because we use >>3 on the depth input
            newX = (int)(mat[0, 0] * x + mat[0, 1] * y + mat[0, 2] + mat[0, 3] / (z * 8));
            newY = (int)(mat[1, 0] * x + mat[1, 1] * y + mat[1, 2] + mat[1, 3] / (z * 8));
        }
        
        public BitmapSource CalibrateDepthII(BitmapSource depth)
        {
            int stride = depth.PixelWidth * 4;
            int size = depth.PixelHeight * stride;
            byte[] source = new byte[size];
            byte[] target = new byte[size];
            depth.CopyPixels(source, stride, 0);
            for (int x = 0; x < depth.Width; x++)
            {
                for(int y = 0; y < depth.Height; y++)
                {
                    int newX = 0, newY = 0;
                    int idx = y * stride + 4 * x;
                    int z = source[idx];
                    MappingDepthToColor(x, y, z, out newX, out newY);
                    if (newX >= 0 && newX < depth.Width && newY >= 0 && newY <= depth.Height)
                    {
                        int targetIdx = newY * stride + 4 * newX;
                        target[targetIdx] = (byte)z;
                        target[targetIdx  +1] = (byte)z;
                        target[targetIdx + 2] = (byte)z;
                    }
                }
            }
            BitmapImage image = new BitmapImage();
            image.BeginInit();
            image.StreamSource = new System.IO.MemoryStream(target);
            image.EndInit();

            return image;

        }

        public Bitmap CalibrateDepth(BitmapSource depth)
        {
            Bitmap bmpDepth = ToWinFormsBitmap(depth);
            Bitmap calibrated = new Bitmap(bmpDepth.Width, bmpDepth.Height);
            for (int x = 0; x < bmpDepth.Width; x++)
            {
                for (int y = 0; y < bmpDepth.Height; y++)
                {
                    calibrated.SetPixel(x, y, System.Drawing.Color.Black);
                }
            }
            for (int x = 0; x < bmpDepth.Width; x++)
            {
                for (int y = 0; y < bmpDepth.Height; y++)
                {
                    int z = bmpDepth.GetPixel(x, y).R;
                    MappingDepthToColor(x, y, z, out int newX, out int newY);
                    if (newX >= 0 && newX < depth.Width && newY >= 0 && newY <= depth.Height)
                    {
                        calibrated.SetPixel(newX, newY, bmpDepth.GetPixel(x, y));
                    }
                }
            }
            return calibrated;
        }

        public Bitmap ToWinFormsBitmap(BitmapSource bitmapsource)
        {
            using (MemoryStream stream = new MemoryStream())
            {
                BitmapEncoder enc = new BmpBitmapEncoder();
                enc.Frames.Add(BitmapFrame.Create(bitmapsource));
                enc.Save(stream);

                using (var tempBitmap = new Bitmap(stream))
                {
                    // According to MSDN, one "must keep the stream open for the lifetime of the Bitmap."
                    // So we return a copy of the new bitmap, allowing us to dispose both the bitmap and the stream.
                    return new Bitmap(tempBitmap);
                }
            }
        }
    }
}