namespace SimulationCommon;

public class TZFX
{
      public static double CalculateVectorAngle(double[] vector)
    {
        double vectorCos = vector[0] / Math.Sqrt(vector[0] * vector[0] + vector[1] * vector[1]);
        double vectorAngle = 0;
        if (vector[1] >= 0)
        {
            vectorAngle = Math.Acos(vectorCos);
        }
        else if (vector[1] < 0)
        {
            vectorAngle = 2 * Math.PI - Math.Acos(vectorCos);
        }
        return vectorAngle;
    }

    public static Point MovePointByVandA(double angle, double distance, Point startPoint, double[] vector)
    {
        Point finalPoint = new Point();
        double vectorAngle = CalculateVectorAngle(vector);
        vectorAngle += angle;
        double vectorAngleSin = Math.Sin(vectorAngle);
        double vectorAngleCos = Math.Cos(vectorAngle);

        finalPoint.lon = startPoint.lon + distance * vectorAngleCos;
        finalPoint.lat = startPoint.lat + distance * vectorAngleSin;
        return finalPoint;
    }

    public static List<Point> GenerateWaypoints(Point rv0, Point rv1, Point rv2, Point rv3, double a, double sweepWidth)
    {
        List<Point> outputWaypoints = new List<Point>();
        double[] leftdir = { rv0.lon - rv1.lon, rv0.lat - rv1.lat };
        double[] updir = { rv1.lon - rv2.lon, rv1.lat - rv2.lat };

        Point CP1 = MovePointByVandA(0, a / 2, rv1, leftdir);
        Point CP = MovePointByVandA(Math.PI, a / 2, CP1, updir);
        outputWaypoints.Add(CP);

        double forwardDistance = 0;
        Point midWaypoint = new Point { lat = CP.lat, lon = CP.lon };

        for (int i = 1; forwardDistance < a/2; i++)
        {
            Point MovePoint1 = MovePointByVandA(0, i * sweepWidth, midWaypoint, leftdir);
            outputWaypoints.Add(MovePoint1);
            midWaypoint.lon = MovePoint1.lon;
            midWaypoint.lat = MovePoint1.lat;
            Point MovePoint2 = MovePointByVandA(0, i * sweepWidth, midWaypoint, updir);
            outputWaypoints.Add(MovePoint2);
            midWaypoint.lon = MovePoint2.lon;
            midWaypoint.lat = MovePoint2.lat;

            forwardDistance += sweepWidth;
            i++;

            Point MovePoint3 = MovePointByVandA(Math.PI, i * sweepWidth, midWaypoint, leftdir);
            outputWaypoints.Add(MovePoint3);
            midWaypoint.lon = MovePoint3.lon;
            midWaypoint.lat = MovePoint3.lat;
            Point MovePoint4 = MovePointByVandA(Math.PI, i * sweepWidth, midWaypoint, updir);
            outputWaypoints.Add(MovePoint4);
            midWaypoint.lon = MovePoint4.lon;
            midWaypoint.lat = MovePoint4.lat;
        }

        return outputWaypoints;
    }

}