class PathNode
{
    int x;
    int y;

    int fScore;
    int gScore;

    PathNode cameFrom;

    PathNode Init(int x, int y)
    {
        self.x = x;
        self.y = y;
        gScore = 2147483647;
        fScore = 2147483647;
        cameFrom = null;
        return self;
    }
}

class PathfindingHandler : EventHandler
{
    const RESOLUTION = 32;
    const CNT_X = 65536/RESOLUTION;
    const CNT_Y = 65536/RESOLUTION;

    Sector SBlockMap[CNT_Y][CNT_X];

    private void MakeNull(int x1, int y1, int x2, int y2)
    {
        int x,y,dx,dy,dx1,dy1,px,py,xe,ye,i;
        dx=x2-x1;
        dy=y2-y1;
        dx1=abs(dx);
        dy1=abs(dy);
        px=2*dy1-dx1;
        py=2*dx1-dy1;
        if(dy1<=dx1)
        {
            if(dx>=0)
            {
                x=x1;
                y=y1;
                xe=x2;
            }
            else
            {
                x=x2;
                y=y2;
                xe=x1;
            }
            SBlockMap[y][x] = null;
            for(i=0;x<xe;i++)
            {
                x=x+1;
                if(px<0)
                {
                    px=px+2*dy1;
                }
                else
                {
                    if((dx<0 && dy<0) || (dx>0 && dy>0))
                    {
                        y=y+1;
                    }
                    else
                    {
                        y=y-1;
                    }
                    px=px+2*(dy1-dx1);
                }
                SBlockMap[y][x] = null;
            }
        }
        else
        {
            if(dy>=0)
            {
                x=x1;
                y=y1;
                ye=y2;
            }
            else
            {
                x=x2;
                y=y2;
                ye=y1;
            }
            SBlockMap[y][x] = null;
            for(i=0;y<ye;i++)
            {
                y=y+1;
                if(py<=0)
                {
                    py=py+2*dx1;
                }
                else
                {
                    if((dx<0 && dy<0) || (dx>0 && dy>0))
                    {
                        x=x+1;
                    }
                    else
                    {
                        x=x-1;
                    }
                    py=py+2*(dx1-dy1);
                }
                SBlockMap[y][x] = null;
            }
        }
    }

    vector2 RelToAbs(vector2 rel)
    {
        return (rel.x + 32768, rel.y + 32768);
    }

    vector2 AbsToRel(vector2 abs)
    {
        return (abs.x - 32768, abs.y - 32768);
    }

    override void WorldLoaded(WorldEvent ev)
    {
        for (int y = 0; y < 65536; y += RESOLUTION)
        {
            for (int x = 0; x < 65536; x += RESOLUTION)
            {
                Sector s = Sector.PointInSector(AbsToRel((x, y)));
                SBlockMap[y/RESOLUTION][x/RESOLUTION] = s;
            }
        }

        // now, go mark all cells that have 1-sided lines as null
        for (int i = 0; i < level.lines.Size(); i++)
        {
            Line l = level.lines[i];
            if (!l.backsector || l.flags & Line.ML_BLOCKING)
            {
                vector2 v1 = RelToAbs(l.v1.p);
                vector2 v2 = RelToAbs(l.v2.p);
                MakeNull(v1.x/RESOLUTION, v1.y/RESOLUTION, v2.x/RESOLUTION, v2.y/RESOLUTION);
            }
        }

        int notwalkable = 0;
        for (int y = 0; y < CNT_Y; y++)
        {
            for (int x = 0; x < CNT_X; x++)
            {
                if (!SBlockMap[y][x])
                    notwalkable++;
            }
        }

        TestIt();
    }

    Array<PathNode> Path;

    int HeuristicCostEstimate(PathNode a, PathNode b)
    {
        return ((a.x-b.x, a.y-b.y).Length())*16;
    }

    double Ang180(double ang)
    {
        if (ang > 180)
            ang = -(ang-180);
        return ang;
    }

    int GetCost(PathNode a, PathNode b)
    {
        // get angle difference
        return 1;
    }

    Array<PathNode> nodes;
    PathNode GetNode(int x, int y)
    {
        int s = nodes.Size();
        for (int i = 0; i < s; i++)
        {
            PathNode n = nodes[i];
            if (n.x == x && n.y == y)
                return n;
        }
        PathNode n = new('PathNode').Init(x, y);
        nodes.Push(n);
        return n;
    }

    Array<PathNode> closedSet;
    Array<PathNode> openSet;
    Array<PathNode> neighbours;
    void AddNeighbourMaybe(PathNode ref, int xDelta, int yDelta)
    {
        int newX = ref.x+xDelta;
        int newY = ref.y+yDelta;
        if (newX < 0 || newX >= CNT_X)
            return;
        if (newY < 0 || newY >= CNT_Y)
            return;
        if (!SBlockMap[newY][newX])
            return;
        PathNode n = GetNode(newX, newY);
        if (closedSet.Find(n)!=closedSet.Size()) return;
        //Console.Printf("adding %d, %d", newX*RESOLUTION-32768, newY*RESOLUTION-32768);
        neighbours.Push(n);
    }

    bool FindPath(PathNode start, PathNode goal)
    {
        Path.Clear();
        closedSet.Clear();
        openSet.Clear();
        openSet.Push(start);
        start.gScore = 0;
        start.fScore = HeuristicCostEstimate(start, goal);
        start.cameFrom = null;

        int iter = 0;

        while (openSet.Size())
        {
            PathNode current = null;
            int s = openSet.Size();
            for (int i = 0; i < s; i++)
            {
                PathNode currentP = openSet[i];
                if (!current || currentP.fScore < current.fScore)
                    current = currentP;
            }

            if (current == goal) // path found
            {
                // trace path.
                PathNode p = current;
                while (p)
                {
                    Path.Insert(0, p);
                    p = p.cameFrom;
                }
                return true;
            }

            openSet.Delete(openSet.Find(current));
            closedSet.Push(current);
            neighbours.Clear();
            AddNeighbourMaybe(current, -1, 0);
            AddNeighbourMaybe(current, 1, 0);
            AddNeighbourMaybe(current, 0, -1);
            AddNeighbourMaybe(current, 0, 1);
            // diagonal
            //AddNeighbourMaybe(current, -1, -1);
            //AddNeighbourMaybe(current, -1, 1);
            //AddNeighbourMaybe(current, 1, -1);
            //AddNeighbourMaybe(current, 1, 1);
            //Console.Printf("node %d, %d; neighbours = %d", current.x*RESOLUTION-32768, current.y*RESOLUTION-32768, neighbours.Size());

            //iter++;
            //if (iter > 150)
                //break;

            for (int i = 0; i < neighbours.Size(); i++)
            {
                PathNode neighbour = neighbours[i];
                int tentative_gScore = current.gScore + GetCost(current, neighbour);
                // check if it exists already
                if (openSet.Find(neighbour)==openSet.Size())
                    openSet.Push(neighbour);
                else if (tentative_gScore >= neighbour.gScore)
                    continue;

                // this path is the best until now, record it
                neighbour.cameFrom = current;
                neighbour.gScore = tentative_gScore;
                neighbour.fScore = tentative_gScore + HeuristicCostEstimate(neighbour, goal);
            }
        }

        return false;
    }

    bool FindPathByCoordinates(vector2 a, vector2 b)
    {
        nodes.Clear();
        a = RelToAbs(a);
        b = RelToAbs(b);
        PathNode p_a = GetNode(a.x/RESOLUTION, a.y/RESOLUTION);
        PathNode p_b = GetNode(b.x/RESOLUTION, b.y/RESOLUTION);
        return FindPath(p_a, p_b);
    }

    Actor testActor;
    int testTicks;
    int testIdx;

    void LogBlockmapAt(int x, int y)
    {
        Console.Printf("blockmap at %d,%d=%d", x, y, !!SBlockMap[(y+32768)/RESOLUTION][(x+32768)/RESOLUTION]);
    }

    void TestIt()
    {
        testActor = Actor.Spawn('PlasmaBall', (0, 0, 2));
        testTicks = 0;
        testIdx = 0;
        bool found = FindPathByCoordinates((testActor.pos.x, testActor.pos.y), (players[consoleplayer].mo.pos.x, players[consoleplayer].mo.pos.y));
        Console.Printf("found = %d; path = %d", found, Path.Size());
    }

    override void WorldTick()
    {
        if (!testActor)
            return;

        if (Path.Size() <= testIdx+1)
            return;

        testTicks++;
        if (testTicks > 2)
        {
            testIdx++;
            PathNode pn = Path[testIdx];
            vector2 newPos = AbsToRel((pn.x*RESOLUTION, pn.y*RESOLUTION));
            testActor.SetOrigin((newPos.x, newPos.y, testActor.pos.z), true);
            testTicks = 0;

            bool found = FindPathByCoordinates((testActor.pos.x-16, testActor.pos.y-16), (players[consoleplayer].mo.pos.x, players[consoleplayer].mo.pos.y));
            Console.Printf("found = %d; path = %d", found, Path.Size());
        }
    }
}