#include "PathFollowerComponent.h"
#include "../Renderer.h"
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/glm.hpp>
#include <glm/gtx/vector_angle.hpp>
#include <cmath>
#include "../Entitys/Object.h"
#include "../core/utils/ExceptionHandler.h"

using core::types::Vec2;

PathFollowerComponent::PathFollowerComponent(Object *owner)
    : Component(owner) {}

void PathFollowerComponent::AddTarget(const Vec2 &point)
{
    m_targets.push_back(point);
}

void PathFollowerComponent::ClearPath()
{
    m_targets.clear();
    m_currentTarget = 0;
}

float PathFollowerComponent::Ease(float t) const
{
    switch (m_easing)
    {
    case EasingFunction::EaseInOut:
        return t * t * (3 - 2 * t);
    case EasingFunction::EaseOutCubic:
        return 1 - std::pow(1 - t, 3);
    default:
        return t;
    }
}

float PathFollowerComponent::GetCornerSharpness(const Vec2 &a, const Vec2 &b, const Vec2 &c) const
{
    glm::vec2 d1 = glm::normalize(glm::vec2(b - a));
    glm::vec2 d2 = glm::normalize(glm::vec2(c - b));
    float angle = glm::degrees(glm::angle(d1, d2));
    return glm::clamp(angle / 180.0f, 0.1f, 1.0f);
}




void PathFollowerComponent::Update(float dt)
{
    if (m_targets.empty())
        return;

    if (!m_initialized) {
        m_currentPosition = owner->GetLocalPosition();
        m_initialized = true;
    }

    Vec2 target = m_targets[m_currentTarget];
    Vec2 delta  = target - m_currentPosition;
    float dist  = delta.Length();

    constexpr int m_arrivalRadius = 10;

    if (dist < m_arrivalRadius) {
        switch (m_pathMode) {
            case PathMode::Loop:
                m_currentTarget = (m_currentTarget + 1) % m_targets.size();
                break;

            case PathMode::PingPong:
                if (m_forward) {
                    if (m_currentTarget + 1 < m_targets.size()) m_currentTarget++;
                    else { m_forward = false; m_currentTarget--; }
                } else {
                    if (m_currentTarget > 0) m_currentTarget--;
                    else { m_forward = true; m_currentTarget++; }
                }
                break;

            case PathMode::Stop:
                if (m_currentTarget + 1 < m_targets.size()) m_currentTarget++;
                else return; // done
                break;
        }
        return;
    }

    Vec2 direction = delta / dist;

    float cornerFactor = 1.0f;
    if (m_currentTarget > 0 && m_currentTarget + 1 < m_targets.size()) {
        Vec2 inDir  = (m_targets[m_currentTarget]     - m_targets[m_currentTarget - 1]).Normalized();
        Vec2 outDir = (m_targets[m_currentTarget + 1] - m_targets[m_currentTarget]).Normalized();

        float dot = std::clamp(inDir.Dot(outDir), -1.0f, 1.0f);
        float angle = std::acos(dot);
        cornerFactor = 1.0f - (angle / static_cast<float>(PI));
        cornerFactor = std::clamp(cornerFactor, 0.0f, 1.0f);
    }

    // apply easing and speed
    float eased   = Ease(cornerFactor);
    float v       = eased * m_speed;

    // move and write back
    m_currentPosition += direction * v * dt;
    owner->SetLocalPosition(m_currentPosition);
}





void PathFollowerComponent::Save(YAML::Emitter &out) const
{
    out << YAML::BeginMap;
    out << YAML::Key << "type" << YAML::Value << GetName();
    out << YAML::Key << "speed" << YAML::Value << m_speed;
    out << YAML::Key << "easing" << YAML::Value << static_cast<int>(m_easing);
    out << YAML::Key << "mode" << YAML::Value << static_cast<int>(m_pathMode);


    out << YAML::Key << "targets" << YAML::Value << YAML::BeginSeq;
    for (const auto &pt : m_targets)
    {
        out << YAML::BeginMap;
        out << YAML::Key << "x" << YAML::Value << pt.x;
        out << YAML::Key << "y" << YAML::Value << pt.y;
        out << YAML::EndMap;
    }
    out << YAML::EndSeq;
    out << YAML::EndMap;
}

void PathFollowerComponent::Load(const YAML::Node &node)
{
    try
    {
        m_speed = node["speed"] ? node["speed"].as<float>() : 100.0f;
        m_easing = node["easing"] ? static_cast<EasingFunction>(node["easing"].as<int>()) : EasingFunction::Linear;
        if (node["mode"])
            m_pathMode = static_cast<PathMode>(node["mode"].as<int>());

        m_targets.clear();

        if (node["targets"])
        {
            for (const auto &pt : node["targets"])
            {
                float x = pt["x"] ? pt["x"].as<float>() : 0.0f;
                float y = pt["y"] ? pt["y"].as<float>() : 0.0f;
                m_targets.emplace_back(x, y);
            }
        }

        m_currentTarget = 0;
        m_initialized = false;
    }
    catch (const std::exception &e)
    {
        RecoverableError("Failed to load PathFollowerComponent: " + std::string(e.what()), Create::Exceptions::ComponentLoad).Handle();
    }
}
