using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class EmTrajectoryPoint
{
    public EmTrajectoryPoint(
        double x,
        double y,
        double yaw,
        double signedLongitudinalVelocity,
        double timeFromStart,
        double vehicleCurvature,
        int segmentIndex,
        double segmentLocalS,
        double pathS,
        TravelDirection direction,
        EmBoundaryType boundaryType,
        double longitudinalAcceleration,
        double longitudinalJerk)
    {
        ContractNumeric.RequireFinite(x, nameof(x));
        ContractNumeric.RequireFinite(y, nameof(y));
        ContractNumeric.RequireFinite(yaw, nameof(yaw));
        ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity));
        ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart));
        ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
        ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS));
        ContractNumeric.RequireFinite(pathS, nameof(pathS));
        ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration));
        ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk));
        if (segmentIndex < 0)
            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
        if (timeFromStart < 0d)
            throw new ArgumentOutOfRangeException(nameof(timeFromStart));
        if (segmentLocalS < 0d)
            throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
        if (pathS < 0d)
            throw new ArgumentOutOfRangeException(nameof(pathS));
        if (!Enum.IsDefined(typeof(TravelDirection), direction))
            throw new ArgumentOutOfRangeException(nameof(direction));
        if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
            throw new ArgumentOutOfRangeException(nameof(boundaryType));

        X = x;
        Y = y;
        Yaw = yaw;
        SignedLongitudinalVelocity = signedLongitudinalVelocity;
        Speed = Math.Abs(signedLongitudinalVelocity);
        VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
        VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
        YawRate = signedLongitudinalVelocity * vehicleCurvature;
        TimeFromStart = timeFromStart;
        VehicleCurvature = vehicleCurvature;
        SegmentIndex = segmentIndex;
        SegmentLocalS = segmentLocalS;
        PathS = pathS;
        Direction = direction;
        BoundaryType = boundaryType;
        LongitudinalAcceleration = longitudinalAcceleration;
        LongitudinalJerk = longitudinalJerk;
    }

    public double X { get; }
    public double Y { get; }
    public double Yaw { get; }
    public double SignedLongitudinalVelocity { get; }
    public double Speed { get; }
    public double VelocityX { get; }
    public double VelocityY { get; }
    public double YawRate { get; }
    public double TimeFromStart { get; }
    public double VehicleCurvature { get; }
    public int SegmentIndex { get; }
    public double SegmentLocalS { get; }
    public double PathS { get; }
    public TravelDirection Direction { get; }
    public EmBoundaryType BoundaryType { get; }

    internal double LongitudinalAcceleration { get; }

    internal double LongitudinalJerk { get; }
}