#include "GameWorld.h"
#include "Utils/WallIntersectionTests.h"

GameWorld::~GameWorld() {
    for (auto vehicle : m_Vehicles) {
        delete vehicle;
    }
    for (auto projectile : m_Projectiles) {
        delete projectile;
    }
}

void GameWorld::init(gameMap& map) {
    auto wallPoints{map.getWallPoints()};
    auto entities{map.getBoundingSpheres()};
    auto sizes{map.getRoomSizes()};
    std::size_t currentRoom{0};

    currentLevel.setNumOfRooms(wallPoints.size());

    for (auto& room : wallPoints) {
        Room newRoom;
        newRoom.centre = (sizes[currentRoom].first + sizes[currentRoom].second)
                         / 2.f;
        newRoom.min.x = sizes[currentRoom].first.x;
        newRoom.min.y = sizes[currentRoom].first.z;
        newRoom.max.x = sizes[currentRoom].second.x;
        newRoom.max.y = sizes[currentRoom].second.z;

        for (auto& wall : room) {
            WallType newWall(glm::vec2(wall[0].x, wall[0].z),
                             glm::vec2(wall[1].x, wall[1].z),
                             glm::vec2(wall[2].x, wall[2].z));
            newRoom.walls.push_back(newWall);
        }

        for (auto& obstacle : entities[currentRoom]) {
            auto newEntity = new BasicEntity(-1,
                                             glm::vec2(obstacle.first.x,
                                                       obstacle.first.z),
                                             glm::vec2(obstacle.second),
                                             obstacle.second);
            newRoom.obstacles.push_back(newEntity);
        }
        currentLevel.addRoom(newRoom);
        ++currentRoom;
    }

    auto wallPointsCorridor{map.getCorridorWallPoints()};
    for (auto& corridor : wallPointsCorridor) {
        Room newRoom;
        newRoom.centre = (sizes[currentRoom].first + sizes[currentRoom].second)
                         / 2.f;
        newRoom.min.x = sizes[currentRoom].first.x;
        newRoom.min.y = sizes[currentRoom].first.z;
        newRoom.max.x = sizes[currentRoom].second.x;
        newRoom.max.y = sizes[currentRoom].second.z;

        for (auto& wall : corridor) {
            WallType newWall(glm::vec2(wall[0].x, wall[0].z),
                             glm::vec2(wall[1].x, wall[1].z),
                             glm::vec2(wall[2].x, wall[2].z));
            newRoom.walls.push_back(newWall);
        }
        currentLevel.addRoom(newRoom);
        ++currentRoom;
    }

    currentLevel.updateAllWalls();
    currentLevel.updateAllEntities();
}

void GameWorld::Update(float elapsedTime) {
    auto vehicleItr = m_Vehicles.begin();
    while (vehicleItr != m_Vehicles.end()) {
        auto vehicle = *vehicleItr;
        // Don't delete player on death
        if ((!currentLevel.isInBounds(vehicle->getPosition())
             || vehicle->isDead())
            && vehicle != m_Vehicles.front()) {
            m_deadVehicles.push_back(vehicle->getID());
            m_Vehicles.erase(vehicleItr++);
        } else {
            vehicle->Update(elapsedTime);
            ++vehicleItr;
        }
    }

    auto projectileItr = m_Projectiles.begin();
    while (projectileItr != m_Projectiles.end()) {
        auto projectile = *projectileItr;
        projectile->update(elapsedTime);
        if (!currentLevel.isInBounds(projectile->getPosition())
            || projectile->isDead()) {
            m_deadProjectiles.emplace_back(projectile->getID(),
                                           glm::normalize(
                                               projectile->getOrigin()
                                               - projectile->getPosition()));
            m_Projectiles.erase(projectileItr++);
        } else {
            ++projectileItr;
        }
    }
}

void GameWorld::flagVehiclesWithinViewRange(BasicEntity* vehicle, float range) {
    flagNeighbors(vehicle, m_Vehicles, range);
}

void GameWorld::flagObstaclesWithinViewRange(BasicEntity* vehicle,
                                             float range) {
    auto obstacles = currentLevel.getAllObstacles();
    flagNeighbors(vehicle, obstacles, range);
}

glm::vec2 GameWorld::getClosestWallIntersection(glm::vec2 origin,
                                                glm::vec2 direction,
                                                float& distance) {
    float distToClosestIP = (std::numeric_limits<float>::max)();
    glm::vec2 point(0.0),   // used for storing temporary info
        closestPoint(0.0),  // holds the closest intersection point
        tempWallZ(0.0);
    auto walls = currentLevel.getAllWalls();

    for (auto& wall : walls) {
        if (glm::intersectRayPlane(origin,                 // line origin
                                   direction,              // line end
                                   wall.getOriginPoint(),  // wall origin
                                   wall.getNormal(),
                                   distance)) {
            if (distance < distToClosestIP) {
                closestPoint = origin + (glm::vec2(distance) * direction);
            }
        }
    }
    return closestPoint;
}

bool GameWorld::playerMoveCheck(glm::vec2& position) {
    if (!doWallsObstructLineSegment(m_Vehicles.front()->getPosition(),
                                    position,
                                    currentLevel.getAllWalls())) {
        return false;
    }

    auto& room = currentLevel.getNearestRoom(position);
    for (auto& obstacle : room.obstacles) {
        float distance = glm::abs(
            glm::distance(position, obstacle->getPosition()));
        if (distance < obstacle->getBoundingRadius()) {
            return false;
        }
    }
    return true;
}

//---------------------------- isLOSOkay --------------------------------------
//
//  returns true if the ray between A and B is unobstructed.
//------------------------------------------------------------------------------
bool GameWorld::isLOSOkay(glm::vec2 A, glm::vec2 B) const {
    // return !doWallsObstructLineSegment(A, B, currentLevel.getAllWalls());

    // return true;

    auto& room = currentLevel.getNearestRoom(A);
    auto& room2 = currentLevel.getNearestRoom(B);
    if (&room == &room2) {
        // Throw away variables
        glm::vec2 a, b, c, d;
        for (auto& obstacle : room.obstacles) {
            if (glm::intersectLineSphere(A,
                                         B,
                                         obstacle->getPosition(),
                                         obstacle->getBoundingRadius(),
                                         a,
                                         b,
                                         c,
                                         d)) {
                return false;
            }
        }
        return true;
    }
    return false;
}

//------------------------- isPathObstructed ----------------------------------
//
//  returns true if a bot cannot move from A to B without bumping into
//  world geometry. It achieves this by stepping from A to B in steps of
//  size BoundingRadius and testing for intersection with world geometry at
//  each point.
//-----------------------------------------------------------------------------
bool GameWorld::isPathObstructed(glm::vec2 A,
                                 glm::vec2 B,
                                 float BoundingRadius) const {
    glm::vec2 ToB = glm::normalize(B - A);
    glm::vec2 curPos = A;
    /*
        glm::vec2 right = glm::vec2(-ToB.y, ToB.x) * glm::vec2(0.5f);
        glm::vec2 left = glm::vec2(-ToB.y, ToB.x) * glm::vec2(-0.5f);

        glm::vec2 rightA = right + A;
        glm::vec2 leftA = left + A;

        glm::vec2 rightB = right + B;
        glm::vec2 leftB = left + B;


        float distance;
        glm::vec2 normal;

        for(auto obs: m_Obstacles){
            if(glm::intersectRaySphere(rightA,
                                           ToB,
                                         obs->getPosition(),
                                         obs->getBoundingRadius(),
                                         distance)){
                return true;
            }
            if(glm::intersectRaySphere(leftA,
                                           ToB,
                                           obs->getPosition(),
                                           obs->getBoundingRadius(),
                                           distance)){
                return true;
            }
        }
        for(auto bot: m_Vehicles){

            if(glm::intersectRaySphere(rightA,
                                           ToB,
                                           bot->getPosition(),
                                           bot->getBoundingRadius(),
                                           distance)){
                    return true;
                }
            if(glm::intersectRaySphere(leftA,
                                           ToB,
                                           bot->getPosition(),
                                           bot->getBoundingRadius(),
                                           distance)){
                return true;
            }
        }
    */
    while (glm::length(curPos - B) > BoundingRadius) {
        // advance curPos one step
        curPos += ToB * glm::vec2(0.5f * BoundingRadius);

        // test all walls against the new position
        if (doWallsIntersectCircle(currentLevel.getAllWalls(),
                                   curPos,
                                   BoundingRadius)) {
            return true;
        }
    }

    return false;
}

//----------------------------- GetAllBotsInFOV ------------------------------
//
//  returns a vector of pointers to bots within the given bot's field of view
//-----------------------------------------------------------------------------
std::vector<Vehicle*> GameWorld::GetAllBotsInFOV(const Vehicle* Bot) const {
    std::vector<Vehicle*> VisibleBots;

    // std::list<Vehicle*>::const_iterator curBot = m_Bots.begin();
    for (auto curBot : m_Vehicles)
    // for (curBot; curBot != m_Bots.end(); ++curBot)
    {
        // make sure time is not wasted checking against the same bot or against
        // a
        //  bot that is dead or re-spawning
        if (curBot == Bot || !curBot->isAlive())
            continue;

        // first of all test to see if this bot is within the FOV
        if (isSecondInFOVOfFirst(Bot->getPosition(),
                                 Bot->getFacingVec(),
                                 curBot->getPosition(),
                                 Bot->getFieldOfView(),
                                 Bot->getViewDistance())) {
            // cast a ray from between the bots to test visibility. If the bot
            // is visible add it to the vector
            if (!doWallsObstructLineSegment(Bot->getPosition(),
                                            curBot->getPosition(),
                                            currentLevel.getAllWalls())) {
                VisibleBots.push_back(curBot);
            }
        }
    }

    return VisibleBots;
}

//---------------------------- isSecondVisibleToFirst -------------------------

bool GameWorld::isSecondVisibleToFirst(const Vehicle* First,
                                       const Vehicle* Second) const {
    // if the two bots are equal or if one of them is not alive return false
    if (!(First == Second) && Second->isAlive()) {
        // first of all test to see if this bot is within the FOV
        if (isSecondInFOVOfFirst(First->getPosition(),
                                 First->getFacingVec(),
                                 Second->getPosition(),
                                 First->getFieldOfView(),
                                 First->getViewDistance())) {
            // test the line segment connecting the bot's positions against the
            // walls. If the bot is visible add it to the vector
            if (!doWallsObstructLineSegment(First->getPosition(),
                                            Second->getPosition(),
                                            currentLevel.getAllWalls())) {
                return true;
            }
        }
    }

    return false;
}

bool GameWorld::isSecondInFOVOfFirst(glm::vec2 posFirst,
                                     glm::vec2 facingFirst,
                                     glm::vec2 posSecond,
                                     float fov,
                                     float viewRange) const {
    glm::vec2 toTarget = posSecond - posFirst;
    float rangeToTarget = glm::length(toTarget);
    glm::vec2 toTargetNormal = glm::normalize(toTarget);

    return glm::dot(facingFirst, toTargetNormal) >= cos(fov / 2.0)
           && rangeToTarget < viewRange;
}