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

/*
 * This class is used to provide functions which are used to find paths in the map
*/
public abstract class PathFinding {

	public static Path findPath(DeployedUnit unit, Vertex endMeterVertex, bool shift = false) {

		GameVertex startVertex = Converter.metersToGameCoo(shift ? unit.getDestination() : unit.getPosition());
		GameVertex endVertex = Converter.metersToGameCoo(endMeterVertex);
        Segment straightLine = new Segment(startVertex, endVertex);
		PathConstraints constraints = unit.getClass().getPathConstraints();

        //Debug.Log(startVertex.x() + " ; " + startVertex.y());
        //Debug.Log(endVertex.x() + " ; " + endVertex.y());

        //Init

        //Debug.Log(terrainTriangles.toList().Count);

        TerrainTriangle startTriangle = null;
        TerrainTriangle endTriangle = null;
		Set<TerrainTriangle> terrainTriangles = GameController.getTerrainTriangles();
        foreach (TerrainTriangle terrainTriangle in terrainTriangles) {

            terrainTriangle.previous = null;
            terrainTriangle.weight = double.MaxValue;
            terrainTriangle.processed = false;

            if (terrainTriangle.isPointInside(startVertex)) {
                startTriangle = terrainTriangle;
                //uniformTriangle.addToGame();
            } 
            if (terrainTriangle.isPointInside(endVertex)) {
                endTriangle = terrainTriangle;
            }
        }

        /*Debug.Log(startTriangle[0].x() + " ; " + startTriangle[0].y());
        Debug.Log(startTriangle[1].x() + " ; " + startTriangle[1].y());
        Debug.Log(startTriangle[2].x() + " ; " + startTriangle[2].y());*/

        if (startTriangle == null) {
            Debug.LogWarning("Start point not in a triangle");
            return null;
        } 
        else if (endTriangle == null) {
            Debug.LogWarning("End point not in a triangle");
            return null;
        }

        LinkedList<TerrainTriangle> queue = new LinkedList<TerrainTriangle>();
        queue.AddFirst(startTriangle);
        bool found = false;
        //Find list of triangles
        while (queue.Count != 0 && !found) { //we use here a A* based algorithm to first find the list of triangles in which the path will go trough
            TerrainTriangle current = queue.First.Value;
            queue.RemoveFirst();

            if (current.processed) {
                continue;
            }

            current.processed = true;

            //current.addToGame();

            if (current == endTriangle) {
                found = true;
            } else {
				List<TerrainTriangle> currentNeighborgs = current.getNeighborgs();

                for (int i = 0; i < currentNeighborgs.Count; i++) {

                    TerrainTriangle neighborg = currentNeighborgs[i];

                    if (!neighborg.processed && constraints.canGoThrough(neighborg)) {
                        double weight = getTriangleWeight(straightLine, neighborg);

                        if (neighborg.weight > weight) { //(re)insert neib into the queue
                            
                            neighborg.weight = weight;
                            neighborg.previous = current;

                            if (queue.Count > 0) { // we have to insert the triangle in the right position in the queue
                                bool inserted = false;
                                LinkedListNode<TerrainTriangle> node = queue.First;
                                do {
                                    if (node.Value.weight > weight) {
                                        queue.AddBefore(node, neighborg);
                                        inserted = true;
                                    }

                                    node = node.Next;

                                    if (node == null) { // end of the list
                                        break;
                                    }
                                } while (!inserted);

                                if (!inserted) {
                                    queue.AddLast(neighborg);
                                }
                            } else {
                                queue.AddFirst(neighborg);
                            }

                        }
                    }
                }
            }
        }

        if (!found) {
            Debug.LogWarning("Path Not found");
            return null;
        }

        //Find points
        List<Segment> sharedEdges;
		List<GameVertex> list = findBasicPath(startVertex, endVertex, startTriangle, endTriangle, out sharedEdges);

		//Remove zig zags
		return new Path(removeZigZags(unit.getClass().getPathConstraints(), ecremage(list, sharedEdges)), unit);
    }

    static double getTriangleWeight(Segment straightLine, TerrainTriangle t) {
        return t.minDistanceTo(straightLine.getSecondPoint());
    }

    //In this method, we 'unroll' the list of triangles from the end, to get the first version of a path
	private static List<GameVertex> findBasicPath(GameVertex start, GameVertex end, TerrainTriangle startTriangle, TerrainTriangle endTriangle, out List<Segment> sharedEdges) {
        Segment straightLine = new Segment(start, end);

		List<GameVertex> list = new List<GameVertex>();
        sharedEdges = new List<Segment>();

        TerrainTriangle curr = endTriangle; // curr is the triangle we are currently processing
		list.Add(end);

		GameVertex lastPoint = end;

        do {
            TerrainTriangle prev = curr.previous;

            if (prev != null) {
                // add intersection point between curr and prev that is closest to end

                Segment shared = prev.edgeShared(curr);
				GameVertex newPoint;
                if (shared == null) { // the two triangles only share a point (with our current setup, this never happens)
                    prev.vertexShared(curr, out newPoint);
                } else {
					GameVertex sharedStraight = shared.closestPointTo(straightLine);

                    if (shared.intersect(straightLine)) { // intersection
                        newPoint = sharedStraight; //follow the straight line
                    } else {
                        newPoint = shared.closestPointTo(lastPoint); // hot potato routing
                    }

                    sharedEdges.Add(shared);
                }
				list.Insert(0, newPoint);
                lastPoint = newPoint;


            } else { // we are at start triangle
				list.Insert(0, start);
            }

            curr = prev;

        } while (curr != null);

        return list;
    }

	//In this method, we try to straighten the path, by removing some point
	private static List<GameVertex> removeZigZags(PathConstraints constraints, List<GameVertex> list) {
		List<GameVertex> currentList = list;

		if (currentList.Count < 3) {
			return currentList;
		}

		int currentIndex = 1;
		do {
			Segment toBeTested = new Segment(currentList[currentIndex - 1], currentList[currentIndex + 1]);
			bool intersect = false;
			foreach(MapPolygon mapPoly in GameController.getMap()) {
				if (!constraints.canGoThrough(mapPoly.getTerrain()) && mapPoly.getSlicedPolygon().intersect(toBeTested)) {
					intersect = true;
					break;
				}
			}

			if (!intersect) {
				currentList.RemoveAt(currentIndex);
				//do not change currentIndex
			}
			else {
				currentIndex ++;
			}

		} while (currentIndex < currentList.Count - 1);

		return currentList;
	}

    //In this method, we try to straighten the path, by removing some point
	private static List<GameVertex> ecremage(List<GameVertex> list, List<Segment> sharedEdges) {
		List<GameVertex> newPath = new List<GameVertex>();
        sharedEdges.Reverse(); // we added the segments from end to start, so we need to reverse the list

		List<GameVertex> oldPath = list;

        List<bool> isDeleted = new List<bool>();
        List<Tuple<int, double>> anglesPerPoint = new List<Tuple<int, double>>(); // angles between each line

        isDeleted.Add(false);
        anglesPerPoint.Add(new Tuple<int, double>(0, 0.0f));// start
        for (int i = 1; i < oldPath.Count - 1; i++) {
            isDeleted.Add(false);

			anglesPerPoint.Add(new Tuple<int, double>(i, Math.Acos(Segment.dot((oldPath[i - 1] - oldPath[i]).getValue().normalized, (oldPath[i + 1] - oldPath[i]).getValue().normalized))));
        }
        isDeleted.Add(false);
        anglesPerPoint.Add(new Tuple<int, double>(oldPath.Count - 1, 0.0f));// end

        //we try to first remove points with high angles (which can mean zig zag)
        anglesPerPoint = anglesPerPoint.OrderByDescending(t => t._2).ToList();

        bool done = false;
        while (!done) {
            done = true;

            //At this point, we know that all consecutive points can be reached in a straight line
            for (int index = 0; index < anglesPerPoint.Count; index++) {
                int i = anglesPerPoint[index]._1;
                anglesPerPoint.RemoveAt(index);

                if (i > 0 && i < oldPath.Count - 1) { //cannot remove start or end
                    // try to remove point i

                    //get previous and next not deleted points
                    int previous = i - 1;
                    for (int j = i - 1; j >= 0; j--) {
                        if (!isDeleted[j]) {
                            previous = j;
                            break;
                        }
                    }

                    int next = i + 1;
                    for (int j = i + 1; j < oldPath.Count; j++) {
                        if (!isDeleted[j]) {
                            next = j;
                            break;
                        }
                    }

                    //Check if new line is correct
					Segment seg = new Segment(oldPath[previous], oldPath[next]);

                    bool allIntersect = true;
                    for (int j = previous + 1; j < next; j++) {
                        if (!sharedEdges[j - 1].intersect(seg)) {
                            allIntersect = false;
                        }
                    }

                    if (allIntersect) {
                        isDeleted[i] = true;
                        done = false;
                        break;
                    }
                }
            }
        }

        for (int i = 0; i < oldPath.Count; i++) {
            if (!isDeleted[i]) {
                newPath.Add(oldPath[i]);
            }
        }

        return newPath;
    }
}


