using System.Collections.Generic;
using System;
using UnityEngine;
using System.Xml;
using System.Linq;

//source: https://numerics.mathdotnet.com/
using MathNet.Numerics.LinearAlgebra;
//NuGet in Unity editor source: https://github.com/GlitchEnzo/NuGetForUnity

// source: https://github.com/SrejonKhan/AnotherFileBrowser
using AnotherFileBrowser.Windows;


public class AlignModels : MonoBehaviour
{
    #region class

    // <summary> save all needed datas </summary>
    public class CollidorData
    {
        /// <summary> distance of resulting normal </summary>
        public float distance;
        /// <summary> reflectivity difference </summary>
        public float refDiff;
        /// <summary> reflectivity width difference </summary>
        public float refWidthDiff;
        /// <summary> temperature difference </summary>
        public float tempDiff;
        //convertion to Color32 - one color is saved as 4B
    }

    #endregion

    #region data mambers

    /// <summary> Get data from both models </summary>
    [SerializeField] private RoscanModelUnity model1, model2;
    /// <summary> reference to ICP - point to plane </summary>
    [SerializeField] private ICP5 objekt;
    /// <summary> Reference to mirror cube for ray casting </summary>
    [SerializeField] private GameObject cube;

    /// <summary> Save vertices in bounds of mesh (secound model) </summary>
    private List<Vector3> boundVertices = new List<Vector3>();
    /// <summary> Save indices of vertices in bounds of mesh (secound model) </summary>
    private List<int> boundIndices = new List<int>();
    /// <summary> Save weights of vertices in region </summary>
    private List<float> weights = new List<float>();
    /// <summary> Calculated normals between closest point - plane </summary>
    public Vector3[] newNormals;
    /// <summary> save all thermal diffrences between nearest points </summary>
    private List<float> differences = new List<float>();
    /// <summary> save all new datas from alignment </summary>
    private List<CollidorData> points = new List<CollidorData>();
    /// <summary> path to export new model </summary>
    private string path;

    /// <summary> Storing new Points </summary>
    public Matrix<double> changeRealPoints;
    /// <summary> Storing new Points - temporery </summary>
    public Matrix<double> changeRealPointsT;
    /// <summary> Metrix of mean coordinates of model 1 </summary>
    Matrix<float> meanA = Matrix<float>.Build.Dense(3, 1);
    /// <summary> Metrix of mean coordinates of model 2 </summary>
    Matrix<float> meanB = Matrix<float>.Build.Dense(3, 1);
    /// <summary> Storing new Points - temporery </summary>
    private Matrix<double> newPoints;
    /// <summary> Created matrices for compute </summary>
    Matrix<float> matrixPointsA, matrixPointsB;

    /// <summary> Manual or Automatic alignment </summary>
    public bool AlignCondition = true;
    /// <summary> Average temperature feature condition </summary>
    public bool averageCondition = false;
    /// <summary> Gradient temperature feature condition </summary>
    public bool gradientCondition = false;

    #endregion

    #region public methods

    #region User Interface
    /// <summary> Button Align was clicked and choose the algorithm </summary>
    public void Buttonclicked()
    {
        if (AlignCondition)
        {
            ICPKD(); //automatic
        }
        else
            CalculateNewPoints();  //manual
        
        // update changes on meshes
        ChangeColorModels();
        SetVertices();
        model2.EnableModel();
        model2.CalculateNormals(model2.points, model2.indices);
        model2.MeshUpdate();
        model1.MeshUpdate();
        model1.PointReset2();
        model2.PointReset2();
        model1.CreateLines();
        model2.CreateLines();
    }

    /// <summary> Creating model by comparing using rays </summary>
    public void CreateModel()
    {
        // hide visuals on models - make troubles (Unity bug)
        model1.hideVisuals();
        model2.hideVisuals();

        // ask user for path to export model
        getExportPath();     

        // checking the path
        if (path == null)
        {
            Debug.Log("Path is incorrect");
            return;
        }
        
        // MeshFilter reducing some vertices after setting buffers -> updating for correct answers
        model1.GetComponent<MeshFilter>().mesh.vertices = model1.vertices;
        model1.GetComponent<MeshFilter>().mesh.triangles = model1.indices;
        model1.GetComponent<MeshFilter>().mesh.normals = model1.origNormal;
        model2.GetComponent<MeshFilter>().mesh.vertices = model2.vertices;
        model2.GetComponent<MeshFilter>().mesh.triangles = model2.indices;
        model2.GetComponent<MeshFilter>().mesh.normals = model2.origNormal;

        // set normals to change collidor
        changeCube();
        // Filtration of temperature if was chosen
        if(averageCondition)
            AverageTemp();
        if(gradientCondition)
            GradientTemp();       
        // comparison of models
        buildRays();
        // export data to XML      
        exportData();
        // set back normals
        model2.GetComponent<MeshFilter>().mesh.normals = model2.origNormal;
    }

    #endregion

    #endregion

    #region Private methods

    #region Comparison

    #region build inner collidor
    // <summary> Change normals of cube to inverse its collidor </summary>
    private void changeCube()
    {
        Destroy(cube.GetComponent<Collider>());

        Mesh mesh = cube.GetComponent<MeshFilter>().mesh;

        int[] tempP = new int[mesh.triangles.Length];

        for (int i = 0; i < mesh.triangles.Length; ++i)
        {
            tempP[i] = mesh.triangles[mesh.triangles.Length - 1 - i];
        }

        mesh.triangles = tempP;

        cube.GetComponent<MeshFilter>().mesh = mesh;

        cube.AddComponent<MeshCollider>();
    }
    #endregion

    #region comparison of models
    // <summary> Build Ray system in cube </summary>
    private void buildRays()
    {
        // changable
        Mesh model2Mesh = model2.gameObject.GetComponent<MeshFilter>().mesh;

        // set normals methods
        model2Mesh.RecalculateNormals();
        //model2Mesh.normals = CentralNormals(model2Mesh.normals);
        //model2Mesh.normals = CentralAverageNormals(model2Mesh.normals);
        //model2Mesh.normals = PlaneNormals(model2Mesh.normals);
        //model2Mesh.normals = CentersNormals(model2Mesh.normals);
        //model2Mesh.normals = BoundCenterNormals(model2Mesh.normals);

        // reset collider
        Destroy(model2.gameObject.GetComponent<MeshCollider>());
        model2.gameObject.AddComponent<MeshCollider>();
        model2.GetComponent<MeshCollider>().sharedMesh = model2Mesh;
        
        //rays
        RaycastHit[] hits;

        for (int i = 0; i < model2Mesh.normals.Length; ++i)
        {
            Ray ray = new Ray(model2Mesh.vertices[i], model2Mesh.normals[i]);

            hits = Physics.RaycastAll(ray);

            if (WasHit(hits)) // hits ref model
            {
                addPointDataBary(model2Mesh, i, hits);
            }
            else if (hits.Length != 0)// always hits cube
            {
                int indexCube = 0;

                for (int m = 0; m < hits.Length; ++m)
                {
                    if (hits[m].transform.name == "Cube")
                        indexCube = m;
                }

                Ray ray2 = new Ray(hits[indexCube].point, Vector3.Reflect(ray.direction, ray.direction)); // creating new ray (from point of reflection and with 0 angle)

                RaycastHit[] hits2;
                hits2 = Physics.RaycastAll(ray2);

                if (WasHit(hits2)) // check if user clicked on the model
                {
                    addPointDataBary(model2Mesh, i, hits2);
                }
                else
                {
                    int indexCube2 = 0;

                    for (int m = 0; m < hits2.Length; ++m)
                    {
                        if (hits2[m].transform.name == "Cube")
                            indexCube2 = m;
                    }

                    Ray ray3 = new Ray(hits2[indexCube2].point, Vector3.Reflect(ray2.direction, ray2.direction)); // creating new ray (from point of reflection and with 0 angle)

                    RaycastHit[] hits3;
                    hits3 = Physics.RaycastAll(ray3);

                    if(WasHit(hits3))
                    {
                        addPointDataBary(model2Mesh, i, hits3);
                    }
                    else
                    {
                        addSamePointData(i);
                    }
                }
            }
            else
            {
                addSamePointData(i);
            }
        }
    }

    // <summary> Save data to List </summary>
    private void addPointDataBary(Mesh model2Mesh, int index, RaycastHit[] hits)
    {
        CollidorData b = new CollidorData();

        for (int j = 0; j < hits.Length; ++j)
        {
            if (hits[j].transform.name == "Mesh Generator") //name of ref. model
            {
                //barycentric coordination
                Vector3 baryCenter = hits[j].barycentricCoordinate;

                // calculate datas:
                // 1) reflectivity:
                b.refDiff = getDiff(hits[j], baryCenter, index, RoscanModelUnity.ModelLayers.REFLECTIVITY);

                // 2) get refl. width:
                b.refWidthDiff = getDiff(hits[j], baryCenter, index, RoscanModelUnity.ModelLayers.REFL_WIDTH);

                // 3) get temperature:
                b.tempDiff = getDiff(hits[j], baryCenter, index, RoscanModelUnity.ModelLayers.TEMPERATURE);

                // 4) direction:
                b.distance = getDiff(hits[j], baryCenter, index, RoscanModelUnity.ModelLayers.DISTANCE);

                 points.Add(b);
            }
        }
    }

    // <summary> calculate diff of the point </summary>
    private float getDiff(RaycastHit hits, Vector3 baryCenter, int index, RoscanModelUnity.ModelLayers layer)
    {
        //inicialization
        float v1 = 0;
        float v2 = 0;
        float v3 = 0;
        float diff = 0;
        float interpolatedValue = 0;
        Vector3 interpolatedValueNormal = new Vector3(0, 0, 0);
        Vector3 n1 = new Vector3(0, 0, 0);
        Vector3 n2 = new Vector3(0, 0, 0);
        Vector3 n3 = new Vector3(0, 0, 0);

        //update models
        Mesh model1Mesh = model1.GetComponent<MeshFilter>().mesh;
        int[] tempTriangles = model1Mesh.triangles;

        switch (layer)
        {
            case RoscanModelUnity.ModelLayers.REFLECTIVITY:
                // get values of triangle
                v1 = model1.points[tempTriangles[hits.triangleIndex * 3]].Reflectivity;
                v2 = model1.points[tempTriangles[hits.triangleIndex * 3 + 1]].Reflectivity;
                v3 = model1.points[tempTriangles[hits.triangleIndex * 3 + 2]].Reflectivity;
            break;

            case RoscanModelUnity.ModelLayers.REFL_WIDTH:
                v1 = model1.points[tempTriangles[hits.triangleIndex * 3]].ReflWidth;
                v2 = model1.points[tempTriangles[hits.triangleIndex * 3 + 1]].ReflWidth;
                v3 = model1.points[tempTriangles[hits.triangleIndex * 3 + 2]].ReflWidth;
                break;

            case RoscanModelUnity.ModelLayers.TEMPERATURE:
                v1 = model1.points[tempTriangles[hits.triangleIndex * 3]].Temperature;
                v2 = model1.points[tempTriangles[hits.triangleIndex * 3 + 1]].Temperature;
                v3 = model1.points[tempTriangles[hits.triangleIndex * 3 + 2]].Temperature;
                break;

            case RoscanModelUnity.ModelLayers.DISTANCE:
                n1 = model1Mesh.normals[tempTriangles[hits.triangleIndex * 3]];
                n2 = model1Mesh.normals[tempTriangles[hits.triangleIndex * 3 + 1]];
                n3 = model1Mesh.normals[tempTriangles[hits.triangleIndex * 3 + 2]];
                break;

            default:
                v1 = model1.points[tempTriangles[hits.triangleIndex * 3]].Reflectivity;
                v2 = model1.points[tempTriangles[hits.triangleIndex * 3 + 1]].Reflectivity;
                v3 = model1.points[tempTriangles[hits.triangleIndex * 3 + 2]].Reflectivity;
                break;
        }


        if(layer == RoscanModelUnity.ModelLayers.DISTANCE)
            interpolatedValueNormal = n1 * baryCenter.x + n2 * baryCenter.y + n3 * baryCenter.z;
        else
            // calculate interpolated color of point
            interpolatedValue = v1 * baryCenter.x + v2 * baryCenter.y + v3 *  baryCenter.z;

        switch (layer)
        {                
            // calculate diff
            case RoscanModelUnity.ModelLayers.REFLECTIVITY: diff = model2.points[index].Reflectivity - interpolatedValue; break;
            case RoscanModelUnity.ModelLayers.REFL_WIDTH: diff = model2.points[index].ReflWidth - interpolatedValue; break;
            case RoscanModelUnity.ModelLayers.TEMPERATURE: diff = model2.points[index].Temperature - interpolatedValue; break;
            case RoscanModelUnity.ModelLayers.DISTANCE:
                Vector3 tempNormal = model2.points[index].Vertex.Position - hits.point;
                float magnitude = tempNormal.magnitude;
                float angle = Vector3.Angle(interpolatedValueNormal, tempNormal / magnitude);
                if (angle <= 90 && angle >= -90)
                    diff = magnitude;
                else
                    diff = -magnitude;
            break;
            
            default: diff = model2.points[index].Reflectivity - interpolatedValue; break;
        }

        return diff;
    }

    // <summary> previous Point data - no hit </summary>
    private void addSamePointData(int index)
    {
        CollidorData b = new CollidorData();
        
        // direction:
        b.distance = float.NaN;

        // reflectivity:
        b.refDiff = getPreviousColor(index, RoscanModelUnity.ModelLayers.REFLECTIVITY);

        // ref. width:
        b.refWidthDiff = getPreviousColor(index, RoscanModelUnity.ModelLayers.REFL_WIDTH);

        //temperature:
        b.tempDiff = getPreviousColor(index, RoscanModelUnity.ModelLayers.TEMPERATURE);

        points.Add(b);
    }

    // <summary> Return NAN </summary>
    private float getPreviousColor(int index, RoscanModelUnity.ModelLayers layer)
    {
        // get points
        switch (layer)
        {
            case RoscanModelUnity.ModelLayers.REFLECTIVITY:
                return float.NaN;
            case RoscanModelUnity.ModelLayers.REFL_WIDTH:
                return float.NaN;
            case RoscanModelUnity.ModelLayers.TEMPERATURE:
                return float.NaN;
            default:
                return float.NaN;
        }
    }


    // <summary> Detect if ref. model was hit </summary>
    private bool WasHit(RaycastHit[] hits)
    {
        bool temp = false;

        foreach(var i in hits)
        {
            if (i.transform.name == "Mesh Generator")
                temp = true;
        }

        return temp;
    }

    /// <summary> Create normals of every vertices with direction to the model center </summary>
    private Vector3[] CentralNormals(Vector3[] recalculateNormals)
    {
        Vector3[] newNormals = new Vector3[recalculateNormals.Length];

        Vector3 modelCenter = new Vector3(model2.ModelCenter[0], model2.ModelCenter[1], model2.ModelCenter[2]);

        for(int i = 0; i < recalculateNormals.Length; ++i)
        {
            newNormals[i] = modelCenter - model2.points[i].Vertex.Position;
            newNormals[i] /= Vector3.Magnitude(newNormals[i]);
            float angle = Vector3.Angle(newNormals[i], recalculateNormals[i]);

            if (angle >= 90 || angle <= -90)
            {
                newNormals[i] = newNormals[i] * (-1);
            }
        }

        return newNormals;
    }

    /// <summary> Create normals of every vertices with direction to the average model center </summary>
    private Vector3[] CentralAverageNormals(Vector3[] recalculateNormals)
    {
        Vector3[] newNormals = new Vector3[recalculateNormals.Length];

        Vector3 modelCenter = new Vector3(0, 0, 0);

        for(int j = 0; j < model2.points.Count; ++j)
        {
            modelCenter += model2.points[j].Vertex.Position; 
        }

        modelCenter /= model2.points.Count;

        for (int i = 0; i < recalculateNormals.Length; ++i)
        {
            newNormals[i] = modelCenter - model2.points[i].Vertex.Position;
            newNormals[i] /= Vector3.Magnitude(newNormals[i]);
            float angle = Vector3.Angle(newNormals[i], recalculateNormals[i]);

            if (angle >= 90 || angle <= -90)
            {
                newNormals[i] = newNormals[i] * (-1);
            }
        }

        return newNormals;
    }

    /// <summary> Create normals of every vertices with direction to the plane </summary>
    private Vector3[] PlaneNormals(Vector3[] recalculateNormals)
    {
        Vector3[] newNormals = new Vector3[recalculateNormals.Length];

        for (int i = 0; i < recalculateNormals.Length; ++i)
        {
            newNormals[i] = (new Vector3(model2.points[i].Vertex.Position.x, model2.points[i].Vertex.Position.y, 0)) - model2.points[i].Vertex.Position;
            newNormals[i] /= Vector3.Magnitude(newNormals[i]);
            float angle = Vector3.Angle(newNormals[i], recalculateNormals[i]);

            if (angle >= 90 || angle <= -90)
            {
                newNormals[i] = newNormals[i] * (-1);
            }
        }

        return newNormals;
    }

    /// <summary> Create normals which will have direction same as normal between modelcenters </summary>
    private Vector3[] CentersNormals(Vector3[] normals)
    {
        Vector3[] newNormals = new Vector3[normals.Length];

        //get normal between modelcenters
        Vector3 model1Center = new Vector3(0, 0, 0);
        Vector3 model2Center = new Vector3(0, 0, 0);

        foreach (var i in model1.points)
            model1Center += i.Vertex.Position;

        foreach (var i in model2.points)
            model2Center += i.Vertex.Position;

        model1Center /= model1.points.Count;
        model2Center /= model2.points.Count;

        Vector3 normal = model2Center - model1Center;
        normal /= normal.magnitude;

        // set normals
        for (int i = 0; i < newNormals.Length; ++i)
        {
            newNormals[i] = normal;
            float angle = Vector3.Angle(newNormals[i], normals[i]);

            if (angle >= 90 || angle <= -90)
            {
                newNormals[i] = newNormals[i] * (-1);
            }
        }

        return newNormals;
    }

    /// <summary> Create normals which will have direction same as vector between modelcenters - vertices bounded </summary>
    private Vector3[] BoundCenterNormals(Vector3[] normals)
    {
        Vector3[] newNormals = new Vector3[normals.Length];

        //get normal between modelcenters
        Vector3 model1Center = new Vector3(0, 0, 0);
        Vector3 model2Center = new Vector3(0, 0, 0);

        // user didnt used any option of features for temperature
        if (boundVertices.Count == 0)
        {
            if (model1.vertices.Length >= model2.vertices.Length)
            {
                // get mesh bound of smaller model
                Bounds bound = model2.GetComponent<MeshFilter>().mesh.bounds;
                GetVerticeBounds(bound, model1.vertices);

                foreach (var i in boundVertices)
                    model1Center += i;

                foreach (var i in model2.points)
                    model2Center += i.Vertex.Position;

                model1Center /= boundVertices.Count;
                model2Center /= model2.points.Count;
            }
            else
            {
                Bounds bound = model1.GetComponent<MeshFilter>().mesh.bounds;
                GetVerticeBounds(bound, model2.vertices);

                foreach (var i in model1.points)
                    model1Center += i.Vertex.Position;

                foreach (var i in boundVertices)
                    model2Center += i;

                model1Center /= model1.points.Count;
                model2Center /= boundVertices.Count;
            }
        }

        Vector3 normal = model2Center - model1Center;
        normal /= normal.magnitude;

        // set normals
        for (int i = 0; i < newNormals.Length; ++i)
        {
            newNormals[i] = normal;
            float angle = Vector3.Angle(newNormals[i], normals[i]);

            if (angle >= 90)
            {
                newNormals[i] = newNormals[i] * (-1);
            }
        }

        return newNormals;
    }

    #endregion

    #region export model

    /// <summary> get Path to export the model </summary>
    private void getExportPath()
    {
        #if UNITY_STANDALONE_WIN

        var bp = new BrowserProperties();
        bp.filter = "3dmod files (*.3dmod)|*.3dmod";
        bp.filterIndex = 0;

        new FileBrowser().SaveFileBrowser(bp, "FileName", ".3dmod", tempPath =>
        {
            path = tempPath;
        });
        #endif
    }

    /// <summary> write data to file </summary>
    private void exportData()
    {
        XmlDocument doc = new XmlDocument();

        // load old model
        doc.Load(model2.path);

        // delete blocks
        for(int i = 1; i <= 7; ++i)
        {
            var y = doc.GetElementsByTagName("block")[3];
            doc.DocumentElement.RemoveChild(y);
        }

        //create new blocks
        XmlElement reflBlock = doc.CreateElement("block");
        XmlElement refWidthBlock = doc.CreateElement("block");
        XmlElement tempBlock = doc.CreateElement("block");
        XmlElement distanceBlock = doc.CreateElement("block");
         
        reflBlock.SetAttribute("type", "reflectivity");
        reflBlock.SetAttribute("length", (points.Count*4).ToString()); // float = 4 bytes
        refWidthBlock.SetAttribute("type", "refl_width");
        refWidthBlock.SetAttribute("length", (points.Count*4).ToString());
        tempBlock.SetAttribute("type", "temperature");
        tempBlock.SetAttribute("length", (points.Count * 4).ToString()); // float = 4 bytes
        distanceBlock.SetAttribute("type", "distance");
        distanceBlock.SetAttribute("length", (points.Count * 4).ToString()); // float = 4 bytes

        // creating buffers to save convertion
        byte[] tempBuffer = new byte[points.Count * 4];
        byte[] tempBuffer2 = new byte[points.Count * 4];
        byte[] tempBuffer3 = new byte[points.Count * 4];
        byte[] tempBuffer4 = new byte[points.Count * 4];

        // fill the buffers
        Buffer.BlockCopy(points.Select(x => x.refDiff).ToArray(), 0, tempBuffer, 0, tempBuffer.Length);
        Buffer.BlockCopy(points.Select(x => x.refWidthDiff).ToArray(), 0, tempBuffer2, 0, tempBuffer2.Length);
        Buffer.BlockCopy(points.Select(x => x.tempDiff).ToArray(), 0, tempBuffer3, 0, tempBuffer3.Length);
        Buffer.BlockCopy(points.Select(x => x.distance).ToArray(), 0, tempBuffer4, 0, tempBuffer4.Length);

        // encode - Base64
        string s = Convert.ToBase64String(tempBuffer);
        string s2 = Convert.ToBase64String(tempBuffer2);
        string s3 = Convert.ToBase64String(tempBuffer3);
        string s4 = Convert.ToBase64String(tempBuffer4);

        reflBlock.InnerText = s;
        refWidthBlock.InnerText = s2;
        tempBlock.InnerText = s3;
        distanceBlock.InnerText = s4;

        doc.DocumentElement.AppendChild(reflBlock);
        doc.DocumentElement.AppendChild(refWidthBlock);
        doc.DocumentElement.AppendChild(tempBlock);
        doc.DocumentElement.AppendChild(distanceBlock);

        // save new file to path
        doc.Save(path);
    }

    #endregion

    #endregion

    #region Alignment

    #region ICP - Auto alignment

    /// <summary> ICP point to plane with KD tree </summary>
    private void ICPKD()
    {
        ///definitions
        var m = Matrix<double>.Build;
        int Np = model2.points.Count;
        changeRealPoints = Matrix<double>.Build.Dense(3, model2.points.Count);

        for (float i = 0.2f; i <= 1; i += 0.2f)
        {
            for (int l = 0; l < model2.vertices.Length; ++l)
            {
                changeRealPoints[0, l] = model2.vertices[l].x;
                changeRealPoints[1, l] = model2.vertices[l].y;
                changeRealPoints[2, l] = model2.vertices[l].z;
            }

            if (i != 1)
            {
                model1.quality = i;
                model2.quality = i;

                model1.SimplifyMesh();
                model2.SimplifyMesh();
            }
            else
            {
                model1.simplVert = model1.vertices;

                // avoiding a Unity bug 
                Mesh mesh = model1.GetComponent<MeshFilter>().mesh;
                mesh.vertices = model1.simplVert;
                mesh.triangles = model1.indices;
                mesh.RecalculateBounds();
                mesh.RecalculateNormals();
                model1.simplNorm = mesh.normals;
                //********

                model2.simplVert = model2.vertices;
            }

            Matrix<double> refPoints = Matrix<double>.Build.Dense(3, model1.simplVert.Length);
            Matrix<double> changePoints = Matrix<double>.Build.Dense(3, model2.simplVert.Length);
            Matrix<double> refNormals = Matrix<double>.Build.Dense(3, model1.simplNorm.Length);

            for (int j = 0; j < model1.simplVert.Length; ++j)
            {
                refPoints[0, j] = model1.simplVert[j].x;
                refPoints[1, j] = model1.simplVert[j].y;
                refPoints[2, j] = model1.simplVert[j].z;
            }

            for (int k = 0; k < model2.simplVert.Length; ++k)
            {
                changePoints[0, k] = model2.simplVert[k].x;
                changePoints[1, k] = model2.simplVert[k].y;
                changePoints[2, k] = model2.simplVert[k].z;
            }

            for (int mk = 0; mk < model1.simplNorm.Length; ++mk)
            {
                refNormals[0, mk] = model1.simplNorm[mk].x;
                refNormals[1, mk] = model1.simplNorm[mk].y;
                refNormals[2, mk] = model1.simplNorm[mk].z;
            }

            Matrix<double> changePointsT = changePoints.Transpose();
            Matrix<double> refPointsT = refPoints.Transpose();
            Matrix<double> refNormalsT = refNormals.Transpose();

            // ICP point-to-plane
            newPoints = objekt.ICP_run3(changePointsT, refPointsT, refNormalsT);

            for (int ch = 0; ch < newPoints.ColumnCount; ch++)
            {
                model2.vertices[ch] = new Vector3((float)newPoints[0, ch], (float)newPoints[1, ch], (float)newPoints[2, ch]);
            }

            if (objekt.endLoop)
            {
                objekt.endLoop = false;
                break;
            }
        }
    }

    #endregion

    #region SVD - manual alignment

    // <summary> Calculate center of Mass </summary>
    private void GetCentroids()
    {
        float meanAx = 0; float meanBx = 0; float meanAy = 0; float meanBy = 0; float meanAz = 0; float meanBz = 0;


        for (int i = 0; i < model1.measShape1Points.Count; i++)
        {
            meanAx += model1.points[model1.measShape1Points[i]].Vertex.Position.x;
            meanAy += model1.points[model1.measShape1Points[i]].Vertex.Position.y;
            meanAz += model1.points[model1.measShape1Points[i]].Vertex.Position.z;
        }

        for (int j = 0; j < model2.measShape2Points.Count; j++)
        {
            meanBx += model2.points[model2.measShape1Points[j]].Vertex.Position.x;
            meanBy += model2.points[model2.measShape1Points[j]].Vertex.Position.y;
            meanBz += model2.points[model2.measShape1Points[j]].Vertex.Position.z;
        }

        meanA[0, 0] = meanAx;
        meanA[1, 0] = meanAy;
        meanA[2, 0] = meanAz;

        meanB[0, 0] = meanBx;
        meanB[1, 0] = meanBy;
        meanB[2, 0] = meanBz;

        meanA = meanA * (1 / model1.measShape1Points.Count);
        meanB = meanB * (1 / model2.measShape1Points.Count);
    }

    // <summary> Calculate new points </summary>
    private void CalculateNewPoints()
    {
        GetCentroids();

        if (model1.measShape1Points.Count > model2.measShape1Points.Count)
        {
            matrixPointsA = Matrix<float>.Build.Dense(model1.measShape1Points.Count, 3);
            matrixPointsB = Matrix<float>.Build.Dense(model1.measShape1Points.Count, 3);
        }
        else if (model1.measShape1Points.Count < model2.measShape1Points.Count)
        {
            matrixPointsA = Matrix<float>.Build.Dense(model2.measShape1Points.Count, 3);
            matrixPointsB = Matrix<float>.Build.Dense(model2.measShape1Points.Count, 3);
        }
        else
        {
            matrixPointsA = Matrix<float>.Build.Dense(model1.measShape1Points.Count, 3);
            matrixPointsB = Matrix<float>.Build.Dense(model2.measShape1Points.Count, 3);
        }

        for (int i = 0; i < model1.measShape1Points.Count; i++)
        {
            matrixPointsA[i, 0] = model1.points[model1.measShape1Points[i]].Vertex.Position.x - meanA[0, 0];
            matrixPointsA[i, 1] = model1.points[model1.measShape1Points[i]].Vertex.Position.y - meanA[1, 0];
            matrixPointsA[i, 2] = model1.points[model1.measShape1Points[i]].Vertex.Position.z - meanA[2, 0];
        }

        for (int i = 0; i < model2.measShape1Points.Count; i++)
        {
            matrixPointsB[i, 0] = model2.points[model2.measShape1Points[i]].Vertex.Position.x - meanB[0, 0];
            matrixPointsB[i, 1] = model2.points[model2.measShape1Points[i]].Vertex.Position.y - meanB[1, 0];
            matrixPointsB[i, 2] = model2.points[model2.measShape1Points[i]].Vertex.Position.z - meanB[2, 0];
        }

        Matrix<float> matrixC;
        Matrix<float> matrixD = Matrix<float>.Build.Dense(model2.points.Count, 3);

        matrixC = matrixPointsA.Transpose() * matrixPointsB;

        var Rotation = matrixC.Svd().VT.Transpose() * matrixC.Svd().U.Transpose();

        for (int i = 0; i < model2.points.Count; i++)
        {
            matrixD[i, 0] = model2.points[i].Vertex.Position.x;
            matrixD[i, 1] = model2.points[i].Vertex.Position.y;
            matrixD[i, 2] = model2.points[i].Vertex.Position.z;

        }

        Matrix<float> translation = Matrix<float>.Build.DenseIdentity(4);

        var translationSub = meanA - (Rotation * meanB);
        translation.SetSubMatrix(3, 0, translationSub.Transpose());

        Matrix<float> MatrixBtran = Matrix<float>.Build.Dense(matrixD.RowCount, 4, 1);
        MatrixBtran.SetSubMatrix(0, 0, matrixD);

        var matrixPointsC = MatrixBtran * translation;

        matrixD.SetColumn(0, matrixPointsC.Column(0));
        matrixD.SetColumn(1, matrixPointsC.Column(1));
        matrixD.SetColumn(2, matrixPointsC.Column(2));

        matrixD = matrixD * Rotation;

        model2.vertices = new Vector3[matrixD.RowCount];

        for (int i = 0; i < matrixD.RowCount; i++)
        {
           model2.vertices[i] = new Vector3(matrixD[i, 0], matrixD[i, 1], matrixD[i, 2]);
        }
    }
    #endregion

    #endregion

    #region filtration - temperature feature
    // <summary> Substract patologic temperature </summary>
    private void AverageTemp()
    {
        if (AlignCondition) //automatic - true, manual - false
        {
            // get bounds vertices
            if (model1.vertices.Length >= model2.vertices.Length)
            {
                // get mesh bound of smaller model
                Bounds bound = model2.GetComponent<MeshFilter>().mesh.bounds;

                // find vertices in bound and save their indices
                for (int i = 0; i < model2.points.Count; ++i)
                {
                    if (bound.Contains(model2.points[i].Vertex.Position))
                        boundIndices.Add(i);
                }

                // calculate average temp and put it into model2 tempearture
                GetAverageTemp(model2.points, boundIndices, model1.points, "model2");
            }
            else
            {
                Bounds bound = model1.GetComponent<MeshFilter>().mesh.bounds;

                // find vertices in bound and save their indices
                for (int i = 0; i < model1.points.Count; ++i)
                {
                    if (bound.Contains(model1.points[i].Vertex.Position))
                        boundIndices.Add(i);
                }

                // calculate average temp and put it into model2 tempearture
                GetAverageTemp(model1.points, boundIndices, model2.points, "model1");
            }

            boundIndices.Clear();
        }
        else
        {
            Vector3 avgModelCenter = new Vector3(0, 0, 0);

            if (model1.measShape1Points.Count > 0)
            {
                // SHAPE 1 REGION
                // create boundBox
                Bounds boundBox = new Bounds();

                // get center of boundbox
                foreach (var i in model1.measShape1Points)
                    avgModelCenter += model1.points[i].Vertex.Position;
                avgModelCenter /= model1.measShape1Points.Count;

                boundBox.center = avgModelCenter;

                foreach (var i in model1.measShape1Points)
                    boundBox.Encapsulate(model1.points[i].Vertex.Position);

                // find vertices in bound and save their indices
                for (int i = 0; i < model2.points.Count; ++i)
                {
                    if (boundBox.Contains(model2.points[i].Vertex.Position))
                        boundIndices.Add(i);
                }

                //get average Temp
                GetAverageTemp(model2.points, boundIndices, model1.points, model1.measShape1Points, "model2");

                boundIndices.Clear();
            }

            if (model1.measShape2Points.Count > 0)
            {
                // SHAPE 2 REGION
                // create boundBox
                Bounds boundBox = new Bounds();

                // get center of boundbox
                foreach (var i in model1.measShape2Points)
                    avgModelCenter += model1.points[i].Vertex.Position;
                avgModelCenter /= model1.measShape2Points.Count;

                boundBox.center = avgModelCenter;

                foreach (var i in model1.measShape1Points)
                    boundBox.Encapsulate(model1.points[i].Vertex.Position);

                // find vertices in bound and save their indices
                for (int i = 0; i < model2.points.Count; ++i)
                {
                    if (boundBox.Contains(model2.points[i].Vertex.Position))
                        boundIndices.Add(i);
                }

                //get average Temp
                GetAverageTemp(model2.points, boundIndices, model1.points, model1.measShape2Points, "model2");

                boundIndices.Clear();
            }
        }
    }

    // <summary> Substract patologic temperature - gradient</summary>
    private void GradientTemp()
    {
        float tempDistance = float.MaxValue;
        int finalIndex = 0;
        float tempThermalDiff = float.NaN;
        Vector3 avgModelCenter = new Vector3(0, 0, 0);
        Bounds boundBox = new Bounds();

        // only for manual
        // for SHAPE 1

        // get thermal differences between NEAREST points

        for (int i = 0; i < model2.measShape1Points.Count; ++i)
        {
            for (int j = 0; j < model1.measShape1Points.Count; ++j)
            {
                float tempMag = 0;
                Vector3 tempP = model2.points[model2.measShape1Points[i]].Vertex.Position - model1.points[model1.measShape1Points[j]].Vertex.Position;
                tempMag = tempP.sqrMagnitude;

                if (tempMag < tempDistance)
                {
                    tempDistance = tempMag;
                    finalIndex = j;
                }
            }

            tempThermalDiff = model2.points[model2.measShape1Points[i]].Temperature - model1.points[model1.measShape1Points[finalIndex]].Temperature;
            differences.Add(tempThermalDiff);
            model2.points[model2.measShape1Points[i]].Temperature -= tempThermalDiff;

            tempDistance = float.MaxValue;
            finalIndex = 0;
        }

        // get boundbox on model2
        // get center of boundbox
        foreach (var i in model2.measShape1Points)
            avgModelCenter += model2.points[i].Vertex.Position;
        avgModelCenter /= model2.measShape1Points.Count;

        boundBox.center = avgModelCenter;

        foreach (var i in model2.measShape1Points)
            boundBox.Encapsulate(model2.points[i].Vertex.Position);

        // find vertices in bound and save their indices
        for (int i = 0; i < model2.points.Count; ++i)
        {
            if (boundBox.Contains(model2.points[i].Vertex.Position))
                boundIndices.Add(i);
            else
                model2.points[i].Temperature = float.NaN;
        }

        // interpolation - Generalized Barycentric coordinates
        foreach (var index in boundIndices)
        {
            float interDiff = 0;

            computeBaryCord(model2.points[index].Vertex.Position);

            if (weights.Count != 0)
            {

                for (int i = 0; i < weights.Count; ++i)
                {
                    interDiff += (weights[i] * differences[i]);
                }

                if (!float.IsNaN(interDiff))
                {
                    model2.points[index].Temperature -= interDiff;
                }

                weights.Clear();
            }
            else
                model2.points[index].Temperature = float.NaN;
        }

        boundIndices.Clear();
    }

    private void computeBaryCord(Vector3 point)
    {
        float weightSum = 0;
        int n = model2.measShape1Points.Count;

        for (int i = 0; i < n; ++i)
        {
            int prev = (i + n - 1) % n;
            int next = (i + 1) % n;
            float w1 = contangent(point, model2.points[model2.measShape1Points[i]].Vertex.Position, model2.points[model2.measShape1Points[prev]].Vertex.Position);
            float w2 = contangent(point, model2.points[model2.measShape1Points[i]].Vertex.Position, model2.points[model2.measShape1Points[next]].Vertex.Position);
            float weight = (w1 + w2) / (Mathf.Pow((point - model2.points[model2.measShape1Points[i]].Vertex.Position).magnitude, 2));
            weightSum += weight;

            weights.Add(weight);
        }

        for (int i = 0; i < weights.Count; ++i)
        {
            if (weights[i] >= 0)
                weights[i] /= weightSum;
            else
            {
                weights.Clear();
                break;
            }
        }
    }

    private float contangent(Vector2 a, Vector2 b, Vector2 c)
    {
        Vector2 ba = a - b;
        Vector2 bc = c - b;
        Vector3 crossVectors = Vector3.Cross(bc, ba);

        return Vector2.Dot(bc, ba) / Vector3.Magnitude(crossVectors);
    }
    #endregion

    #region Helping methods
    // <summary> Change color of the models </summary>
    private void ChangeColorModels()
    {
        for (int i = 0; i < model1.points.Count; i++)
            model1.points[i].Vertex.Color = new Color(255, 88, 85, 255);

        for (int i = 0; i < model2.points.Count; i++)
            model2.points[i].Vertex.Color = new Color(85, 88, 185, 255);
    }

    /// <summary> Get vertices of model in bounds of the 2nd model </summary>
    private void GetVerticeBounds(Bounds boundModel, Vector3[] model2Vertices)
    {
        foreach(var i in model2Vertices)
            if(boundModel.Contains(i))
                boundVertices.Add(i); 
    }
    

    /// <summary> Calculate average temperature </summary>
    private void GetAverageTemp(List<RoscanModelUnity.ModelPoint> modelPoints, List<int> indices, List<RoscanModelUnity.ModelPoint> modelPoints2, string Order)
    {
        float averTemp1 = 0;
        float averTemp2 = 0;
        float diff = 0;
        int counter = 0;

        for( int i = 0; i < indices.Count; ++i)
        {
            if (!float.IsNaN(modelPoints[indices[i]].Temperature))
                averTemp1 += modelPoints[indices[i]].Temperature;
            else
                ++counter;
        }
        averTemp1 /= (indices.Count-counter);
        counter = 0;

        foreach (var i in modelPoints2)
        {
            if (!float.IsNaN(i.Temperature))
                averTemp2 += i.Temperature;
            else
                ++counter;
        }
        averTemp2 /= (modelPoints2.Count-counter);

        // order - modelPoints = model1(ref)
        if (Order == "model1")
        {
            diff = averTemp2 - averTemp1;

            foreach (var i in modelPoints2)
                i.Temperature -= diff;
        }
        else // modelPoints = model2
        {
            diff = averTemp1 - averTemp2;

            for (int i = 0; i < indices.Count; ++i)
                modelPoints[indices[i]].Temperature -= diff;
        }
    }

    /// <summary> Calculate average temperature </summary>
    private void GetAverageTemp(List<RoscanModelUnity.ModelPoint> modelPoints, List<int> indices, List<RoscanModelUnity.ModelPoint> modelPoints2, List<int> indices2, string Order)
    {
        float averTemp1 = 0;
        float averTemp2 = 0;
        float diff = 0;
        int counter = 0;

        for (int i = 0; i < indices.Count; ++i)
        {
            if (!float.IsNaN(modelPoints[indices[i]].Temperature))
                averTemp1 += modelPoints[indices[i]].Temperature;
            else
                ++counter;
        }
        averTemp1 /= indices.Count;
        counter = 0;

        foreach (var i in indices2)
        {
            if (!float.IsNaN(modelPoints2[i].Temperature))
                averTemp2 += modelPoints2[i].Temperature;
            else
                ++counter;
        }
        averTemp2 /= modelPoints2.Count;
        counter = 0;

        diff = averTemp2 - averTemp1;

        foreach (var i in modelPoints)
            i.Temperature -= diff;
    }

    /// <summary> Save new Vertices to main Data structure </summary>
    private void SetVertices()
    {
        for(int i = 0; i < model2.vertices.Length; ++i)
            model2.points[i].Vertex.Position = model2.vertices[i];
    }

    #endregion

    #endregion
}

