using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class SpaceHomography : MonoBehaviour
{
    public Vector3 space;

    Matrix4x4 front_homo;
    Matrix4x4 left_homo;
    Matrix4x4 right_homo;

    Vector2 texture_size;

    private void Start()
    {
        float dim = space.x + space.z * 2.0f;
        //Vector2 canvas_size = new Vector2(space.x + space.z * 2.0f, space.y + space.z * 2.0f);
        //Vector2 clipped_size = new Vector2(space.x, 3200);
        var rt = GetComponent<Camera>().targetTexture;
        texture_size = new Vector2(rt.width, rt.height);

        float offset = 0.5f;
        var src = new List<Vector2>();
        var dst = new List<Vector2>();

        // front
        {
            src.Clear();
            src.Add(new Vector2(0, 0));
            src.Add(new Vector2(dim, 0));
            src.Add(new Vector2(space.z, space.z));
            src.Add(new Vector2(space.z + space.x, space.z));
            for (int i = 0; i < src.Count; i++)
                src[i] += Vector2.one * offset;

            dst.Clear();
            dst.Add(new Vector2(space.z, 0));
            dst.Add(new Vector2(space.z + space.x, 0));
            dst.Add(new Vector2(space.z, space.z));
            dst.Add(new Vector2(space.z + space.x, space.z));
            for (int i = 0; i < dst.Count; i++)
                dst[i] += Vector2.one * offset;

            front_homo = Matrix4x4.identity;
            FindHomography(dst.ToArray(), src.ToArray(), ref front_homo);
        }

        // left
        {
            src.Clear();
            src.Add(new Vector2(0, 0));
            src.Add(new Vector2(0, dim));
            src.Add(new Vector2(space.z, space.z));
            src.Add(new Vector2(space.z, space.z + space.x));
            for (int i = 0; i < src.Count; i++)
                src[i] += Vector2.one * offset;

            dst.Clear();
            dst.Add(new Vector2(0, space.z));
            dst.Add(new Vector2(0, space.z + space.x));
            dst.Add(new Vector2(space.z, space.z));
            dst.Add(new Vector2(space.z, space.z + space.x));
            for (int i = 0; i < dst.Count; i++)
                dst[i] += Vector2.one * offset;

            left_homo = Matrix4x4.identity;
            FindHomography(dst.ToArray(), src.ToArray(), ref left_homo);
        }

        // right
        {
            src.Clear();
            src.Add(new Vector2(dim, 0));
            src.Add(new Vector2(dim, dim));
            src.Add(new Vector2(space.z + space.x, space.z));
            src.Add(new Vector2(space.z + space.x, space.z + space.x));
            for (int i = 0; i < src.Count; i++)
                src[i] += Vector2.one * offset;

            dst.Clear();
            dst.Add(new Vector2(dim, space.z));
            dst.Add(new Vector2(dim, space.z + space.x));
            dst.Add(new Vector2(space.z + space.x, space.z));
            dst.Add(new Vector2(space.z + space.x, space.z + space.x));
            for (int i = 0; i < dst.Count; i++)
                dst[i] += Vector2.one * offset;

            right_homo = Matrix4x4.identity;
            FindHomography(dst.ToArray(), src.ToArray(), ref right_homo);
        }

        Shader.SetGlobalMatrix("front_homo", front_homo);
        Shader.SetGlobalMatrix("left_homo", left_homo);
        Shader.SetGlobalMatrix("right_homo", right_homo);
        Shader.SetGlobalVector("homo_space", space);
        Shader.SetGlobalVector("texture_size", texture_size);
    }

    private void FindHomography(Vector2[] src, Vector2[] dest, ref Matrix4x4 homography)
    {

        float[,] P = new float[,]{
        {-src[0].x, -src[0].y, -1,   0,   0,  0, src[0].x*dest[0].x, src[0].y*dest[0].x, -dest[0].x }, // h11  
        {  0,   0,  0, -src[0].x, -src[0].y, -1, src[0].x*dest[0].y, src[0].y*dest[0].y, -dest[0].y }, // h12  
          
        {-src[1].x, -src[1].y, -1,   0,   0,  0, src[1].x*dest[1].x, src[1].y*dest[1].x, -dest[1].x }, // h13  
        {  0,   0,  0, -src[1].x, -src[1].y, -1, src[1].x*dest[1].y, src[1].y*dest[1].y, -dest[1].y }, // h21  
          
        {-src[2].x, -src[2].y, -1,   0,   0,  0, src[2].x*dest[2].x, src[2].y*dest[2].x, -dest[2].x }, // h22  
        {  0,   0,  0, -src[2].x, -src[2].y, -1, src[2].x*dest[2].y, src[2].y*dest[2].y, -dest[2].y }, // h23  
          
        {-src[3].x, -src[3].y, -1,   0,   0,  0, src[3].x*dest[3].x, src[3].y*dest[3].x, -dest[3].x }, // h31  
        {  0,   0,  0, -src[3].x, -src[3].y, -1, src[3].x*dest[3].y, src[3].y*dest[3].y, -dest[3].y }, // h32  
    	};

        GaussianElimination(ref P, 9);

        float[] aux_H ={ P[0,8],P[3,8],0,P[6,8], // h11  h21 0 h31  
	                     P[1,8],P[4,8],0,P[7,8], // h12  h22 0 h32  
	                     0      ,      0,1,0,    // 0    0   0 0  
	                     P[2,8],P[5,8],0,1};     // h13  h23 0 h33  

        for (int i = 0; i < 16; i++) homography[i] = aux_H[i];

    }

    private void GaussianElimination(ref float[,] A, int n)
    {

        int i = 0;
        int j = 0;
        int m = n - 1;
        while (i < m && j < n)
        {

            int maxi = i;
            for (int k = i + 1; k < m; k++)
            {
                if (Mathf.Abs(A[k, j]) > Mathf.Abs(A[maxi, j]))
                {
                    maxi = k;
                }
            }
            if (A[maxi, j] != 0)
            {

                if (i != maxi)
                    for (int k = 0; k < n; k++)
                    {
                        float aux = A[i, k];
                        A[i, k] = A[maxi, k];
                        A[maxi, k] = aux;
                    }

                float A_ij = A[i, j];
                for (int k = 0; k < n; k++)
                {
                    A[i, k] /= A_ij;
                }

                for (int u = i + 1; u < m; u++)
                {

                    float A_uj = A[u, j];
                    for (int k = 0; k < n; k++)
                    {
                        A[u, k] -= A_uj * A[i, k];
                    }

                }

                i++;
            }
            j++;
        }
        for (int k = m - 2; k >= 0; k--)
        {
            for (int l = k + 1; l < n - 1; l++)
            {
                A[k, m] -= A[k, l] * A[l, m];

            }
        }
    }


}