using Assets.NXR.Scripts.Slam;
using System;
using System.Collections.Generic;
using UnityEngine;

public class NxrSlamPlane : MonoBehaviour
{
    public bool Test;
    public bool Reload;
    NxrSlam.NXRPlaneData testdata;
    // Start is called before the first frame update
    void Start()
    {
        Debug.Log("NxrSlamPlane.Start." + GetHashCode());
        // test
        testdata = new NxrSlam.NXRPlaneData(null);
        testdata.size = 14;
        testdata.points = new NxrSlam.NXRPlaneData.Point3D[]
        {
            new NxrSlam.NXRPlaneData.Point3D(0.597963809967041,-0.483312696218491,0.43642520904541),
            new NxrSlam.NXRPlaneData.Point3D(0.448058247566223,-0.483312696218491,0.369752883911133),
            new NxrSlam.NXRPlaneData.Point3D(0.448058247566223,-0.483312696218491,0.369752883911133),
            new NxrSlam.NXRPlaneData.Point3D(0.32343664765358,-0.483312696218491,0.323436379432678),
            new NxrSlam.NXRPlaneData.Point3D(0.3141910135746,-0.483312696218491,0.323615431785584),
            new NxrSlam.NXRPlaneData.Point3D(0.0990831553936005,-0.483312696218491,0.33366933465004),
            new NxrSlam.NXRPlaneData.Point3D(0.0974671021103859,-0.483312696218491,0.333752363920212),
            new NxrSlam.NXRPlaneData.Point3D(0.0561475902795792,-0.483312696218491,0.337889224290848),
            new NxrSlam.NXRPlaneData.Point3D(-0.465384006500244,-0.483312696218491,0.51269006729126),
            new NxrSlam.NXRPlaneData.Point3D(-0.466641992330551,-0.483312696218491,0.517888009548187),
            new NxrSlam.NXRPlaneData.Point3D(-0.232375964522362,-0.483312696218491,0.975189507007599),
            new NxrSlam.NXRPlaneData.Point3D(0.267775535583496,-0.483312696218491,0.737699329853058),
            new NxrSlam.NXRPlaneData.Point3D(0.360924899578094,-0.483312696218491,0.65373432636261),
            new NxrSlam.NXRPlaneData.Point3D(0.597963809967041,-0.483312696218491,0.43642520904541)
        }; 
        // test
    }

    // Update is called once per frame
    void Update()
    {
        if (Test)
        {
            Test = false;
            NxrSlam.Instance.GeneratePlaneObject(testdata);
        } else if(Reload)
        {
            MakePlane(testdata);
        }

    }

    public GameObject PointPrefab;
    Stack<GameObject> PointsRecycleStack = new Stack<GameObject>();
    List<GameObject> PointsList = new List<GameObject>();

    public int PlanePointsCount { set; get; }
    public int PlaneId { set; get; }
    public NxrSlam.NXRPlaneData PlaneData;
    public void MakePlane(NxrSlam.NXRPlaneData data)
    {
        PlaneData = data;
        // Debug.Log(data.id + ".NxrSlamPlane.MakePlane=" +(GetComponent<MeshFilter>()==null) +GetHashCode());
        PlanePointsCount = data.size;
        PlaneId = data.id;
        double xSum = 0, ySum = 0, zSum = 0;
        Vector2[] meshUvs = new Vector2[data.size];
        Vector3[] meshVertices = new Vector3[data.size];
        Vector2[] vertices2D = new Vector2[data.size];

        float minX = 0, maxX = 0;
        float minZ = 0, maxZ = 0;
        for (int i = 0; i < data.size; i++)
        {
            NxrSlam.NXRPlaneData.Point3D point3d = data.points[i];
            xSum += point3d.x;
            ySum += point3d.y;
            zSum += point3d.z;
            meshVertices[i].x = (float)point3d.x;
            meshVertices[i].y = (float)point3d.y;
            meshVertices[i].z = (float)point3d.z;

            //GameObject cubeObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
            //cubeObj.name = "C" + i;
            //cubeObj.transform.position =new Vector3((float)point3d.x, (float)point3d.y, (float)point3d.z);

            if (point3d.x < minX)
            {
                minX = (float) point3d.x;
            }

            if (point3d.x > maxX)
            {
                maxX = (float)point3d.x;
            }

            if (point3d.z < minZ)
            {
                minZ = (float)point3d.z;
            }

            if (point3d.z > maxZ)
            {
                maxZ = (float)point3d.z;
            }

            vertices2D[i] =new Vector2((float) point3d.x, (float)point3d.z);
        }

        float xLength = maxX - minX;
        float zLength = maxZ - minZ;
        for (int i = 0; i < data.size; i++)
        {
            float u = meshVertices[i].x / xLength;
            float v = meshVertices[i].z / zLength;
            meshUvs[i] = new Vector2(u, v);
        }

        NxrTriangulator nxrTriangulator = new NxrTriangulator(vertices2D);
        int[] triangles = nxrTriangulator.Triangulate();
       // System.Array.Reverse(triangles);
        string trianglesStr = "";
        foreach (int i in triangles)
        {
            trianglesStr += i;
            trianglesStr += ",";
        }
        Debug.Log("triangles=" + trianglesStr);

        Vector3 center;
        center.x = (float)(xSum / data.size);
        center.y = (float)(ySum / data.size);
        center.z = (float)(zSum / data.size);

        // this.transform.position = center;
        // Debug.Log("PlaneCenter=" + center.x + "," + center.y + "," + center.z);

        Mesh mesh = new Mesh
        {
            vertices = meshVertices,
            uv = meshUvs,
            triangles = triangles,
            name = "planeMesh" + data.id
        };
        GetComponent< MeshFilter>().mesh = mesh;
        mesh.RecalculateNormals();
        mesh.RecalculateBounds();


        GeneratePoints(data);
    }

    private void GeneratePoints(NxrSlam.NXRPlaneData data)
    {
        if (PointPrefab == null)
        {
            PointPrefab = Resources.Load<GameObject>("Prefabs/PlanePoint");
        }
        // 先回收
        foreach (GameObject p in PointsList)
        {
            PointsRecycleStack.Push(p);
        }
        PointsList.Clear();

        // 隐藏
        foreach (GameObject p in PointsRecycleStack)
        {
            p.SetActive(false);
        }

        // Debug.Log("GeneratePoints : Data=" + data.size + ",PointsList=" + PointsList.Count + ",PointsRecycleStack=" + PointsRecycleStack.Count);

        for (int i = 0, size = data.size; i < size; i++)
        {
            NxrSlam.NXRPlaneData.Point3D point3d = data.points[i];
            Vector3 pointPos = new Vector3((float)point3d.x, (float)point3d.y, (float)point3d.z);

            GameObject go = null;
            if (PointsRecycleStack.Count > 0)
            {
                go = PointsRecycleStack.Pop();
            }
            else
            {
                go = Instantiate(PointPrefab, Vector3.zero, Quaternion.identity, transform) as GameObject;
                go.name = "Point_" + data.id + "_" + i;
            }

            go.SetActive(true);

            pointPos.y += 0.03f;
            go.transform.position = pointPos;

            TextMesh textMesh = go.GetComponent<TextMesh>();
            if (textMesh != null)
            {
                textMesh.text = "" + i;
            }

            PointsList.Add(go);
        }
    }

}