#pragma once

#include <iostream>
#include "BasicEntity.h"
#include "Utils/WallIntersectionTests.h"
#include "Vehicle.h"
#include "WallType.h"

#include <glm/ext.hpp>  // for obstacle avoidance local to global space
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/intersect.hpp>  // for computing wall intersections for wall avoidance
#include <glm/gtx/transform.hpp>
#include <random>
// for feelers
#include <algorithm>  // std::min
#include <array>
#include <list>
#include <vector>

class Vehicle;
class WallType;

//--------------------------- Constants ----------------------------------
// the radius of the constraining circle for the wander behavior
const float WanderRad = 0.2f;
// distance the wander circle is projected in front of the agent
const float WanderDist = 0.01f;
// the maximum amount of displacement along the circle each frame
const float WanderJitterPerSec = 80.0f;
// used in path following
const float WaypointSeekDist = 20.0f;

enum summingMethod { weightedAvg, prioritized, dithered };

/**
 * @class SteeringBehaviours
 * @brief  calculates a steering vector from a entities set behaviours
 * Code is based on the textbook:
 * "Programming game AI by Example" by Matt Buckland
 *
 * @author Michael John
 * @version 0.1
 * @date 24/10/2021
 *
 * @todo
 *
 * @bug
 */

class SteeringBehaviours {
   public:
    explicit SteeringBehaviours(Vehicle* agent);

    virtual ~SteeringBehaviours();

    /**
     * Calculates a steering vector from the this objects settings
     * @return a summed vector to be applied to a vehicle object
     */
    glm::vec2 Calculate();

    /**
     * calculates the component of the steering force that is parallel
     *  with the vehicles heading
     * @return amount of steering vector that is forward
     */
    float getForwardComponent();

    /**
     * calculates the component of the steering force that is perpendicular
     *  with the vehicles heading
     * @return amount of steering vector that is forward
     */
    float getSideComponent();

    /**
     * Sets a target position for the entity
     * @param t glm::vec2 target position to be set
     */
    void setTarget(const glm::vec2 t) { m_Target = t; }

    /**
     * Toggles space partitioning on/off
     */
    void ToggleSpacePartitioningOnOff() { m_CellSpaceOn = !m_CellSpaceOn; }

    /**
     * Returns the status of cell space
     * @return true if space partitioning is on
     */
    bool isSpacePartitioningOn() const { return m_CellSpaceOn; }

    /**
     * Set the steering vector summing method
     * @param sm enumerated summing method to employ
     */
    void setSummingMethod(summingMethod sm) { m_SummingMethod = sm; }

    /**
     * set seek behaviour on
     */
    void SeekOn() { m_Flags |= seek; }

    /**
     * Set Arrive behaviour on
     */
    void ArriveOn() { m_Flags |= arrive; }

    /**
     * Sets wander behaviour on
     */
    void WanderOn() { m_Flags |= wander; }

    /**
     * Set separation behaviour on
     */
    void SeparationOn() { m_Flags |= separation; }

    /**
     * Set the obstacle avoidance behaviour on
     */
    void ObstacleAvoidanceOn() { m_Flags |= obstacle_avoidance; }

    /**
     * Set the wall avoidance behaviour on
     */
    void WallAvoidanceOn() { m_Flags |= wall_avoidance; }

    /**
     * Set the seek behaviour off
     */
    void SeekOff() {
        if (On(seek))
            m_Flags ^= seek;
    }

    /**
     * Set the arrive behaviour off
     */
    void ArriveOff() {
        if (On(arrive))
            m_Flags ^= arrive;
    }

    /**
     * Set the wander behaviour off
     */
    void WanderOff() {
        if (On(wander))
            m_Flags ^= wander;
    }

    /**
     * Set the separation behaviour off
     */
    void SeparationOff() {
        if (On(separation))
            m_Flags ^= separation;
    }

    /**
     * Set the obstacle avoidance behaviour off
     */
    void ObstacleAvoidanceOff() {
        if (On(obstacle_avoidance))
            m_Flags ^= obstacle_avoidance;
    }

    /**
     * Set the wall avoidance behaviour off
     */
    void WallAvoidanceOff() {
        if (On(wall_avoidance))
            m_Flags ^= wall_avoidance;
    }

    /**
     * Check if the Seek behaviour is on
     * @return true if seek is on
     */
    bool isSeekOn() { return On(seek); }

    /**
     * Check if the Arrive behaviour is on
     * @return true if Arrive is on
     */
    bool isArriveOn() { return On(arrive); }

    /**
     * Check if the Wander behaviour is on
     * @return true if Wander is on
     */
    bool isWanderOn() { return On(wander); }

    /**
     * Check if the Separation behaviour is on
     * @return true if Separation is on
     */
    bool isSeparationOn() { return On(separation); }

    /**
     * Check if the Obstacle Avoidance behaviour is on
     * @return true if Obstacle Avoidance is on
     */
    bool isObstacleAvoidanceOn() { return On(obstacle_avoidance); }

    /**
     * Check if the Wall Avoidance behaviour is on
     * @return true if Wall Avoidance is on
     */
    bool isWallAvoidanceOn() { return On(wall_avoidance); }

    /**
     * Get the vector of feelers used for wall detection
     * @return std::vector<glm::vec2> feelers
     */
    std::vector<glm::vec2>& getFeelers() { return m_Feelers; }

    /**
     * Gets the wander jitter used for the wander variation
     * @return float wander jitter
     */
    float WanderJitter() const { return m_WanderJitter; }

    /**
     * Gets the wander distance used for projecting the
     * wander circle in front of the bot
     * @return float distance to cast wander circle
     */
    float WanderDistance() const { return m_WanderDistance; }

    /**
     * Gets the wander radius used for setting the size of the
     * wander circle in front of the bot
     * @return float radius of the wander circle
     */
    float WanderRadius() const { return m_WanderRadius; }

    /**
     * Gets the weight used for enforcing separation
     * @return
     */
    float SeparationWeight() const { return m_WeightSeparation; }

    /**
     * Get the length of the detection box used for obstacle avoidance
     * This is the range at which obstacles will be checked if they
     * are in the bots path
     * @return float the length of the detection box
     */
    float getDBoxLength() { return m_DetectionBoxLength; }

   protected:
   private:
    Vehicle* m_Vehicle{nullptr};   /// a pointer to the owner of this instance
    glm::vec2 m_SteeringForce{0};  /// the combined steering force of behaviours
    Vehicle* m_TargetAgent1{nullptr};   /// used to track another vehicle
    Vehicle* m_TargetAgent2{nullptr};   /// used to track another vehicle
    glm::vec2 m_Target{1};              /// the current position target
    float m_DetectionBoxLength{1.f};    /// length of the detection box
    float m_minDetectionLength{0.25f};  /// detection box scales with speed,
                                        /// this enforces a minimum value
    std::vector<glm::vec2>
        m_Feelers{};  /// container for the wall detection feelers
    float m_WallDetectionFeelerLength{0.3f};  /// length of the detection
                                              /// feelers
    glm::vec2 m_WanderTarget{0};              /// the current wander target
    float m_WanderJitter{0};  /// amount of random jitter along the wander
                              /// circle
    float m_WanderRadius{0};  /// radius of the circle to apply jitter along
    float m_WanderDistance{
        0};  /// distance the wander circle is placed in front of vehicle
    float m_WeightSeparation{0.5f};  /// importance weight of separation
    float m_WeightWander{1.f};       /// importance weight of wander
    float m_WeightObstacleAvoidance{
        10.f};                          /// importance weight of obstacle avoid
    float m_WeightWallAvoidance{10.f};  /// importance weight of wall avoid
    float m_WeightSeek{10.f};           /// importance weight of seek
    float m_WeightArrive{1.f};          /// importance weight of arrive
    float m_ViewDistance{1.f};  /// how far the vehicle can see in its FOV
    int m_Flags{0};             /// contains enumerated behaviours set on/off

    enum Deceleration {
        slow = 3,
        normal = 2,
        fast = 1
    };  /// Settings of deceleration

    Deceleration m_Deceleration{slow};           /// default deceleration speed
    summingMethod m_SummingMethod{prioritized};  /// summing of steering forces
    bool m_CellSpaceOn{false};  /// is cell space partitioning on/off

    /// behaviours on/off to set m_FLags
    enum behavior_type {
        none = 0x00000,
        seek = 0x00002,
        flee = 0x00004,
        arrive = 0x00008,
        wander = 0x00010,
        cohesion = 0x00020,
        separation = 0x00040,
        allignment = 0x00080,
        obstacle_avoidance = 0x00100,
        wall_avoidance = 0x00200,
        follow_path = 0x00400,
        pursuit = 0x00800,
        evade = 0x01000,
        interpose = 0x02000,
        hide = 0x04000,
        flock = 0x08000,
        offset_pursuit = 0x10000,
    };
    bool On(behavior_type bt) {
        return (m_Flags & bt) == bt;
    }  /// returns if a byte flag is set

    bool AccumulateForce(
        glm::vec2& sf,
        glm::vec2 ForceToAdd);  /// accumulate steering forces until a maximum
    void CreateFeelers();       /// creates feeler that extend from the vehicles
                                /// position

    glm::vec2 Seek(glm::vec2 targetPosition);  /// returns a steering vector to
                                               /// the target position
    glm::vec2 Arrive(
        glm::vec2 targetPos,
        Deceleration deceleration);  /// returns a steering vector to softly
                                     /// slow to a stop on the target position
    glm::vec2 Wander();              /// returns a steering vector for wandering
    glm::vec2 ObstacleAvoidance(
        const std::vector<BasicEntity*>&
            obstacles);  /// returns a steering vector to avoid obstacles
    glm::vec2 WallAvoidance(
        const std::vector<WallType>&
            walls);  /// returns a steering vector to avoid walls
    glm::vec2 Separation(
        const std::list<Vehicle*>& agents);  /// returns a steering vector to
                                             /// separate from other vehicles
    glm::vec2 CalculatePrioritized();  /// returns a combined steering vector
                                       /// based on the priority weights of
                                       /// behaviours
};