#pragma once

#include <f2.h>

#include <unordered_map>
#include <unordered_set>

#include <Rect.h>

#include <queue>

#include <algorithm>

#include <functional>

using std::unordered_map;
using std::pair;
using std::vector;
using std::make_tuple;
using std::cout;
using std::endl;
using std::unordered_set;

using namespace std::placeholders;

template<typename T, typename Number=float>
struct PriorityQueue
{
  typedef pair<Number, T> PQElement;
  std::priority_queue<PQElement, vector<PQElement>, std::greater<PQElement>> elements;

  inline bool empty() const { return elements.empty(); }

  inline void put(T item, Number priority)
  {
    elements.emplace(priority, item);
  }

  inline T get()
  {
    T best_item = elements.top().second;
    elements.pop();
    return best_item;
  }
};

inline float heuristic(f2 a, f2 b)
{
  return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

class Node
{
public:
    Node(){}

    Node(const f2& position, vector<Node*> links = vector<Node*>(0))
    {
        position_ = position;
        links_ = links;
    }

f2 position_;
vector<Node*> links_;
};

inline float nodeDistanceSquared(f2 position, Node* node)
{
    return (position - node->position_).lengthSquared();
}

inline bool distanceCompare(f2 position, Node* node1, Node* node2)
{
    return nodeDistanceSquared(position, node1) < nodeDistanceSquared(position, node2);
}


class Pathfinding
{
public:
    Pathfinding(){}

    Pathfinding(const f2& gridSize, const f2& nodeSize, const vector<Rect>& staticObstacles = vector<Rect>(0))
    {
        obstacles_ = staticObstacles;
        gridSize_ = gridSize;
        nodeSize_ = nodeSize;
        halfNodeSize_ = nodeSize / 2;
        generateNodes(obstacles_);
    }

    inline vector<Node*> neighbors(f2 position)
    {
        vector<Node*> results;
        if (nodes_.count(position))
        {
            Node* node = nodes_[position];

            for (int i = 0; i < node->links_.size(); ++i)
            {
                results.push_back(node->links_[i]);
            }
        }
        else
        {
            bool bottomRight = false;
            bool bottomLeft = false;
            bool topLeft = false;
            bool topRight = false;

            for (int i = 0; i < nodesV_.size(); ++i)
            {
                Node* otherNode = nodesV_[i];
                f2 vec = position - otherNode->position_;
                if (vec.x <= 0 && vec.y < 0 && !bottomLeft)
                {
                    if (straightPath(position, otherNode->position_))
                    {
                        bottomLeft = true;
                        results.push_back(otherNode);
                    }
                }
                else if (vec.x > 0 && vec.y <= 0 && !bottomRight)
                {
                    if (straightPath(position, otherNode->position_))
                    {
                        bottomRight = true;
                        results.push_back(otherNode);
                    }
                }
                else if (vec.x > 0 && vec.y >= 0 && !topRight)
                {
                    if (straightPath(position, otherNode->position_))
                    {
                        topRight = true;
                        results.push_back(otherNode);
                    }
                }
                else if (vec.x < 0 && vec.y >= 0 && !topLeft)
                {
                    if (straightPath(position, otherNode->position_))
                    {
                        topLeft = true;
                        results.push_back(otherNode);
                    }
                }
                if (topLeft && topRight && bottomRight && bottomLeft)
                {
                    return results;
                }
            }
        }
        return results;
    }

    inline bool straightPath(f2 start, f2 goal)
    {
        return straightPath(start, goal, halfNodeSize_);
    }

    static bool straightPath(f2 start, f2 goal, vector<Rect>* obstacles)
    {
        f2 originalStart = start;
        f2 originalGoal = goal;
        f2 dir = goal - start;

        if (dir.x >= 0)
        {
            if (dir.y >= 0)
            {
                // direction is bottomRight
                for (int i = 0; i < obstacles->size(); ++i)
                {
                    f2 dir2 = (*obstacles)[i].bottomRight();
                    f2 corner = (*obstacles)[i].topLeft();
                    if (start.x <= dir2.x && start.y <= dir2.y && goal.x >= corner.x && goal.y >= corner.y)
                    {
                        if (linesIntersect(originalStart, originalGoal, (*obstacles)[i].topRight(), (*obstacles)[i].bottomLeft()))
                        {
                            return false;
                        }
                    }
                }
            }
            else
            {
                // direction is topright
                for (int i = 0; i < obstacles->size(); ++i)
                {
                    f2 dir2 = (*obstacles)[i].topRight();
                    f2 corner = (*obstacles)[i].bottomLeft();
                    if (start.x <= dir2.x && start.y >= dir2.y && goal.x >= corner.x && goal.y <= corner.y)
                    {
                        if (linesIntersect(originalStart, originalGoal, (*obstacles)[i].bottomRight(), (*obstacles)[i].topLeft()))
                        {
                            return false;
                        }
                    }
                }
            }
        }
        else
        {
            if (dir.y >= 0)
            {
                // direction is bottomLeft
                for (int i = 0; i < obstacles->size(); ++i)
                {
                    f2 dir2 = (*obstacles)[i].bottomLeft();
                    f2 corner = (*obstacles)[i].topRight();
                    if (start.x > dir2.x && start.y <= dir2.y && goal.x <= corner.x && goal.y >= corner.y)
                    {
                        if (linesIntersect(originalStart, originalGoal, (*obstacles)[i].bottomRight(), (*obstacles)[i].topLeft()))
                        {
                            return false;
                        }
                    }
                }
            }
            else
            {
                // direction is topLeft
                for (int i = 0; i < obstacles->size(); ++i)
                {
                    f2 dir2 = (*obstacles)[i].topLeft();
                    f2 corner = (*obstacles)[i].bottomRight();
                    if (start.x > dir2.x && start.y > dir2.y && goal.x <= corner.x && goal.y <= corner.y)
                    {
                        if (linesIntersect(originalStart, originalGoal, (*obstacles)[i].topRight(), (*obstacles)[i].bottomLeft()))
                        {
                            return false;
                        }
                    }
                }
            }
        }
        return true;
    }

    inline bool straightPath(f2 start, f2 goal, const f2& halfSize)
    {
        f2 originalStart = start;
        f2 originalGoal = goal;
        f2 dir = goal - start;
        f2 topLeftCorner = start - halfSize;
        f2 bottomRightCorner = start + halfSize;
        f2 topLeftCornerGoal = goal - halfSize;
        f2 bottomRightCornerGoal = goal + halfSize;

        if (dir.x >= 0)
        {
            if (dir.y >= 0)
            {
                // direction is bottomRight
                start -= halfSize;
                goal += halfSize;

                f2 start1 = start + 2 * f2(halfSize.x, 0);
                f2 start2 = start + 2 * f2(0, halfSize.y);
                f2 goal2  = goal - 2  * f2(halfSize.x, 0);
                f2 goal1  = goal - 2  * f2(0, halfSize.y);

                for (int i = 0; i < obstacles_.size(); ++i)
                {
                    f2 dir2 = obstacles_[i].bottomRight();
                    f2 corner = obstacles_[i].topLeft();
                    if (start.x <= dir2.x && start.y <= dir2.y && goal.x >= corner.x && goal.y >= corner.y)
                    {
                        if (linesIntersect(start1, goal1, obstacles_[i].topRight(), obstacles_[i].bottomLeft()) ||
                            linesIntersect(start2, goal2, obstacles_[i].topRight(), obstacles_[i].bottomLeft()) ||
                            linesIntersect(originalStart, originalGoal, obstacles_[i].topRight(), obstacles_[i].bottomLeft()) ||
                            (obstacles_[i].topLeft().x < bottomRightCorner.x && obstacles_[i].topLeft().y < bottomRightCorner.y &&
                             obstacles_[i].bottomRight().x > topLeftCorner.x && obstacles_[i].bottomRight().y > topLeftCorner.y) ||
                            (obstacles_[i].topLeft().x < bottomRightCornerGoal.x &&
                            obstacles_[i].topLeft().y < bottomRightCornerGoal.y &&
                            obstacles_[i].bottomRight().x > topLeftCornerGoal.x &&
                            obstacles_[i].bottomRight().y > topLeftCornerGoal.y))
                        {
                            return false;
                        }
                    }
                }
            }
            else
            {
                // direction is topright

                start += f2( - halfSize.x, halfSize.y);
                goal += f2(halfSize.x, - halfSize.y);

                f2 start1 = start + 2 * f2(halfSize.x, 0);
                f2 start2 = start - 2 * f2(0, halfSize.y);
                f2 goal2 = goal - 2 * f2(halfSize.x, 0);
                f2 goal1 = goal + 2 * f2(0, halfSize.y);

                for (int i = 0; i < obstacles_.size(); ++i)
                {
                    f2 dir2 = obstacles_[i].topRight();
                    f2 corner = obstacles_[i].bottomLeft();
                    if (start.x <= dir2.x && start.y >= dir2.y && goal.x >= corner.x && goal.y <= corner.y)
                    {
                        if (linesIntersect(start1, goal1, obstacles_[i].bottomRight(), obstacles_[i].topLeft()) ||
                            linesIntersect(start2, goal2, obstacles_[i].bottomRight(), obstacles_[i].topLeft()) ||
                            linesIntersect(originalStart, originalGoal, obstacles_[i].bottomRight(), obstacles_[i].topLeft()) ||
                            (obstacles_[i].topLeft().x < bottomRightCorner.x && obstacles_[i].topLeft().y < bottomRightCorner.y &&
                             obstacles_[i].bottomRight().x > topLeftCorner.x && obstacles_[i].bottomRight().y > topLeftCorner.y) ||
                            (obstacles_[i].topLeft().x < bottomRightCornerGoal.x &&
                            obstacles_[i].topLeft().y < bottomRightCornerGoal.y &&
                            obstacles_[i].bottomRight().x > topLeftCornerGoal.x &&
                            obstacles_[i].bottomRight().y > topLeftCornerGoal.y))
                        {
                            return false;
                        }
                    }
                }
            }
        }
        else
        {
            if (dir.y >= 0)
            {
                // direction is bottomLeft

                start -= f2( - halfSize.x, halfSize.y);
                goal -= f2(halfSize.x, - halfSize.y);

                f2 start1 = start - 2 * f2(halfSize.x, 0);
                f2 start2 = start + 2 * f2(0, halfSize.y);
                f2 goal2 = goal + 2 * f2(halfSize.x, 0);
                f2 goal1 = goal - 2 * f2(0, halfSize.y);

                for (int i = 0; i < obstacles_.size(); ++i)
                {
                    f2 dir2 = obstacles_[i].bottomLeft();
                    f2 corner = obstacles_[i].topRight();
                    if (start.x > dir2.x && start.y <= dir2.y && goal.x <= corner.x && goal.y >= corner.y)
                    {
                        if (linesIntersect(start1, goal1, obstacles_[i].bottomRight(), obstacles_[i].topLeft()) ||
                            linesIntersect(start2, goal2, obstacles_[i].bottomRight(), obstacles_[i].topLeft())  ||
                            linesIntersect(originalStart, originalGoal, obstacles_[i].bottomRight(), obstacles_[i].topLeft())  ||
                            (obstacles_[i].topLeft().x < bottomRightCorner.x && obstacles_[i].topLeft().y < bottomRightCorner.y &&
                             obstacles_[i].bottomRight().x > topLeftCorner.x && obstacles_[i].bottomRight().y > topLeftCorner.y) ||
                            (obstacles_[i].topLeft().x < bottomRightCornerGoal.x &&
                            obstacles_[i].topLeft().y < bottomRightCornerGoal.y &&
                            obstacles_[i].bottomRight().x > topLeftCornerGoal.x &&
                            obstacles_[i].bottomRight().y > topLeftCornerGoal.y))
                        {
                            return false;
                        }
                    }
                }
            }
            else
            {
                // direction is topLeft
                start += halfSize;
                goal -= halfSize;

                f2 start1 = start - 2 * f2(halfSize.x, 0);
                f2 start2 = start - 2 * f2(0, halfSize.y);
                f2 goal2 = goal + 2 * f2(halfSize.x, 0);
                f2 goal1 = goal + 2 * f2(0, halfSize.y);

                for (int i = 0; i < obstacles_.size(); ++i)
                {
                    f2 dir2 = obstacles_[i].topLeft();
                    f2 corner = obstacles_[i].bottomRight();
                    if (start.x > dir2.x && start.y > dir2.y && goal.x <= corner.x && goal.y <= corner.y)
                    {
                        if (linesIntersect(start1, goal1, obstacles_[i].topRight(), obstacles_[i].bottomLeft()) ||
                            linesIntersect(start2, goal2, obstacles_[i].topRight(), obstacles_[i].bottomLeft()) ||
                            linesIntersect(originalStart, originalGoal, obstacles_[i].topRight(), obstacles_[i].bottomLeft()) ||
                            (obstacles_[i].topLeft().x < bottomRightCorner.x && obstacles_[i].topLeft().y < bottomRightCorner.y &&
                             obstacles_[i].bottomRight().x > topLeftCorner.x && obstacles_[i].bottomRight().y > topLeftCorner.y) ||
                            (obstacles_[i].topLeft().x < bottomRightCornerGoal.x &&
                            obstacles_[i].topLeft().y < bottomRightCornerGoal.y &&
                            obstacles_[i].bottomRight().x > topLeftCornerGoal.x &&
                            obstacles_[i].bottomRight().y > topLeftCornerGoal.y))
                        {
                            return false;
                        }
                    }
                }
            }
        }
        return true;
    }

    // standard A* search algorithm for finding a single point
    inline vector<f2> aStarSearch(const f2& start, f2 goal)
    {
        if (straightPath(start, goal))
        {
            return {start, goal};
        }

        addNode(goal);
        for (int i = 0; i < nodesV_.size(); ++i)
        {
            if (nodes_.count(goal) > 0 && straightPath(nodesV_[i]->position_, goal))
            {
                nodesV_[i]->links_.push_back(nodes_[goal]);
            }
        }

        addNode(start);
        for (int i = 0; i < nodesV_.size(); ++i)
        {
            if (nodes_.count(start) > 0 && straightPath(nodesV_[i]->position_, start))
            {
                nodes_[start]->links_.push_back(nodesV_[i]);
            }
        }


        // i don't think this does what i wanted it to do
        if (nodes_.count(goal) > 0 && nodes_[goal]->links_.size())
        {
            float distance = - 1;
            f2 pos;

            for(int i = 0; i < nodesV_.size(); ++i)
            {
                float newLength = (nodesV_[i]->position_ - goal).lengthSquared();
                if (distance == - 1 || distance > newLength)
                {
                    distance = newLength;
                    pos = nodesV_[i]->position_;
                }
            }
            goal = pos;
        }

        PriorityQueue<f2> frontier;
        unordered_map<f2, float> costSoFar;
        unordered_map<f2, f2> cameFrom;

        frontier.put(start, 0);

        cameFrom[start] = start;
        costSoFar[start] = 0;

        int counter = 0;

        while(!frontier.empty())
        {
            counter++;

        f2 current = frontier.get();

            if (current == goal)
            {
                return reconstructPath(cameFrom, start, goal);
            }

            vector<Node*> nb;
            if (nodes_.count(current))
            {
                nb = nodes_[current]->links_;
            }
            else
            {
                nb = neighbors(current);
            }
            for (int i = 0; i < nb.size(); ++i)
            {
                f2 next = nb[i]->position_;

                float newCost = costSoFar[current] + (current - next).length();
                if (!costSoFar.count(next) || newCost < costSoFar[next])
                {
                    costSoFar[next] = newCost;
                    float priority = newCost + heuristic(next, goal);
                    frontier.put(next, priority);
                    cameFrom[next] = current;
                }
        }
        }

        float distance = - 1;
        f2 pos;

        for(auto i : cameFrom)
        {
            float newLength = (i.second - goal).lengthSquared();
            if (distance == - 1 || distance > newLength)
            {
                distance = newLength;
                pos = i.second;
            }
        }
        if (start == pos)
        {
            return {};
        }
        else
        {
            return reconstructPath(cameFrom, start, pos);
        }
    }

    // A* search algorithm modified to search for a path to any of the given goals
    inline vector<f2> aStarSearch(const f2& start, vector<Rect> goals)
    {
        int startingNodeCount = nodesV_.size();

        generateNodes(goals);

        addNode(start);
        for (int i = 0; i < nodesV_.size(); ++i)
        {
            if (nodes_.count(start) > 0 && straightPath(nodesV_[i]->position_, start))
            {
                nodes_[start]->links_.push_back(nodesV_[i]);
            }
        }

        PriorityQueue<f2> frontier;
        unordered_map<f2, float> costSoFar;
        unordered_map<f2, f2> cameFrom;

        frontier.put(start, 0);

        cameFrom[start] = start;
        costSoFar[start] = 0;

        int counter = 0;

        while(!frontier.empty())
        {
            counter++;

        f2 current = frontier.get();

        for (int i = startingNodeCount; i < nodesV_.size(); ++i)
        {
            if (current == nodesV_[i]->position_)
            {
                return reconstructPath(cameFrom, start, nodesV_[i]->position_);
            }
        }

            vector<Node*> nb;
            if (nodes_.count(current))
            {
                nb = nodes_[current]->links_;
            }
            else
            {
                nb = neighbors(current);
            }
            for (int i = 0; i < nb.size(); ++i)
            {
                f2 next = nb[i]->position_;

                float newCost = costSoFar[current];
                if (!costSoFar.count(next) || newCost < costSoFar[next])
                {
                    costSoFar[next] = newCost;
                    float priority = newCost;
                    frontier.put(next, priority);
                    cameFrom[next] = current;
                }
        }
        }
        return vector<f2>(0);
    }

    // Create the path from the cameFrom map
    inline vector<f2> reconstructPath(unordered_map<f2, f2> cameFrom, f2 start, f2 goal)
    {
        vector<f2> path;

        path.push_back(goal);

        f2 next = goal;
        for (int i = 0; i < cameFrom.size(); ++i)
        {
            next = cameFrom[next];
            path.push_back(next);
            if (next == start)
            {
                std::reverse(path.begin(), path.end());
                return path;
            }
        }
        return path;
    }

    inline void addLink(Node* node1, Node* node2)
    {
        if (straightPath(node1->position_, node2->position_, halfNodeSize_))
        {
            node1->links_.push_back(node2);
            node2->links_.push_back(node1);
        }
    }

    inline Node* addNode(const f2& position)
    {
        f2 topLeftCorner = position - halfNodeSize_;
        f2 bottomRightCorner = position + halfNodeSize_;
        // no node is created if the node is inside an obstacle
        for (int i = 0; i < obstacles_.size(); ++i)
        {
            if (obstacles_[i].topLeft().x < bottomRightCorner.x && obstacles_[i].topLeft().y < bottomRightCorner.y &&
                    obstacles_[i].bottomRight().x > topLeftCorner.x && obstacles_[i].bottomRight().y > topLeftCorner.y)
            {
                return NULL;
            }
        }

        Node* node = new Node(position);
        bool bottomRight = false;
        bool bottomLeft = false;
        bool topLeft = false;
        bool topRight = false;

        // sorts the nodes based on their distance
        // this makes sure the nodes are connected to the closest possible nodes
        // hurts performance quite a bit
        //std::sort(nodesV_.begin(), nodesV_.end(), std::bind(distanceCompare, position, _1, _2));

        for (int i = 0; i < nodesV_.size(); ++i)
        {
            Node* otherNode = nodesV_[i];

            // this first if statement controls the amount of links a node can have
            // though each node will still only generate 4 links
            if (otherNode->links_.size() < 9)
            {
                f2 vec = position - otherNode->position_;

                if (vec.x >= 0)
                {
                    if (vec.y > 0)
                    {
                        if (!topRight)
                        {
                            if (straightPath(position, otherNode->position_, halfNodeSize_))
                            {
                                topRight = true;
                                node->links_.push_back(otherNode);
                                otherNode->links_.push_back(node);
                            }
                        }
                    }
                    else
                    {
                        if (!bottomRight)
                        {
                            if (straightPath(position, otherNode->position_))
                            {
                                bottomRight = true;
                                node->links_.push_back(otherNode);
                                otherNode->links_.push_back(node);
                            }
                        }
                    }
                }
                else
                {
                    if (vec.y >= 0)
                    {
                        if (!topLeft)
                        {
                            if (straightPath(position, otherNode->position_, halfNodeSize_))
                            {
                                topLeft = true;
                                node->links_.push_back(otherNode);
                                otherNode->links_.push_back(node);
                            }
                        }
                    }
                    else
                    {
                        if (!bottomLeft)
                        {
                            if (straightPath(position, otherNode->position_, halfNodeSize_))
                            {
                                bottomLeft = true;
                                node->links_.push_back(otherNode);
                                otherNode->links_.push_back(node);
                            }
                        }
                    }
                }
            }
            if (topLeft && topRight && bottomRight && bottomLeft)
            {
                break;
            }
        }
        nodesV_.push_back(node);
        nodes_[position] = node;
        return node;
    }

    inline void removeNode(Node* node)
    {
        for (int i = 0; i < node->links_.size(); ++i)
        {
            for (int j = 0; j < node->links_[i]->links_.size(); ++j)
            {
                if (node->links_[i]->links_[j] == node)
                {
                    node->links_[i]->links_.erase(node->links_[i]->links_.begin() + j);
                    break;
                }
            }
        }

        nodesV_.erase(std::remove(nodesV_.begin(), nodesV_.end(), node), nodesV_.end());

        nodes_.erase(node->position_);

        delete(node);
    }

    inline void generateNodes(const vector<Rect>& obstacles)
    {
        for (int i = 0; i < obstacles.size(); ++i)
        {
            for (int x = - 1; x < 2; x += 2)
            {
                for (int y = - 1; y < 2; y += 2)
                {
                    addNode(obstacles[i].center() + f2(x * ((obstacles[i].width / 2) + (halfNodeSize_.x + 1)),
                                            y * ((obstacles[i].height / 2) + (halfNodeSize_.y + 1))));
                }
            }
        }
    }

    inline void update()
    {
        nodesV_.clear();
        nodes_.clear();
        // std::sort(obstacles_.begin(), obstacles_.end(), );
        generateNodes(obstacles_);
    }

    bool checkPath(vector<f2> path)
    {
        for (int i = 1; i < path.size(); ++i)
        {
            if (!straightPath(path[i - 1], path[i]))
            {
                return false;
            }
        }
        return true;
    }

f2 position_;
f2 gridSize_;
f2 nodeSize_;
f2 halfNodeSize_;
vector<Rect> obstacles_;
unordered_map<f2, Node*> nodes_;
vector<Node*> nodesV_;
};