## Planner.cs
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace KortstePad
{
    public class Planner
    {
        private int width;
        private int height;

        public Point start;
        public Point end;

        public bool done = false;

        public List<Point> points = new List<Point>();
        public List<Line> lines = new List<Line>();
        public List<Rectangle> obstacles = new List<Rectangle>();

        public Planner(int width, int height, Point start, Point end, List<Rectangle> obstacles)
        {
            this.width = width;
            this.height = height;
            this.start = start;
            this.end = end;
            this.obstacles = obstacles;

            points.Add(start);
        }

        public void calculateNextPoint() {
            // Kijken of we een direct pad naar het einde kunnen vinden of niet
            Line direct_path_to_end = findDirectLine(end, points);

            if (direct_path_to_end != null)
            {
                lines.Add(direct_path_to_end);
                done = true;
            }
            else
            {
                // Random punt kiezen
                Random rand = new Random();
                int x = rand.Next(width);
                int y = rand.Next(height);
                Point randomPoint = new Point(x, y);

                // Dichtstbijzijnde punt zoeken
                Point nearestPoint = findNearestPoint(randomPoint, points);

                // Indien geen obstakel lijntje maken
                if (!collidesWithObstacles(nearestPoint, randomPoint))
                {
                    points.Add(randomPoint);
                    lines.Add(new Line(nearestPoint, randomPoint));
                }
            }
        }

        public List<Line> getPath()
        {
            List<Line> path = new List<Line>();

            // We zoeken het laatste segment van het pad
            Line line = lines.Find(l => l.b == end);
            path.Add(line);

            // Zolang het startpunt van een segment niet gelijk is aan ons startpunt van het pad blijven we het pad terugvolgen
            while (line.a != start)
            {
                // We zoeken een segment waarvan het eindpunt gelijk is aan het beginpunt van het volgende segment (want we keren terug)
                line = lines.Find(l => l.b == line.a);
                path.Add(line);
            }

            return path;
        }

        private Line findDirectLine(Point p, List<Point> points)
        {
            for (int i = 0; i < points.Count; i++)
            {
                // Als geen obstakel geven we het pad
                if (!collidesWithObstacles(p, points[i]))
                {
                    return new Line(points[i], p);
                }
            }

            return null;
        }

        private bool collidesWithObstacles(Point a, Point b)
        {
            foreach (Rectangle rect in obstacles)
            {
                if (LineIntersectsRect(a, b, rect)) return true;
            }

            return false;
        }

        private Point findNearestPoint(Point p, List<Point> points)
        {
            Point nearestPoint = points[0];

            // Pythagoras afstand
            int tempDistance = (p.X - points[0].X) * (p.X - points[0].X) + (p.Y - points[0].Y) * (p.Y - points[0].Y);

            for (int i = 1; i < points.Count; i++)
            {
                int distance = (p.X - points[i].X) * (p.X - points[i].X) + (p.Y - points[i].Y) * (p.Y - points[i].Y);
                if (distance < tempDistance)
                {
                    tempDistance = distance;
                    nearestPoint = points[i];
                }
            }

            return nearestPoint;
        }

        // Source: https://stackoverflow.com/questions/5514366/how-to-know-if-a-line-intersects-a-rectangle
        private bool LineIntersectsRect(Point p1, Point p2, Rectangle r)
        {
            return LineIntersectsLine(p1, p2, new Point(r.X, r.Y), new Point(r.X + r.Width, r.Y)) ||
                   LineIntersectsLine(p1, p2, new Point(r.X + r.Width, r.Y), new Point(r.X + r.Width, r.Y + r.Height)) ||
                   LineIntersectsLine(p1, p2, new Point(r.X + r.Width, r.Y + r.Height), new Point(r.X, r.Y + r.Height)) ||
                   LineIntersectsLine(p1, p2, new Point(r.X, r.Y + r.Height), new Point(r.X, r.Y)) ||
                   (r.Contains(p1) && r.Contains(p2));
        }

        private bool LineIntersectsLine(Point l1p1, Point l1p2, Point l2p1, Point l2p2)
        {
            float q = (l1p1.Y - l2p1.Y) * (l2p2.X - l2p1.X) - (l1p1.X - l2p1.X) * (l2p2.Y - l2p1.Y);
            float d = (l1p2.X - l1p1.X) * (l2p2.Y - l2p1.Y) - (l1p2.Y - l1p1.Y) * (l2p2.X - l2p1.X);

            if (d == 0)
            {
                return false;
            }

            float r = q / d;

            q = (l1p1.Y - l2p1.Y) * (l1p2.X - l1p1.X) - (l1p1.X - l2p1.X) * (l1p2.Y - l1p1.Y);
            float s = q / d;

            if (r < 0 || r > 1 || s < 0 || s > 1)
            {
                return false;
            }

            return true;
        }
    }

}