/*
 * Copyright (C) 2012-2017 CypherCore <http://github.com/CypherCore>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

using Framework.Constants;
using Framework.GameMath;
using Game.Entities;
using Game.Maps;
using System;
using System.Linq;

namespace Game.Movement
{
    public class PathGenerator
    {
        public PathGenerator(Unit owner)
        {
            _polyLength = 0;
            pathType = PathType.Blank;
            _useStraightPath = false;
            _forceDestination = false;
            _pointPathLimit = 74;
            _endPosition = Vector3.Zero;
            _sourceUnit = owner;
            _navMesh = null;
            _navMeshQuery = null;
            Log.outDebug(LogFilter.Maps, "PathGenerator:PathGenerator for {0}", _sourceUnit.GetGUID().ToString());

            uint mapId = _sourceUnit.GetMapId();
            if (Global.DisableMgr.IsPathfindingEnabled(mapId))
            {
                _navMesh = Global.MMapMgr.GetNavMesh(mapId, _sourceUnit.GetTerrainSwaps());
                _navMeshQuery = Global.MMapMgr.GetNavMeshQuery(mapId, _sourceUnit.GetInstanceId(), _sourceUnit.GetTerrainSwaps());
            }
            CreateFilter();
        }

        public bool CalculatePath(float destX, float destY, float destZ, bool forceDest = false, bool straightLine = false)
        {
            float x, y, z;
            _sourceUnit.GetPosition(out x, out y, out z);

            if (!GridDefines.IsValidMapCoord(destX, destY, destZ) || !GridDefines.IsValidMapCoord(x, y, z))
                return false;

            Vector3 dest = new Vector3(destX, destY, destZ);
            SetEndPosition(dest);

            Vector3 start = new Vector3(x, y, z);
            SetStartPosition(start);

            _forceDestination = forceDest;
            _straightLine = straightLine;

            Log.outDebug(LogFilter.Maps, "PathGenerator.CalculatePath() for {0} \n", _sourceUnit.GetGUID().ToString());

            // make sure navMesh works - we can run on map w/o mmap
            // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
            if (_navMesh == null || _navMeshQuery == null || _sourceUnit.HasUnitState(UnitState.IgnorePathfinding)
                || !HaveTile(start) || !HaveTile(dest))
            {
                BuildShortcut();
                pathType = PathType.Normal | PathType.NotUsingPath;
                return true;
            }

            UpdateFilter();
            BuildPolyPath(start, dest);
            return true;
        }

        ulong GetPathPolyByPosition(ulong[] polyPath, uint polyPathSize, float[] point, ref float distance)
        {
            if (polyPath == null || polyPathSize == 0)
                return 0;

            ulong nearestPoly = 0;
            float minDist2d = float.MaxValue;
            float minDist3d = 0.0f;

            for (uint i = 0; i < polyPathSize; ++i)
            {
                float[] closestPoint = new float[3];
                bool posOverPoly = false;
                if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPoly(polyPath[i], point, closestPoint, ref posOverPoly)))
                    continue;

                float d = Detour.dtVdist2DSqr(point, closestPoint);
                if (d < minDist2d)
                {
                    minDist2d = d;
                    nearestPoly = polyPath[i];
                    minDist3d = Detour.dtVdistSqr(point, closestPoint);
                }

                if (minDist2d < 1.0f) // shortcut out - close enough for us
                    break;
            }

            distance = (float)Math.Sqrt(minDist3d);

            return (minDist2d < 3.0f) ? nearestPoly : 0u;
        }

        ulong GetPolyByLocation(float[] point, ref float distance)
        {
            // first we check the current path
            // if the current path doesn't contain the current poly,
            // we need to use the expensive navMesh.findNearestPoly
            ulong polyRef = GetPathPolyByPosition(_pathPolyRefs, _polyLength, point, ref distance);
            if (polyRef != 0)
                return polyRef;

            // we don't have it in our old path
            // try to get it by findNearestPoly()
            // first try with low search box
            float[] extents = { 3.0f, 5.0f, 3.0f };    // bounds of poly search area
            float[] closestPoint = { 0.0f, 0.0f, 0.0f };
            if (Detour.dtStatusSucceed(_navMeshQuery.findNearestPoly(point, extents, _filter, ref polyRef, ref closestPoint)) && polyRef != 0)
            {
                distance = Detour.dtVdist(closestPoint, point);
                return polyRef;
            }

            // still nothing ..
            // try with bigger search box
            // Note that the extent should not overlap more than 128 polygons in the navmesh (see dtNavMeshQuery.findNearestPoly)
            extents[1] = 50.0f;
            if (Detour.dtStatusSucceed(_navMeshQuery.findNearestPoly(point, extents, _filter, ref polyRef, ref closestPoint)) && polyRef != 0)
            {
                distance = Detour.dtVdist(closestPoint, point);
                return polyRef;
            }

            return 0;
        }

        void BuildPolyPath(Vector3 startPos, Vector3 endPos)
        {
            // *** getting start/end poly logic ***

            float distToStartPoly = 0;
            float distToEndPoly = 0;
            float[] startPoint = { startPos.Y, startPos.Z, startPos.X };
            float[] endPoint = { endPos.Y, endPos.Z, endPos.X };

            ulong startPoly = GetPolyByLocation(startPoint, ref distToStartPoly);
            ulong endPoly = GetPolyByLocation(endPoint, ref distToEndPoly);

            // we have a hole in our mesh
            // make shortcut path and mark it as NOPATH ( with flying and swimming exception )
            // its up to caller how he will use this info
            if (startPoly == 0 || endPoly == 0)
            {
                Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . (startPoly == 0 || endPoly == 0)\n");
                BuildShortcut();
                bool path = _sourceUnit.IsTypeId(TypeId.Unit) && _sourceUnit.ToCreature().CanFly();

                bool waterPath = _sourceUnit.IsTypeId(TypeId.Unit) && _sourceUnit.ToCreature().CanSwim();
                if (waterPath)
                {
                    // Check both start and end points, if they're both in water, then we can *safely* let the creature move
                    for (uint i = 0; i < _pathPoints.Length; ++i)
                    {
                        ZLiquidStatus status = _sourceUnit.GetMap().getLiquidStatus(_pathPoints[i].X, _pathPoints[i].Y, _pathPoints[i].Z, MapConst.MapAllLiquidTypes);
                        // One of the points is not in the water, cancel movement.
                        if (status == ZLiquidStatus.NoWater)
                        {
                            waterPath = false;
                            break;
                        }
                    }
                }

                pathType = (path || waterPath) ? (PathType.Normal | PathType.NotUsingPath) : PathType.NoPath;
                return;
            }

            // we may need a better number here
            bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f);
            if (farFromPoly)
            {
                Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . farFromPoly distToStartPoly={0:F3} distToEndPoly={1:F3}\n", distToStartPoly, distToEndPoly);

                bool buildShotrcut = false;
                if (_sourceUnit.IsTypeId(TypeId.Unit))
                {
                    Creature owner = _sourceUnit.ToCreature();

                    Vector3 p = (distToStartPoly > 7.0f) ? startPos : endPos;
                    if (_sourceUnit.GetMap().IsUnderWater(p.X, p.Y, p.Z))
                    {
                        Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . underWater case\n");
                        if (owner.CanSwim())
                            buildShotrcut = true;
                    }
                    else
                    {
                        Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . flying case\n");
                        if (owner.CanFly())
                            buildShotrcut = true;
                    }
                }

                if (buildShotrcut)
                {
                    BuildShortcut();
                    pathType = (PathType.Normal | PathType.NotUsingPath);
                    return;
                }
                else
                {
                    float[] closestPoint = new float[3];
                    // we may want to use closestPointOnPolyBoundary instead
                    bool posOverPoly = false;
                    if (Detour.dtStatusSucceed(_navMeshQuery.closestPointOnPoly(endPoly, endPoint, closestPoint, ref posOverPoly)))
                    {
                        Detour.dtVcopy(endPoint, closestPoint);
                        SetActualEndPosition(new Vector3(endPoint[2], endPoint[0], endPoint[1]));
                    }

                    pathType = PathType.Incomplete;
                }
            }

            // *** poly path generating logic ***

            // start and end are on same polygon
            // just need to move in straight line
            if (startPoly == endPoly)
            {
                Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . (startPoly == endPoly)\n");

                BuildShortcut();

                _pathPolyRefs[0] = startPoly;
                _polyLength = 1;

                pathType = farFromPoly ? PathType.Incomplete : PathType.Normal;
                Log.outDebug(LogFilter.Maps, "BuildPolyPath . path type {0}\n", pathType);
                return;
            }

            // look for startPoly/endPoly in current path
            /// @todo we can merge it with getPathPolyByPosition() loop
            bool startPolyFound = false;
            bool endPolyFound = false;
            uint pathStartIndex = 0;
            uint pathEndIndex = 0;

            if (_polyLength != 0)
            {
                for (; pathStartIndex < _polyLength; ++pathStartIndex)
                {
                    // here to carch few bugs
                    if (_pathPolyRefs[pathStartIndex] == 0)
                    {
                        Log.outError(LogFilter.Maps, "Invalid poly ref in BuildPolyPath. _polyLength: {0}, pathStartIndex: {1}," +
                            " startPos: {2}, endPos: {3}, mapid: {4}", _polyLength, pathStartIndex, startPos, endPos, _sourceUnit.GetMapId());
                        break;
                    }

                    if (_pathPolyRefs[pathStartIndex] == startPoly)
                    {
                        startPolyFound = true;
                        break;
                    }
                }

                for (pathEndIndex = _polyLength - 1; pathEndIndex > pathStartIndex; --pathEndIndex)
                    if (_pathPolyRefs[pathEndIndex] == endPoly)
                    {
                        endPolyFound = true;
                        break;
                    }
            }

            if (startPolyFound && endPolyFound)
            {
                Log.outDebug(LogFilter.Maps, "BuildPolyPath : (startPolyFound && endPolyFound)\n");

                // we moved along the path and the target did not move out of our old poly-path
                // our path is a simple subpath case, we have all the data we need
                // just "cut" it out

                _polyLength = pathEndIndex - pathStartIndex + 1;
                Array.Copy(_pathPolyRefs, pathStartIndex, _pathPolyRefs, 0, _polyLength);
            }
            else if (startPolyFound && !endPolyFound)
            {
                Log.outDebug(LogFilter.Maps, "BuildPolyPath : (startPolyFound && !endPolyFound)\n");

                // we are moving on the old path but target moved out
                // so we have atleast part of poly-path ready

                _polyLength -= pathStartIndex;

                // try to adjust the suffix of the path instead of recalculating entire length
                // at given interval the target cannot get too far from its last location
                // thus we have less poly to cover
                // sub-path of optimal path is optimal

                // take ~80% of the original length
                /// @todo play with the values here
                uint prefixPolyLength = (uint)(_polyLength * 0.8f + 0.5f);
                Array.Copy(_pathPolyRefs, pathStartIndex, _pathPolyRefs, 0, prefixPolyLength);

                ulong suffixStartPoly = _pathPolyRefs[prefixPolyLength - 1];

                // we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data
                float[] suffixEndPoint = new float[3];
                bool posOverPoly = false;
                if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, ref posOverPoly)))
                {
                    // we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
                    // try to recover by using prev polyref
                    --prefixPolyLength;
                    suffixStartPoly = _pathPolyRefs[prefixPolyLength - 1];
                    if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, ref posOverPoly)))
                    {
                        // suffixStartPoly is still invalid, error state
                        BuildShortcut();
                        pathType = PathType.NoPath;
                        return;
                    }
                }

                // generate suffix
                uint suffixPolyLength = 0;

                uint dtResult;
                if (_straightLine)
                {
                    float hit = 0;
                    float[] hitNormal = new float[3];

                    dtResult = _navMeshQuery.raycast(
                        suffixStartPoly,
                        suffixEndPoint,
                        endPoint,
                        _filter,
                        ref hit,
                        hitNormal,
                        _pathPolyRefs,
                        ref suffixPolyLength,
                        74 - (int)prefixPolyLength);

                    // raycast() sets hit to FLT_MAX if there is a ray between start and end
                    if (hit != float.MaxValue)
                    {
                        // the ray hit something, return no path instead of the incomplete one
                        pathType = PathType.NoPath;
                        return;
                    }
                }
                else
                {
                    dtResult = _navMeshQuery.findPath(
                        suffixStartPoly,    // start polygon
                        endPoly,            // end polygon
                        suffixEndPoint,     // start position
                        endPoint,           // end position
                        _filter,            // polygon search filter
                        _pathPolyRefs,
                        ref suffixPolyLength,
                        74 - (int)prefixPolyLength);
                }

                if (suffixPolyLength == 0 || Detour.dtStatusFailed(dtResult))
                {
                    // this is probably an error state, but we'll leave it
                    // and hopefully recover on the next Update
                    // we still need to copy our preffix
                    Log.outError(LogFilter.Maps, "{0}'s Path Build failed: 0 length path", _sourceUnit.GetGUID().ToString());
                }

                Log.outDebug(LogFilter.Maps, "m_polyLength={0} prefixPolyLength={1} suffixPolyLength={2} \n", _polyLength, prefixPolyLength, suffixPolyLength);

                // new path = prefix + suffix - overlap
                _polyLength = prefixPolyLength + suffixPolyLength - 1;
            }
            else
            {
                Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . (!startPolyFound && !endPolyFound)\n");

                // either we have no path at all . first run
                // or something went really wrong . we aren't moving along the path to the target
                // just generate new path

                // free and invalidate old path data
                Clear();

                uint dtResult;
                if (_straightLine)
                {
                    float hit = 0;
                    float[] hitNormal = new float[3];

                    dtResult = _navMeshQuery.raycast(
                                    startPoly,
                                    startPoint,
                                    endPoint,
                                    _filter,
                                    ref hit,
                                    hitNormal,
                                    _pathPolyRefs,
                                    ref _polyLength,
                                    74);

                    // raycast() sets hit to FLT_MAX if there is a ray between start and end
                    if (hit != float.MaxValue)
                    {
                        // the ray hit something, return no path instead of the incomplete one
                        pathType = PathType.NoPath;
                        return;
                    }
                }
                else
                {
                    dtResult = _navMeshQuery.findPath(
                        startPoly,          // start polygon
                        endPoly,            // end polygon
                        startPoint,         // start position
                        endPoint,           // end position
                        _filter,           // polygon search filter
                        _pathPolyRefs,     // [out] path
                        ref _polyLength,
                        74);   // max number of polygons in output path
                }

                if (_polyLength == 0 || Detour.dtStatusFailed(dtResult))
                {
                    // only happens if we passed bad data to findPath(), or navmesh is messed up
                    Log.outError(LogFilter.Maps, "{0}'s Path Build failed: 0 length path", _sourceUnit.GetGUID().ToString());
                    BuildShortcut();
                    pathType = PathType.NoPath;
                    return;
                }
            }

            // by now we know what type of path we can get
            if (_pathPolyRefs[_polyLength - 1] == endPoly && !pathType.HasAnyFlag(PathType.Incomplete))
                pathType = PathType.Normal;
            else
                pathType = PathType.Incomplete;

            // generate the point-path out of our up-to-date poly-path
            BuildPointPath(startPoint, endPoint);
        }

        void BuildPointPath(float[] startPoint, float[] endPoint)
        {
            float[] pathPoints = new float[74 * 3];
            int pointCount = 0;
            uint dtResult = Detour.DT_FAILURE;

            if (_straightLine)
            {
                dtResult = Detour.DT_SUCCESS;
                pointCount = 1;
                Array.Copy(startPoint, pathPoints, 3); // first point

                // path has to be split into polygons with dist SMOOTH_PATH_STEP_SIZE between them
                Vector3 startVec = new Vector3(startPoint[0], startPoint[1], startPoint[2]);
                Vector3 endVec = new Vector3(endPoint[0], endPoint[1], endPoint[2]);
                Vector3 diffVec = (endVec - startVec);
                Vector3 prevVec = startVec;
                float len = diffVec.GetLength();
                diffVec *= 4.0f / len;
                while (len > 4.0f)
                {
                    len -= 4.0f;
                    prevVec += diffVec;
                    pathPoints[3 * pointCount + 0] = prevVec.X;
                    pathPoints[3 * pointCount + 1] = prevVec.Y;
                    pathPoints[3 * pointCount + 2] = prevVec.Z;
                    ++pointCount;
                }

                Array.Copy(endPoint, 0, pathPoints, 3 * pointCount, 3); // last point
                ++pointCount;
            }
            else if (_useStraightPath)
            {
                dtResult = _navMeshQuery.findStraightPath(
                    startPoint,         // start position
                    endPoint,           // end position
                    _pathPolyRefs,
                    (int)_polyLength,
                    pathPoints,         // [out] path corner points
                    null,               // [out] flags
                    null,               // [out] shortened path
                    ref pointCount,
                    (int)_pointPathLimit,
                    0);   // maximum number of points/polygons to use
            }
            else
            {
                dtResult = FindSmoothPath(
                    startPoint,         // start position
                    endPoint,           // end position
                    _pathPolyRefs,     // current path
                    _polyLength,       // length of current path
                    out pathPoints,         // [out] path corner points
                    out pointCount,
                    _pointPathLimit);    // maximum number of points
            }

            if (pointCount < 2 || Detour.dtStatusFailed(dtResult))
            {
                // only happens if pass bad data to findStraightPath or navmesh is broken
                // single point paths can be generated here
                /// @todo check the exact cases
                Log.outDebug(LogFilter.Maps, "++ PathGenerator.BuildPointPath FAILED! path sized {0} returned\n", pointCount);
                BuildShortcut();
                pathType = PathType.NoPath;
                return;
            }
            else if (pointCount == _pointPathLimit)
            {
                Log.outDebug(LogFilter.Maps, "++ PathGenerator.BuildPointPath FAILED! path sized {0} returned, lower than limit set to {1}\n", pointCount, _pointPathLimit);
                BuildShortcut();
                pathType = PathType.Short;
                return;
            }

            _pathPoints = new Vector3[pointCount];
            for (uint i = 0; i < pointCount; ++i)
                _pathPoints[i] = new Vector3(pathPoints[i * 3 + 2], pathPoints[i * 3], pathPoints[i * 3 + 1]);

            NormalizePath();

            // first point is always our current location - we need the next one
            SetActualEndPosition(_pathPoints[pointCount - 1]);

            // force the given destination, if needed
            if (_forceDestination && (!pathType.HasAnyFlag(PathType.Normal) || !InRange(GetEndPosition(), GetActualEndPosition(), 1.0f, 1.0f)))
            {
                // we may want to keep partial subpath
                if (Dist3DSqr(GetActualEndPosition(), GetEndPosition()) < 0.3f * Dist3DSqr(GetStartPosition(), GetEndPosition()))
                {
                    SetActualEndPosition(GetEndPosition());
                    _pathPoints[_pathPoints.Length - 1] = GetEndPosition();
                }
                else
                {
                    SetActualEndPosition(GetEndPosition());
                    BuildShortcut();
                }

                pathType = (PathType.Normal | PathType.NotUsingPath);
            }
            Log.outDebug(LogFilter.Maps, "PathGenerator.BuildPointPath path type {0} size {1} poly-size {2}\n", pathType, pointCount, _polyLength);
        }

        uint FixupCorridor(ulong[] path, uint npath, uint maxPath, ulong[] visited, int nvisited)
        {
            int furthestPath = -1;
            int furthestVisited = -1;

            // Find furthest common polygon.
            for (int i = (int)npath - 1; i >= 0; --i)
            {
                bool found = false;
                for (int j = (int)nvisited - 1; j >= 0; --j)
                {
                    if (path[i] == visited[j])
                    {
                        furthestPath = i;
                        furthestVisited = j;
                        found = true;
                    }
                }
                if (found)
                    break;
            }

            // If no intersection found just return current path.
            if (furthestPath == -1 || furthestVisited == -1)
                return npath;

            // Concatenate paths.

            // Adjust beginning of the buffer to include the visited.
            uint req = (uint)(nvisited - furthestVisited);
            uint orig = (uint)((furthestPath + 1) < npath ? furthestPath + 1 : (int)npath);
            uint size = npath > orig ? npath - orig : 0;
            if (req + size > maxPath)
                size = maxPath - req;

            if (size != 0)
                Array.Copy(path, (int)orig, path, (int)req, (int)size);

            // Store visited
            for (uint i = 0; i < req; ++i)
                path[i] = visited[(nvisited - 1) - i];

            return req + size;
        }

        bool GetSteerTarget(float[] startPos, float[] endPos, float minTargetDist, ulong[] path, uint pathSize, out float[] steerPos, out Detour.dtStraightPathFlags steerPosFlag, out ulong steerPosRef)
        {
            steerPosRef = 0;
            steerPos = new float[3];
            steerPosFlag = 0;

            // Find steer target.
            float[] steerPath = new float[3 * 3];
            byte[] steerPathFlags = new byte[3];
            ulong[] steerPathPolys = new ulong[3];
            int nsteerPath = 0;
            uint dtResult = _navMeshQuery.findStraightPath(startPos, endPos, path, (int)pathSize, steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, 3, 0);
            if (nsteerPath == 0 || Detour.dtStatusFailed(dtResult))
                return false;

            // Find vertex far enough to steer to.
            uint ns = 0;
            while (ns < nsteerPath)
            {
                // Stop at Off-Mesh link or when point is further than slop away.
                if ((steerPathFlags[ns].HasAnyFlag((byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
                    !InRangeYZX(steerPath.Skip((int)ns * 3).ToArray(), startPos, minTargetDist, 1000.0f)))
                    break;
                ns++;
            }
            // Failed to find good point to steer to.
            if (ns >= nsteerPath)
                return false;

            Detour.dtVcopy(steerPos, 0, steerPath, (int)ns * 3);
            steerPos[1] = startPos[1];  // keep Z value
            steerPosFlag = (Detour.dtStraightPathFlags)steerPathFlags[ns];
            steerPosRef = steerPathPolys[ns];

            return true;
        }

        uint FindSmoothPath(float[] startPos, float[] endPos, ulong[] polyPath, uint polyPathSize, out float[] smoothPath, out int smoothPathSize, uint maxSmoothPathSize)
        {
            smoothPathSize = 0;
            int nsmoothPath = 0;
            smoothPath = new float[74 * 3];

            ulong[] polys = new ulong[74];
            Array.Copy(polyPath, polys, polyPathSize);
            uint npolys = polyPathSize;

            float[] iterPos = new float[3];
            float[] targetPos = new float[3];
            if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPolyBoundary(polys[0], startPos, iterPos)))
                return Detour.DT_FAILURE;

            if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPolyBoundary(polys[npolys - 1], endPos, targetPos)))
                return Detour.DT_FAILURE;

            Detour.dtVcopy(smoothPath, nsmoothPath * 3, iterPos, 0);
            nsmoothPath++;

            // Move towards target a small advancement at a time until target reached or
            // when ran out of memory to store the path.
            while (npolys != 0 && nsmoothPath < maxSmoothPathSize)
            {
                // Find location to steer towards.
                float[] steerPos;
                Detour.dtStraightPathFlags steerPosFlag;
                ulong steerPosRef = 0;

                if (!GetSteerTarget(iterPos, targetPos, 0.3f, polys, npolys, out steerPos, out steerPosFlag, out steerPosRef))
                    break;

                bool endOfPath = steerPosFlag.HasAnyFlag(Detour.dtStraightPathFlags.DT_STRAIGHTPATH_END);
                bool offMeshConnection = steerPosFlag.HasAnyFlag(Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION);

                // Find movement delta.
                float[] delta = new float[3];
                Detour.dtVsub(delta, steerPos, iterPos);
                float len = (float)Math.Sqrt(Detour.dtVdot(delta, delta));
                // If the steer target is end of path or off-mesh link, do not move past the location.
                if ((endOfPath || offMeshConnection) && len < 4.0f)
                    len = 1.0f;
                else
                    len = 4.0f / len;

                float[] moveTgt = new float[3];
                Detour.dtVmad(moveTgt, iterPos, delta, len);

                // Move
                float[] result = new float[3];
                int MAX_VISIT_POLY = 16;
                ulong[] visited = new ulong[MAX_VISIT_POLY];

                int nvisited = 0;
                _navMeshQuery.moveAlongSurface(polys[0], iterPos, moveTgt, _filter, result, visited, ref nvisited, MAX_VISIT_POLY);
                npolys = FixupCorridor(polys, npolys, 74, visited, nvisited);

                _navMeshQuery.getPolyHeight(polys[0], result, ref result[1]);
                result[1] += 0.5f;
                Detour.dtVcopy(iterPos, result);

                // Handle end of path and off-mesh links when close enough.
                if (endOfPath && InRangeYZX(iterPos, steerPos, 0.3f, 1.0f))
                {
                    // Reached end of path.
                    Detour.dtVcopy(iterPos, targetPos);
                    if (nsmoothPath < maxSmoothPathSize)
                    {
                        Detour.dtVcopy(smoothPath, nsmoothPath * 3, iterPos, 0);
                        nsmoothPath++;
                    }
                    break;
                }
                else if (offMeshConnection && InRangeYZX(iterPos, steerPos, 0.3f, 1.0f))
                {
                    // Advance the path up to and over the off-mesh connection.
                    ulong prevRef = 0;
                    ulong polyRef = polys[0];
                    uint npos = 0;
                    while (npos < npolys && polyRef != steerPosRef)
                    {
                        prevRef = polyRef;
                        polyRef = polys[npos];
                        npos++;
                    }

                    for (uint i = npos; i < npolys; ++i)
                        polys[i - npos] = polys[i];

                    npolys -= npos;

                    // Handle the connection.
                    float[] connectionStartPos = new float[3];
                    float[] connectionEndPos = new float[3];
                    if (Detour.dtStatusSucceed(_navMesh.getOffMeshConnectionPolyEndPoints(prevRef, polyRef, connectionStartPos, connectionEndPos)))
                    {
                        if (nsmoothPath < maxSmoothPathSize)
                        {
                            Detour.dtVcopy(smoothPath, nsmoothPath * 3, connectionStartPos, 0);
                            nsmoothPath++;
                        }
                        // Move position at the other side of the off-mesh link.
                        Detour.dtVcopy(iterPos, connectionEndPos);
                        _navMeshQuery.getPolyHeight(polys[0], iterPos, ref iterPos[1]);
                        iterPos[1] += 0.5f;
                    }
                }

                // Store results.
                if (nsmoothPath < maxSmoothPathSize)
                {
                    Detour.dtVcopy(smoothPath, nsmoothPath * 3, iterPos, 0);
                    nsmoothPath++;
                }
            }

            smoothPathSize = nsmoothPath;

            // this is most likely a loop
            return nsmoothPath < 74 ? Detour.DT_SUCCESS : Detour.DT_FAILURE;
        }

        void NormalizePath()
        {
            for (uint i = 0; i < _pathPoints.Length; ++i)
                _sourceUnit.UpdateAllowedPositionZ(_pathPoints[i].X, _pathPoints[i].Y, ref _pathPoints[i].Z);
        }

        void BuildShortcut()
        {
            Log.outDebug(LogFilter.Maps, "BuildShortcut : making shortcut\n");

            Clear();

            // make two point path, our curr pos is the start, and dest is the end
            _pathPoints = new Vector3[2];

            // set start and a default next position
            _pathPoints[0] = GetStartPosition();
            _pathPoints[1] = GetActualEndPosition();

            NormalizePath();

            pathType = PathType.Shortcut;
        }

        void CreateFilter()
        {
            NavTerrain includeFlags = 0;
            NavTerrain excludeFlags = 0;

            if (_sourceUnit.IsTypeId(TypeId.Unit))
            {
                Creature creature = _sourceUnit.ToCreature();
                if (creature.CanWalk())
                    includeFlags |= NavTerrain.Ground;

                if (creature.CanSwim())
                    includeFlags |= (NavTerrain.Water | NavTerrain.Magma | NavTerrain.Slime);
            }
            else
                includeFlags = (NavTerrain.Ground | NavTerrain.Water | NavTerrain.Magma | NavTerrain.Slime);

            _filter.setIncludeFlags((ushort)includeFlags);
            _filter.setExcludeFlags((ushort)excludeFlags);

            UpdateFilter();
        }

        void UpdateFilter()
        {
            // allow creatures to cheat and use different movement types if they are moved
            // forcefully into terrain they can't normally move in
            if (_sourceUnit.IsInWater() || _sourceUnit.IsUnderWater())
            {
                NavTerrain includedFlags = (NavTerrain)_filter.getIncludeFlags();
                includedFlags |= GetNavTerrain(_sourceUnit.GetPositionX(), _sourceUnit.GetPositionY(), _sourceUnit.GetPositionZ());

                _filter.setIncludeFlags((ushort)includedFlags);
            }
        }

        NavTerrain GetNavTerrain(float x, float y, float z)
        {
            LiquidData data;
            ZLiquidStatus liquidStatus = _sourceUnit.GetMap().getLiquidStatus(x, y, z, MapConst.MapAllLiquidTypes, out data);
            if (liquidStatus == ZLiquidStatus.NoWater)
                return NavTerrain.Ground;

            switch (data.type_flags)
            {
                case MapConst.MapLiquidTypeWater:
                case MapConst.MapLiquidTypeOcean:
                    return NavTerrain.Water;
                case MapConst.MapLiquidTypeMagma:
                    return NavTerrain.Magma;
                case MapConst.MapLiquidTypeSlime:
                    return NavTerrain.Slime;
                default:
                    return NavTerrain.Ground;
            }
        }

        bool InRange(Vector3 p1, Vector3 p2, float r, float h)
        {
            Vector3 d = p1 - p2;
            return (d.X * d.X + d.Y * d.Y) < r * r && Math.Abs(d.Z) < h;
        }

        float Dist3DSqr(Vector3 p1, Vector3 p2)
        {
            return (p1 - p2).GetLengthSquared();
        }

        public void ReducePathLenghtByDist(float dist)
        {
            if (GetPathType() == PathType.Blank)
            {
                Log.outError(LogFilter.Maps, "PathGenerator.ReducePathLenghtByDist called before path was built");
                return;
            }

            if (_pathPoints.Length < 2) // path building failure
                return;

            int i = _pathPoints.Length;
            Vector3 nextVec = _pathPoints[--i];
            while (i > 0)
            {
                Vector3 currVec = _pathPoints[--i];
                Vector3 diffVec = (nextVec - currVec);
                float len = diffVec.GetLength();
                if (len > dist)
                {
                    float step = dist / len;
                    // same as nextVec
                    _pathPoints[i + 1] -= diffVec * step;
                    _sourceUnit.UpdateAllowedPositionZ(_pathPoints[i + 1].X, _pathPoints[i + 1].Y, ref _pathPoints[i + 1].Z);
                    Array.Resize(ref _pathPoints, i + 2);
                    break;
                }
                else if (i == 0) // at second point
                {
                    _pathPoints[1] = _pathPoints[0];
                    Array.Resize(ref _pathPoints, 2);
                    break;
                }

                dist -= len;
                nextVec = currVec; // we're going backwards
            }
        }

        public bool IsInvalidDestinationZ(Unit target)
        {
            return (target.GetPositionZ() - GetActualEndPosition().Z) > 5.0f;
        }

        void Clear()
        {
            _polyLength = 0;
            _pathPoints = null;
        }

        bool HaveTile(Vector3 p)
        {
            int tx = -1, ty = -1;
            float[] point = { p.Y, p.Z, p.X };

            _navMesh.calcTileLoc(point, ref tx, ref ty);

            /// Workaround
            /// For some reason, often the tx and ty variables wont get a valid value
            /// Use this check to prevent getting negative tile coords and crashing on getTileAt
            if (tx < 0 || ty < 0)
                return false;

            return (_navMesh.getTileAt(tx, ty, 0) != null);
        }

        bool InRangeYZX(float[] v1, float[] v2, float r, float h)
        {
            float dx = v2[0] - v1[0];
            float dy = v2[1] - v1[1]; // elevation
            float dz = v2[2] - v1[2];
            return (dx * dx + dz * dz) < r * r && Math.Abs(dy) < h;
        }

        public Vector3 GetStartPosition() { return _startPosition; }
        public Vector3 GetEndPosition() { return _endPosition; }
        public Vector3 GetActualEndPosition() { return _actualEndPosition; }

        public Vector3[] GetPath()
        {
            return _pathPoints;
        }

        public PathType GetPathType() { return pathType; }

        void SetStartPosition(Vector3 point) { _startPosition = point; }
        void SetEndPosition(Vector3 point) { _actualEndPosition = point; _endPosition = point; }
        void SetActualEndPosition(Vector3 point) { _actualEndPosition = point; }

        public void SetUseStraightPath(bool useStraightPath) { _useStraightPath = useStraightPath; }

        public void SetPathLengthLimit(float distance) { _pointPathLimit = Math.Min((uint)(distance / 4.0f), 74); }

        ulong[] _pathPolyRefs = new ulong[74];

        uint _polyLength;
        uint _pointPathLimit;
        bool _straightLine;     // use raycast if true for a straight line path
        Unit _sourceUnit;
        bool _forceDestination;
        bool _useStraightPath;
        Vector3[] _pathPoints;

        Vector3 _actualEndPosition;
        Vector3 _startPosition;
        Vector3 _endPosition;
        PathType pathType;

        Detour.dtQueryFilter _filter = new Detour.dtQueryFilter();
        Detour.dtNavMeshQuery _navMeshQuery;
        Detour.dtNavMesh _navMesh;
    }

    public enum PathType
    {
        Blank = 0x00,   // path not built yet
        Normal = 0x01,   // normal path
        Shortcut = 0x02,   // travel through obstacles, terrain, air, etc (old behavior)
        Incomplete = 0x04,   // we have partial path to follow - getting closer to target
        NoPath = 0x08,   // no valid path at all or error in generating one
        NotUsingPath = 0x10,   // used when we are either flying/swiming or on map w/o mmaps
        Short = 0x20,   // path is longer or equal to its limited path length
    }
    public enum NavTerrain
    {
        Empty = 0x00,
        Ground = 0x01,
        Magma = 0x02,
        Slime = 0x04,
        Water = 0x08,
        Unused1 = 0x10,
        Unused2 = 0x20,
        Unused3 = 0x40,
        Unused4 = 0x80
        // we only have 8 bits
    }

    public enum PolyFlag
    {
        Walk = 1,
        Swim = 2
    }
}
