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

//namespace ... {
    class MHomography {

        private static void computeErrorOfHomography(
            List<MVector3> sourcePoints, List<MVector3> targetPoints, MMatrix4 homography,
            out float minError, out float maxError, out float averageError
        ) {
            minError = maxError = averageError = 0;
            if (sourcePoints.Count < 1)
                return;
            for ( int i = 0; i < sourcePoints.Count; ++i ) {
                MVector3 v = homography.multPoint( sourcePoints[i] );
                float error = ( v - targetPoints[i] ).magnitude();
                //MLogger.Log(string.Format("    error is {0}",error));
                if ( i == 0 ) {
                    minError = maxError = averageError = error;
                }
                else {
                    if ( error < minError ) minError = error;
                    if ( error > maxError ) maxError = error;
                    averageError += error;
                }
            }
            averageError /= sourcePoints.Count;
        }

        // Given a set of source points to transform toward target points,
        // this computes a 4x4 transformation matrix (a homography, I think)
        // that does as good a job as possible to transform from the source space
        // to the target space.
        // Algorithm adapted from
        //    David Kriegman, "Homography Estimation", 2007,
        //    https://cseweb.ucsd.edu/classes/wi07/cse252a/homography_estimation/homography_estimation.pdf
        public static MMatrix4 compute4x4TransformationMatrix(
            List<MVector3> sourcePoints,
            List<MVector3> targetPoints
        ) {
            MLogger.Assert(sourcePoints.Count == targetPoints.Count,string.Format("Homography requested for lists of mismatching length {0},{1}",sourcePoints.Count,targetPoints.Count));
            // MLogger.Log(string.Format("Homography requested for lists of length {0},{1}",sourcePoints.Count,targetPoints.Count));
            int N = sourcePoints.Count; // number of pairs of points
            if ( N < 1 || targetPoints.Count != N )
                return new MMatrix4();
            if ( N == 1 ) {
                // assume the source and target spaces have parallel frames and the same scale
                MMatrix4 M_trans = new MMatrix4();
                M_trans.setToTranslation(targetPoints[0] - sourcePoints[0]);
                return M_trans;
            }
            else if ( N == 2 ) {
                // assume the source and target spaces have the same handedness,
                // and that the "up" vector in both spaces is the y+ axis,
                // and the the scale factor along x and z is the same.
                MMatrix4 M_trans1 = new MMatrix4();
                MMatrix4 M_trans2 = new MMatrix4();
                M_trans1.setToTranslation( - sourcePoints[0]);
                M_trans2.setToTranslation(targetPoints[0]);
                float delta_x_target = targetPoints[1].x - targetPoints[0].x;
                float delta_x_source = sourcePoints[1].x - sourcePoints[0].x;
                float delta_y_target = targetPoints[1].y - targetPoints[0].y;
                float delta_y_source = sourcePoints[1].y - sourcePoints[0].y;
                float delta_z_target = targetPoints[1].z - targetPoints[0].z;
                float delta_z_source = sourcePoints[1].z - sourcePoints[0].z;
#if false
                float vertical_scale = delta_y_target / delta_y_source;
                float horizontal_scale = (float)(Math.Sqrt(delta_x_target*delta_x_target + delta_z_target*delta_z_target)
                    / Math.Sqrt(delta_x_source*delta_x_source + delta_z_source*delta_z_source));
                MMatrix4 M_scale = new MMatrix4();
                M_scale.setToScale(horizontal_scale,vertical_scale,horizontal_scale);
#else
                float scaleFactor = (float)(Math.Sqrt(delta_x_target*delta_x_target + delta_y_target*delta_y_target + delta_z_target*delta_z_target)
                    / Math.Sqrt(delta_x_source*delta_x_source + delta_y_source*delta_y_source +  delta_z_source*delta_z_source));
                MMatrix4 M_scale = new MMatrix4();
                M_scale.setToUniformScale(scaleFactor);
#endif
                float rotationAngle = MVector2.computeSignedAngle( new MVector2(delta_x_source,delta_z_source), new MVector2(delta_x_target,delta_z_target) );
                MMatrix4 M_rotate = new MMatrix4();
                M_rotate.setToRotation( - rotationAngle, new MVector3(0,1,0) );
                return M_trans2 * M_scale * M_rotate * M_trans1;
            }

            int m = 3*N;
            int n = 16;
            double[,] A = new double[m,n];
            for ( int i = 0; i < N; ++i ) {
                MVector3 p1 = sourcePoints[i];
                double x1 = p1.x;
                double y1 = p1.y;
                double z1 = p1.z;
                MVector3 p2 = targetPoints[i];
                double x2 = p2.x;
                double y2 = p2.y;
                double z2 = p2.z;
                int row = i*3;
                A[row, 0] = -x1;
                A[row, 1] = -y1;
                A[row, 2] = -z1;
                A[row, 3] = -1;
                A[row, 4] = 0;
                A[row, 5] = 0;
                A[row, 6] = 0;
                A[row, 7] = 0;
                A[row, 8] = 0;
                A[row, 9] = 0;
                A[row,10] = 0;
                A[row,11] = 0;
                A[row,12] = x2*x1;
                A[row,13] = x2*y1;
                A[row,14] = x2*z1;
                A[row,15] = x2;
                row ++;
                A[row, 0] = 0;
                A[row, 1] = 0;
                A[row, 2] = 0;
                A[row, 3] = 0;
                A[row, 4] = -x1;
                A[row, 5] = -y1;
                A[row, 6] = -z1;
                A[row, 7] = -1;
                A[row, 8] = 0;
                A[row, 9] = 0;
                A[row,10] = 0;
                A[row,11] = 0;
                A[row,12] = y2*x1;
                A[row,13] = y2*y1;
                A[row,14] = y2*z1;
                A[row,15] = y2;
                row ++;
                A[row, 0] = 0;
                A[row, 1] = 0;
                A[row, 2] = 0;
                A[row, 3] = 0;
                A[row, 4] = 0;
                A[row, 5] = 0;
                A[row, 6] = 0;
                A[row, 7] = 0;
                A[row, 8] = -x1;
                A[row, 9] = -y1;
                A[row,10] = -z1;
                A[row,11] = -1;
                A[row,12] = z2*x1;
                A[row,13] = z2*y1;
                A[row,14] = z2*z1;
                A[row,15] = z2;
            }
            double[] w = new double[Math.Min(m,n)];
            double[,] u = new double[m,m];
            double[,] vt = new double[n,n];
            alglib.rmatrixsvd(A,m,n,2,2,2,out w,out u,out vt); // SVD

            // Extract the bottom row of vt (which is the right-most column of v)
            // and store its values in the matrix that we return
            MMatrix4 M1 = new MMatrix4();
            int last = Math.Min(m-1,n-1);
            for ( int i = 0; i < 4; ++i ) {
                for ( int j = 0; j < 4; ++j ) {
                    M1.m[i+j*4] = (float)vt[last,i*4+j];
                }
            }

            // I've sometimes had a problem where the matrix that is computed here will transform points correctly,
            // but when I use the matrix to transform a vector, I get the negative of what I expect.
            // When this happens, in the limited cases that I've tested, I've observed that all the non-zero entries
            // in the matrix are the opposite sign of what I expect.
            // Although I don't fully understand the problem, I will try the below as a simple hack to fix it.
            if ( M1.m[15] < 0 ) { // ... negate all entries
                //MLogger.Log("Flipping the matrix!");
                for ( int i = 0; i <= 15; ++i )
                    M1.m[i] = - M1.m[i];
            }

            float M1_minError, M1_maxError, M1_averageError;
            computeErrorOfHomography(sourcePoints,targetPoints,M1,out M1_minError, out M1_maxError, out M1_averageError);

#if true
            return M1;
#else

            // I have tested the below approach to computing the homography, when using 8-point calibration,
            // and it tends to result in errors that are still small but about 10 times greater than with the
            // above approach of using SVD.
            // With 9-point calibration, where one point is in the center,
            // the below approach can result in division be zero, because the distance from the 9th point
            // to the center of the points is zero.


            // Compute an alternative homography using only tranlation, rotation, and scaling,
            // assuming that y+ is up in both coordinate systems.
            MMatrix4 M2 = new MMatrix4();
            float M2_minError, M2_maxError, M2_averageError;
            {
                MVector3 sourceCenter = MVector3.average(sourcePoints);
                MVector3 targetCenter = MVector3.average(targetPoints);
                MMatrix4 M_trans1 = new MMatrix4();
                MMatrix4 M_trans2 = new MMatrix4();
                M_trans1.setToTranslation( - sourceCenter);
                M_trans2.setToTranslation(targetCenter);
                float averageRotationAngle = 0;
                float averageHorizontalScale = 0;
                float averageVerticalScale = 0;
                for ( int i = 0; i < sourcePoints.Count; ++i ) {
                    float x_target = targetPoints[i].x - targetCenter.x;
                    float x_source = sourcePoints[i].x - sourceCenter.x;
                    float y_target = targetPoints[i].y - targetCenter.y;
                    float y_source = sourcePoints[i].y - sourceCenter.y;
                    float z_target = targetPoints[i].z - targetCenter.z;
                    float z_source = sourcePoints[i].z - sourceCenter.z;

                    float rotationAngle = MVector2.computeSignedAngle( new MVector2(x_source,z_source), new MVector2(x_target,z_target) );
                    // make sure all rotations are in the same direction, i.e. all angles are positive
                    if ( rotationAngle < 0 )
                        rotationAngle = (float)( 2 * Math.PI + rotationAngle );
                    averageRotationAngle += rotationAngle;

                    float verticalScale = y_target / y_source;
                    float horizontalScale = (float)(Math.Sqrt(x_target*x_target + z_target*z_target)
                        / Math.Sqrt(x_source*x_source + z_source*z_source));
                    averageHorizontalScale += horizontalScale;
                    averageVerticalScale += verticalScale;
                }
                averageRotationAngle /= sourcePoints.Count;
                averageHorizontalScale /= sourcePoints.Count;
                averageVerticalScale /= sourcePoints.Count;
                MMatrix4 M_rotate = new MMatrix4();
                M_rotate.setToRotation( - averageRotationAngle, new MVector3(0,1,0) );
                MMatrix4 M_scale = new MMatrix4();
                M_scale.setToScale(averageHorizontalScale,averageVerticalScale,averageHorizontalScale);

                M2 = M_trans2 * M_scale * M_rotate * M_trans1;
            }
            computeErrorOfHomography(sourcePoints,targetPoints,M2,out M2_minError, out M2_maxError, out M2_averageError);

            MLogger.Log(string.Format(
                "Errors in Homography from SVD: {0}, {1}, {2}\nErrors in Homography from TRS: {3}, {4}, {5}\n",
                M1_minError, M1_maxError, M1_averageError,
                M2_minError, M2_maxError, M2_averageError
            ));

            // Return whichever homography yields the smaller error
            return M1_averageError < M2_averageError ? M1 : M2;

#endif
        }


    }
//}

