namespace SimulationCommon;
public class LocationW
{
    public double Lon { get; set; }
    public double Lat { get; set; }
}

public class SectorSearch
{
    // 计算向量角度
    private 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;
    }

    // 根据角度和距离移动点
    private static Point MovePointByVectorAndAngle(double angle, double distance, Point startPoint, double[] vector)
    {
        ///定义并输出新坐标点
        Point finalPoint = new Point();
        //计算已知向量与x轴正方向的夹角
        double vectorAngle = CalculateVectorAngle(vector);
        //计算目标点向量与x轴正方向的夹角
        vectorAngle += angle;
        //计算目标点向量夹角的正弦、余弦值
        double vectorAngleSin = Math.Sin(vectorAngle);
        double vectorAngleCos = Math.Cos(vectorAngle);
        //ArrayList<Double> XY = new ArrayList<Double>();
        finalPoint.lon = startPoint.lon + distance * vectorAngleCos;
        finalPoint.lat = startPoint.lat + distance * vectorAngleSin;
        return finalPoint;
    }

    // 主逻辑
    public static List<Point> sectorSearch(Point basePoint, double searchAngle, double sweepWidth)
    {
        List<Point> boundaryPoints = new List<Point>();
        List<Point> outputWaypoints = new List<Point>();

        double Angle = searchAngle * Math.PI / 180;
        double[] baseDir = { 0, 1 };

        int numPoints = (int)(360 / searchAngle);
        boundaryPoints.Add(basePoint);
        
        double rs = sweepWidth / Math.Sin(searchAngle / 2);
        
        for (int i = 0; i < numPoints; i++)
        {
            Point midpoint = MovePointByVectorAndAngle(-i * Angle, rs, basePoint, baseDir);
            boundaryPoints.Add(midpoint);
            midpoint.lat = basePoint.lat;
            midpoint.lon = basePoint.lon;
        }
            
        int[] num = new int[(int)(3 * (180 / searchAngle) + 1)];
        num[0] = 0;
        for (int i = 1, n = 1, m = 1; n < 3 * (180 / searchAngle); i += 2)
        {

            m = i;
            num[n] = m; n++;
            m = i + 1;
            num[n] = m; n++;
            m = 0;
            num[n] = m; n++;
            m = (int)(i + 1 + 180 / searchAngle);
            num[n] = m; n++;
            m = (int)(i + 180 / searchAngle);
            num[n] = m; n++;
            m = 0;
            num[n] = m; n++;
            
        }
        for (int i = 0; i < num.Length; i++)
        {
            Point p = new Point();
            p.lat = boundaryPoints[num[i]].lat;
            p.lon = boundaryPoints[num[i]].lon;
            outputWaypoints.Add(p);
        }
            
        return outputWaypoints;

    }
}