//
// Created by trotfunky on 06/06/19.
//

#include "Level.h"


Level::Level(const pugi::xml_document& xmlDoc, const TextureStore& textureStore)
    : textures(textureStore),
      size(xmlDoc.child("Level").attribute("w").as_int(),xmlDoc.child("Level").attribute("h").as_int())
{
    pugi::xml_node levelNode = xmlDoc.child("Level");
    for(const pugi::xml_node& child : levelNode.children())
    {
        if(!strncmp(child.name(),"Entity",6))
        {
            entities.emplace_back(child,textures.at(child.attribute("textureId").as_int(0)).get());

            // Initialize the occupied squares vector with the new entity's squares
            std::vector<pro_maat::GridPos> entitySquares = entities.rbegin()->getOccupiedSquares();
            std::move(entitySquares.begin(),entitySquares.end(),std::back_inserter(occupiedSquares));
        }
    }
}

void Level::render(sf::RenderWindow& renderWindow) const
{
    for(const Entity& entity : entities)
    {
        renderWindow.draw(entity.getShape());
    }
}

void Level::runStep()
{
    std::vector<pro_maat::GridPos> newOccupiedSquares{};
    newOccupiedSquares.reserve(occupiedSquares.size());

    for(Entity& entity: entities)
    {
        entity.update();
        int heuristicSign = 0;
        switch (entity.getState())
        {
            case State::Moving:
            {
                heuristicSign = 1;
            }
            case State::Fleeing:
            {
                if(heuristicSign == 0)
                {
                    heuristicSign = -1;
                }
                if(entity.target != entity.getPosition())
                {
                    entity.move(findPath(entity.getPosition(),entity.target,heuristicSign));
                }
                break;
            }
            case State::Waiting:break;
            case State::Idle:break;
        }

        std::vector<pro_maat::GridPos> entitySquares = entity.getOccupiedSquares();

        // FIXME : Very heavy memory usage and a lot of duplicates, slows down occupiedSquares.find() calls too.
        // Copy the squares to the currently occupied squares : prevents moving into an entity that just moved
        occupiedSquares.insert(occupiedSquares.end(),entitySquares.begin(),entitySquares.end());
        std::sort(occupiedSquares.begin(),occupiedSquares.end());

        // Moves the occupied squares from the entity to the new occupied squares vector
        std::move(entitySquares.begin(),entitySquares.end(),std::back_inserter(newOccupiedSquares));
    }

    occupiedSquares.swap(newOccupiedSquares);
    // Sort the vector as to get O(ln(n)) complexity when searching for a square
    std::sort(occupiedSquares.begin(),occupiedSquares.end());
}

Orientation Level::findPath(pro_maat::GridPos start, pro_maat::GridPos goal, int sign)
{
    std::set<pro_maat::GridPos> openNodes{start};
    std::set<pro_maat::GridPos> closedNodes{};

    std::map<pro_maat::GridPos,float> estimatedCosts{{start,pro_maat::manhattanDistance(start,goal)*sign}};
    std::map<pro_maat::GridPos,float> pathCosts{{start,0}};
    std::map<pro_maat::GridPos,pro_maat::GridPos> paths{};


    const std::vector<pro_maat::GridPos> goalNeighbours = pro_maat::getNeighbours(goal,size);
    // Save the iterators : vector is const and .begin() and .end() might get called a lot
    auto goalNeighboursBeginIterator = goalNeighbours.begin();
    auto goalNeighboursEndIterator = goalNeighbours.end();


    // FIXME : Find an efficient way to get rid of openNodes.find calls
    // Lambda checking if the current element is also in the open nodes set
    auto compWithOpen = [&openNodes](const std::pair<pro_maat::GridPos,float>& leftHandSide,
                                     const std::pair<pro_maat::GridPos,float>& rightHandSide){
        if(openNodes.find(leftHandSide.first) == openNodes.end())
        {
            return (false);
        }
        else if(openNodes.find(rightHandSide.first) == openNodes.end())
        {
            return (true);
        }
        else
        {
            return (leftHandSide.second < rightHandSide.second);
        }
    };


    while(!openNodes.empty())
    {
        // Expand from the open node with the smallest estimated cost
        pro_maat::GridPos currentNode = std::min_element(estimatedCosts.begin(),estimatedCosts.end(),compWithOpen)->first;

        if(std::find(goalNeighboursBeginIterator,goalNeighboursEndIterator,currentNode) != goalNeighboursEndIterator)
        {
            if(currentNode == start)
            {
                return(Orientation::None);
            }

            // Trace back to the start
            pro_maat::GridPos& previousNode = currentNode;
            for(;paths[previousNode]!=start;previousNode = paths[previousNode])
            {}

            pro_maat::GridUnit dx = previousNode.first - start.first;
            pro_maat::GridUnit dy = previousNode.second - start.second;

            if(dx < 0)
            {
                return(Orientation::West);
            }
            else if(dx > 0)
            {
                return(Orientation::East);
            }
            else if(dy < 0)
            {
                return(Orientation::North);
            }
            else if(dy > 0 )
            {
                return(Orientation::South);
            }
            else
            {
                return(Orientation::None);
            }
        }

        openNodes.erase(currentNode);
        closedNodes.insert(currentNode);

        for(pro_maat::GridPos neighbour : pro_maat::getNeighbours(currentNode,size))
        {
            // Checks if node has been closed or is an obstacle
            if(std::find(occupiedSquares.begin(),occupiedSquares.end(),neighbour) != occupiedSquares.end() ||
               closedNodes.find(neighbour) != closedNodes.end())
            {
                continue;
            }

            // As the neighbours are adjacent squares, distance from them is always +-1
            float newPathCost = pathCosts[currentNode] + sign;

            if(openNodes.find(neighbour) == openNodes.end())
            {
                openNodes.insert(neighbour);
            }
            else if(newPathCost >= pathCosts[neighbour]) // If the node is open but this path is longer, ignore it
            {
                continue;
            }

            pathCosts.insert_or_assign(neighbour,newPathCost);
            estimatedCosts.insert_or_assign(neighbour,newPathCost + pro_maat::manhattanDistance(neighbour,goal));
            auto returnPathIt = paths.insert_or_assign(neighbour,currentNode);
        }
    }

    // If we did not find a path, do not move
    return Orientation::None;
}