#include "SteeringBehaviours.h"

SteeringBehaviours::SteeringBehaviours(Vehicle* agent) {
    // ctor
    m_Vehicle = agent;
    // m_Feelers.push_back(m_Vehicle->getPosition());
    CreateFeelers();
    m_WanderRadius = WanderRad;
    m_WanderDistance = WanderDist;
    m_WanderJitter = WanderJitterPerSec;

    m_Flags = 0;
    m_Deceleration = normal;
    WallAvoidanceOn();
    ObstacleAvoidanceOn();
}

SteeringBehaviours::~SteeringBehaviours() {
    // dtor
}

// calculates and sums the steering forces from any active behaviors
glm::vec2 SteeringBehaviours::Calculate() {
    // reset the steering force
    m_SteeringForce = glm ::vec2(0.0);

    // use space partitioning to calculate the neighbours of this vehicle
    // if switched on. If not, use the standard tagging system
    if (!isSpacePartitioningOn()) {
        // tag neighbors if any of the following 3 group behaviors are switched
        // on
        if (On(separation) || On(allignment) || On(cohesion)) {
            m_Vehicle->getGameWorld()
                ->flagVehiclesWithinViewRange(m_Vehicle, m_ViewDistance);
        }
    }

    m_SteeringForce = CalculatePrioritized();

    return m_SteeringForce;
}

float SteeringBehaviours::getForwardComponent() {
    return glm::dot(m_Vehicle->getHeadingVec(), m_SteeringForce);
}

float SteeringBehaviours::getSideComponent() {
    return glm::dot(m_Vehicle->getSideVec(), m_SteeringForce);
}

void SteeringBehaviours::CreateFeelers() {
    m_Feelers.clear();
    glm::vec2 head = m_Vehicle->getHeadingVec();

    // create straight ahead vector
    head.x = m_Vehicle->getPosition().x
             + m_WallDetectionFeelerLength * m_Vehicle->getHeadingVec().x;
    head.y = m_Vehicle->getPosition().y
             + m_WallDetectionFeelerLength * m_Vehicle->getHeadingVec().y;
    m_Feelers.push_back(head);

    // create feeler 30 degrees to the left of ahead
    glm::vec2 forwardLeft = m_Vehicle->getHeadingVec()
                            * glm::orientate2(330.0 * M_PI / 180);
    forwardLeft = m_Vehicle->getPosition()
                  + (forwardLeft * m_WallDetectionFeelerLength);
    m_Feelers.push_back(forwardLeft);

    // create feeler 30 degrees to the right of ahead
    glm::vec2 forwardRight = m_Vehicle->getHeadingVec()
                             * glm::orientate2(30.0 * M_PI / 180);
    forwardRight = m_Vehicle->getPosition()
                   + (forwardRight * m_WallDetectionFeelerLength);
    m_Feelers.push_back(forwardRight);
}

glm::vec2 SteeringBehaviours::WallAvoidance(
    const std::vector<WallType>& walls) {
    CreateFeelers();

    // for debugging - want to see intersect points
    m_Vehicle->getGameWorld()->currentLevel.debugIntersections.clear();

    float distToThisIP = 0.0;
    float distToClosestIP = (std::numeric_limits<float>::max)();

    glm::vec2 steeringForce(0.0f),
        intersectPoint(0.0f),  // used for storing temporary info
        closestPoint(0.0f),    // holds the closest intersection point
        tempWallZ(0.0f), closestWallNormal(.0f);

    // examine each feeler in turn
    for (auto& feeler : m_Feelers) {
        // run through each wall checking for any intersection points
        for (const WallType& wall : walls) {
            if (LineSegmentIntersections(m_Vehicle
                                             ->getPosition(),    // line origin
                                         feeler,                 // line end
                                         wall.getOriginPoint(),  // wall origin
                                         wall.getEndPoint(),
                                         intersectPoint,
                                         distToThisIP)) {
                if (wall.getOriginPoint().x == wall.getEndPoint().x) {
                    if (intersectPoint.y <= wall.getEndPoint().y
                        && intersectPoint.y >= wall.getOriginPoint().y) {
                        if (distToThisIP < distToClosestIP) {
                            // for debugging - want to see intersect points
                            m_Vehicle->getGameWorld()
                                ->currentLevel.debugIntersections.push_back(
                                    intersectPoint);
                            distToClosestIP = distToThisIP;

                            closestWallNormal = wall.getNormal();

                            closestPoint = intersectPoint;
                            break;
                        }
                    }
                } else {
                    if (intersectPoint.x <= wall.getEndPoint().x
                        && intersectPoint.x >= wall.getOriginPoint().x) {
                        if (distToThisIP < distToClosestIP) {
                            // for debugging - want to see intersect points
                            m_Vehicle->getGameWorld()
                                ->currentLevel.debugIntersections.push_back(
                                    intersectPoint);
                            distToClosestIP = distToThisIP;

                            closestWallNormal = wall.getNormal();

                            closestPoint = intersectPoint;
                            break;
                        }
                    }
                }
                //}
            }
        }  // next wall
        // if an intersection point has been detected, calculate a force
        // that will direct the agent away

        if (closestWallNormal != glm::vec2(.0f)) {
            // calculate by what distance the projected position of the
            // agent will overshoot the wall
            glm::vec2 overShoot = feeler - closestPoint;

            // create a force in the direction of the wall normal, with a
            // magnitude of the overshoot
            steeringForce = closestWallNormal
                            * glm::vec2(glm::length(
                                overShoot));  // overshoot length a scalar?
        }

    }  // next feeler

    return steeringForce;
}

glm::vec2 SteeringBehaviours::Seek(glm::vec2 targetPosition) {
    glm::vec2 targetVector = targetPosition - m_Vehicle->getPosition();
    glm::vec2 DesiredVelocity = glm::normalize(targetVector)
                                * glm::vec2(m_Vehicle->getMaxSpeed());

    return DesiredVelocity - m_Vehicle->getVelocityVec();
}

glm::vec2 SteeringBehaviours::Arrive(glm::vec2 targetPos,
                                     Deceleration deceleration) {
    glm::vec2 vecToTarget = targetPos - m_Vehicle->getPosition();

    // calculate the distance to the target
    float distance = glm::length(vecToTarget);

    if (distance > 0) {
        // because Deceleration is enumerated as an int, this value is required
        // to provide fine tweaking of the deceleration..
        const float decelerationTweaker = 0.3f;

        // calculate the speed required to reach the target given the desired
        // deceleration
        float speed = distance / ((float)deceleration * decelerationTweaker);

        // make sure the velocity does not exceed the max
        speed = std::min(speed, m_Vehicle->getMaxSpeed());

        // from here proceed just like Seek except we don't need to normalize
        // the ToTarget vector because we have already gone to the trouble
        // of calculating its length: dist.
        glm::vec2 desiredVelocity = vecToTarget
                                    * glm::vec2(speed * (1 / distance));

        return (desiredVelocity - m_Vehicle->getVelocityVec());
    }

    return glm::vec2(0.0);
}

glm::vec2 SteeringBehaviours::Wander() {
    // this behavior is dependent on the update rate, so this line must
    // be included when using time independent framerate.
    float jitterThisTimeSlice = m_WanderJitter * m_Vehicle->getElapsedTime();

    // first, add a small random vector to the target's position
    std::random_device rdevice{};
    std::default_random_engine num{rdevice()};
    std::uniform_real_distribution<float> random{-1, 1};

    m_WanderTarget += glm::vec2(random(num) * jitterThisTimeSlice,
                                random(num) * jitterThisTimeSlice);

    // reproject this new vector back on to a unit circle
    m_WanderTarget = glm::normalize(m_WanderTarget);

    // increase the length of the vector to the same as the radius
    // of the wander circle
    m_WanderTarget *= m_WanderRadius;

    // move the target into a position WanderDist in front of the agent
    glm::vec2 target = m_WanderTarget + glm::vec2(m_WanderDistance, .0f);

    // can convert target from local to world just by adding vehicles position?
    target += m_Vehicle->getPosition();

    // and steer towards it
    return target - m_Vehicle->getPosition();
}

glm::vec2 SteeringBehaviours::ObstacleAvoidance(
    const std::vector<BasicEntity*>& obstacles) {
    // the detection box length is proportional to the agent's velocity
    // should be read from a script later on
    m_DetectionBoxLength = m_minDetectionLength
                           + (m_Vehicle->getSpeed() / m_Vehicle->getMaxSpeed())
                                 * m_minDetectionLength;
    std::list<glm::vec2> aheadVectors;
    aheadVectors.push_back(m_DetectionBoxLength * m_Vehicle->getHeadingVec()
                           + m_Vehicle->getPosition());
    aheadVectors.push_back(aheadVectors.front() * 0.5f);
    aheadVectors.push_back(m_Vehicle->getHeadingVec());
    glm::vec2 closestAheadVec(.0f);
    // tag all obstacles within range of the box for processing
    m_Vehicle->getGameWorld()
        ->flagObstaclesWithinViewRange(m_Vehicle, m_DetectionBoxLength);

    // this will keep track of the closest intersecting obstacle (CIB)
    BasicEntity* closestIntersectingObstacle = NULL;

    // this will be used to track the distance to the CIB
    float distToClosestIP = (std::numeric_limits<float>::max)();

    // this will record the transformed local coordinates of the CIB
    glm::vec2 localPosOfClosestObstacle(.0f);

    for (auto currentObstacle : obstacles) {
        // if the obstacle has been tagged within range proceed
        if (currentObstacle->getFlag()) {
            // find out if its in front or behind using dot product result
            glm::vec2 vecToObstacle = currentObstacle->getPosition()
                                      - m_Vehicle->getPosition();
            float direction = glm::dot(m_Vehicle->getHeadingVec(),
                                       glm::normalize(vecToObstacle));
            if (direction >= 0) {
                // is the end of the detection vector get the vector from
                //  detectionvec to obstacle centre if vector length < obstacle
                //  radius, collision detected
                for (auto aheadVec : aheadVectors) {
                    // added radius check here
                    float distanceToObs = glm::length(
                        currentObstacle->getPosition() - aheadVec);
                    if (distanceToObs < currentObstacle->getBoundingRadius()
                                            + m_Vehicle->getBoundingRadius()) {
                        // calculate vehicles distance to obstacle and check if
                        // its closest
                        if (distanceToObs < distToClosestIP) {
                            distToClosestIP = distanceToObs;
                            closestIntersectingObstacle = currentObstacle;
                            closestAheadVec = aheadVec;
                        }
                    }
                }
            }
        }
        ++currentObstacle;
    }

    // if we have found an intersecting obstacle, calculate a steering
    // force away from it
    glm::vec2 SteeringForce(0.0f);

    if (closestIntersectingObstacle) {
        // NEED to account for bounding radius of vehicle somehwere
        // get a scalar that acts harder on the steering force as it gets closer
        // I used to include the bounding radius addition here but its stupid
        // because if the bounding radius isnt checked earlier and included then
        // it wont calculate this part float boundingCombined =
        // closestIntersectingObstacle->getBoundingRadius() +
        // m_Vehicle->getBoundingRadius();
        float multiplier = 1.0f + m_Vehicle->getBoundingRadius()
                           + (m_DetectionBoxLength - distToClosestIP)
                                 / m_DetectionBoxLength;
        glm::vec2 SteeringForceNormal = glm::normalize(
            closestAheadVec - closestIntersectingObstacle->getPosition());
        // Add bounding radius here???
        SteeringForce = SteeringForceNormal * multiplier;
    }

    return SteeringForce;
}

glm::vec2 SteeringBehaviours::Separation(
    const std::list<Vehicle*>& neighborsList) {
    glm::vec2 SteeringForce(.0f);

    for (auto neighbour : neighborsList) {
        // make sure this agent isn't included in the calculations and that
        // the agent being examined is close enough. ***also make sure it
        // doesn't include the evade target ***
        if ((neighbour != m_Vehicle) && neighbour->getFlag()
            && (neighbour != m_TargetAgent1)) {
            glm::vec2 ToAgent = m_Vehicle->getPosition()
                                - neighbour->getPosition();

            // scale the force inversely proportional to the agents distance
            // from its neighbor.
            SteeringForce += glm::normalize(ToAgent) / glm::length(ToAgent);
        }
    }

    return SteeringForce;
}

glm::vec2 SteeringBehaviours::CalculatePrioritized() {
    // function starts here
    glm::vec2 force(0.0f);

    if (On(wall_avoidance)) {
        force = WallAvoidance(
                    m_Vehicle->getGameWorld()->currentLevel.getAllWalls())
                * m_WeightWallAvoidance;

        if (!AccumulateForce(m_SteeringForce, force))
            return m_SteeringForce;
    }
    if (On(obstacle_avoidance)) {
        force = ObstacleAvoidance(
                    m_Vehicle->getGameWorld()->currentLevel.getAllObstacles())
                * m_WeightObstacleAvoidance;

        if (!AccumulateForce(m_SteeringForce, force))
            return m_SteeringForce;
    }
    if (On(separation)) {
        force = Separation(m_Vehicle->getGameWorld()->m_Vehicles)
                * m_WeightSeparation;

        if (!AccumulateForce(m_SteeringForce, force))
            return m_SteeringForce;
    }

    if (On(seek)) {
        force = Seek(m_Target) * m_WeightSeek;

        if (!AccumulateForce(m_SteeringForce, force))
            return m_SteeringForce;
    }

    if (On(arrive)) {
        force = Arrive(m_Target, m_Deceleration) * m_WeightArrive;

        if (!AccumulateForce(m_SteeringForce, force))
            return m_SteeringForce;
    }
    if (On(wander)) {
        force = Wander() * m_WeightWander;

        if (!AccumulateForce(m_SteeringForce, force))
            return m_SteeringForce;
    }
    return m_SteeringForce;
}

bool SteeringBehaviours::AccumulateForce(glm::vec2& RunningTotal,
                                         glm::vec2 ForceToAdd) {
    // calculate how much steering force the vehicle has used so far
    float MagnitudeSoFar = glm::length(RunningTotal);

    // calculate how much steering force remains to be used by this vehicle
    float MagnitudeRemaining = m_Vehicle->getMaxForce() - MagnitudeSoFar;

    // return false if there is no more force left to use
    if (MagnitudeRemaining <= 0.0)
        return false;

    // calculate the magnitude of the force we want to add
    float MagnitudeToAdd = glm::length(ForceToAdd);

    // if the magnitude of the sum of ForceToAdd and the running total
    // does not exceed the maximum force available to this vehicle, just
    // add together. Otherwise add as much of the ForceToAdd vector is
    // possible without going over the max.
    if (MagnitudeToAdd < MagnitudeRemaining) {
        RunningTotal += ForceToAdd;
    }

    else {
        // add it to the steering force
        RunningTotal += (glm::normalize(ForceToAdd)
                         * glm::vec2(MagnitudeRemaining));
    }

    return true;
}