#ifndef VCORE_CLIENTLESS_FLIGHT_PATH_H
#define VCORE_CLIENTLESS_FLIGHT_PATH_H
#include <algorithm>
#include <array>
#include <cmath>
#include <functional>
#include <limits>
#include <vector>
// Geometry-only receding-horizon navigation. Server adapters supply phased
// terrain/ceiling and static+dynamic collision queries. No Player shortcuts.
namespace ClientlessFlightPath
{
    struct Point
    {
        float X = 0, Y = 0, Z = 0;
    };
    struct Surface
    {
        float Ground = 0, Ceiling = 0;
        bool Valid = false;
    };
    struct Settings
    {
        float Clearance = 60, Safety = 8, Radius = 2;
        float Step = 25, Lookahead = 100, Slope = 0.6f;
    };
    struct Memory
    {
        float LowerTerrainDistance = 0;
        float HeadingX = 1, HeadingY = 0;
        bool Departed = false;
    };
    struct Result
    {
        Point Next;
        bool Valid = false, Avoiding = false, Landing = false;
    };
    using Sample = std::function < Surface(float, float) >;
    using Ray = std::function < bool(Point const &, Point const &) >;
    inline float Horizontal(Point const & a, Point const & b)
    {
        return std::hypot(b.X - a.X, b.Y - a.Y);
    }
    inline float Distance(Point const & a, Point const & b)
    {
        return std::hypot(Horizontal(a, b), b.Z - a.Z);
    }
    inline Point Mix(Point const & a, Point const & b, float t)
    {
        return
        {
            a.X +(b.X - a.X) * t, a.Y +(b.Y - a.Y) * t, a.Z +(b.Z - a.Z) * t
        };
    }
    inline bool Corridor(Point const & from, Point const & to, Point const & goal, Settings const & cfg, Sample const & sample, Ray const & ray, bool departure = false)
    {
        int const count = std::max(1, int(std::ceil(Distance(from, to) / 5)));
        Surface const origin = sample(from.X, from.Y);
        if (! origin.Valid) return false;
        for (int i = 0; i <= count; ++ i)
        {
            Point p = Mix(from, to, float(i) / count);
            // Only takeoff/actual target touchdown may approach the surface. This
            // allowance follows the real rising/descending slope, not arbitrary terrain.
            float margin = cfg.Safety;
            if (departure) margin = std::min(margin, std::max(0.0f, from.Z - origin.Ground) + Horizontal(from, p) * std::min(0.3f, std::max(0.0f, to.Z - from.Z) / std::max(1.0f, Horizontal(from, to)) * 0.75f));
            margin = std::min(margin, std::max(0.0f, Horizontal(p, goal) * 0.2f));
            Surface s = sample(p.X, p.Y);
            if (! s.Valid || ! std::isfinite(s.Ground) || ! std::isfinite(s.Ceiling) || p.Z + 0.5f < s.Ground + margin || p.Z + 5 > s.Ceiling) return false;
        }
        // A volume, not one center ray. Keep the lower rays above foot level so
        // a valid ground departure/touchdown does not intersect the ground plane.
        for (auto offset : std::array < Point, 5 >
        {
            {
                {
                    0, 0, 0
                },
                {
                    cfg.Radius, 0, 0
                },
                {
                    - cfg.Radius, 0, 0
                },
                {
                    0, cfg.Radius, 0
                },
                {
                    0, - cfg.Radius, 0
                }
            }
        }) for (float height :
        {
            0.8f, 4.5f
        })
        {
            Point a
            {
                from.X + offset.X, from.Y + offset.Y, from.Z + height
            };
            Point b
            {
                to.X + offset.X, to.Y + offset.Y, to.Z + height
            };
            if (! ray(a, b)) return false;
        }
        return true;
    }
    inline Result Next(Point const & from, Point const & goal, Settings const & cfg, Memory & memory, Sample const & sample, Ray const & ray)
    {
        float const distance = Horizontal(from, goal);
        Surface const here = sample(from.X, from.Y), end = sample(goal.X, goal.Y);
        if (! here.Valid || ! end.Valid || std::fabs(end.Ground - goal.Z) > 2.5f) return
        {
        };
        // A short, low final descent need not open another horizontal approach.
        // Validate the entire landing volume, including obstacles and terrain.
        if (memory.Departed && from.Z>=goal.Z && from.Z-goal.Z<=12 &&
            Distance(from,goal)<=cfg.Step && Corridor(from,goal,goal,cfg,sample,ray,false))
            return {goal,true,false,true};
        if (distance < 2 && std::fabs(from.Z - goal.Z) < 1.5f) return
        {
            goal, Corridor(from, goal, goal, cfg, sample, ray, true), false, true
        };
        float ux = distance > 0.1f ?(goal.X - from.X) / distance : memory.HeadingX;
        float uy = distance > 0.1f ?(goal.Y - from.Y) / distance : memory.HeadingY;
        if (distance > 0.1f)
        {
            memory.HeadingX = ux;
            memory.HeadingY = uy;
        }
        std::array < float, 5 > heights;
        float ceiling = here.Ceiling;
        for (unsigned i = 0; i < heights.size(); ++ i)
        {
            float d = std::min(distance, float(i) * cfg.Lookahead / 4);
            Surface s = sample(from.X + ux * d, from.Y + uy * d);
            if (! s.Valid) return
            {
            };
            heights[i] = s.Ground;
            ceiling = std::min(ceiling, s.Ceiling);
        }
        std::sort(heights.begin(), heights.end());
        float desired = heights[2] + cfg.Clearance;
        // ignore short radical peaks in nominal height
        bool const approach = distance < std::max(150.0f,(from.Z - goal.Z) / cfg.Slope + cfg.Step);
        if (approach) desired = goal.Z + distance * cfg.Slope * 0.65f;
        if (! approach && desired < from.Z - 10)
        {
            memory.LowerTerrainDistance += cfg.Step;
            if (memory.LowerTerrainDistance < 150) desired = from.Z;
        }
        else memory.LowerTerrainDistance = 0;
        float const horizon = std::min(cfg.Lookahead, std::max(cfg.Step, distance));
        bool const departure = ! memory.Departed;
        if (departure) desired = std::max(desired, here.Ground + 20);
        desired = std::min(desired, ceiling - 6);
        Result best;
        float bestScore = std::numeric_limits < float >::infinity();
        // Gentle nominal height tracking is separate from obstacle-climbing
        // candidates. Upward avoidance can exceed nominal clearance, never ceiling.
        for (float angle :
        {
            0.0f, 0.45f, - 0.45f, 0.9f, - 0.9f, 1.35f, - 1.35f
        }) for (float extraRise :
        {
            0.0f, - 1.0f, 25.0f, 55.0f
        })
        {
            float dx = ux * std::cos(angle) - uy * std::sin(angle);
            float dy = uy * std::cos(angle) + ux * std::sin(angle);
            float dz = std::clamp(desired - from.Z, - horizon * cfg.Slope, horizon * cfg.Slope);
            if (extraRise < 0) dz = 0;
            if (extraRise > 0) dz = std::min(horizon * cfg.Slope, std::max(0.0f, dz) + extraRise);
            float run = std::min(horizon, distance);
            // If an avoidance climb left us high near the destination, first
            // open the approach instead of descending vertically onto the goal.
            if (approach && distance < 2 *(from.Z - goal.Z) / cfg.Slope && extraRise == 0)
            {
                dz = - std::min(horizon * cfg.Slope * 0.7f, std::max(0.0f, from.Z - goal.Z));
                if (distance <(from.Z - goal.Z) / cfg.Slope * 0.7f)
                {
                    dx = - dx;
                    dy = - dy;
                    run = horizon;
                    dz = std::min(0.0f, std::max(dz, here.Ground + cfg.Safety + 4 - from.Z));
                }
            }
            Point far
            {
                from.X + dx * run, from.Y + dy * run, from.Z + dz
            };
            if (! departure && approach && angle == 0 && extraRise == 0 && distance <= horizon && std::fabs(from.Z - goal.Z) <= distance * cfg.Slope) far = goal;
            if (departure && far.Z <= from.Z + 0.5f) continue;
            if (Distance(from, far) < 0.5f ||(Horizontal(from, far) < 1 && std::fabs(far.Z - from.Z) > 2)) continue;
            if (! Corridor(from, far, goal, cfg, sample, ray, departure)) continue;
            float score = Distance(far, goal) + run + std::fabs(angle) * 20 + std::max(0.0f, extraRise) * 0.4f + std::fabs(far.Z - desired) * 0.5f;
            bool const openApproach = approach && angle == 0 && extraRise == 0;
            if (!openApproach && score >= bestScore) continue;
            float t = std::min(1.0f, cfg.Step / std::max(1.0f, Horizontal(from, far)));
            best =
            {
                Mix(from, far, t), true, angle != 0 || extraRise > 0, Distance(far, goal) < 0.1f && t == 1
            };
            bestScore = score;
            // Follow a clear nominal approach immediately. Level-flight scoring
            // must not postpone descent until directly above the destination.
            if (openApproach)
            {
                memory.Departed = true;
                return best;
            }
        }
        if (best.Valid) memory.Departed = true;
        return best;
    }
    struct SharedTrail
    {
        std::vector<Point> Points;
        bool Aborted=false;
    };
    struct SharedResult {Result Step;bool Waiting=false;};
    inline SharedResult FollowTrail(Point const& current,Point const& goal,Settings const& settings,
        SharedTrail const& trail,std::size_t& index,Sample const& sample,Ray const& ray)
    {
        if(trail.Aborted)return {};
        while(index<trail.Points.size() && Distance(current,trail.Points[index])<0.7f)++index;
        if(index==trail.Points.size())return {{},true};
        auto next=trail.Points[index];
        return {{next,Corridor(current,next,goal,settings,sample,ray,true),false,Distance(next,goal)<0.7f},false};
    }

}

#endif