package org.opentrafficsim.core.geometry;
   
   import org.djunits.value.vdouble.scalar.Angle;
   import org.djutils.draw.line.PolyLine2d;
   import org.djutils.draw.point.OrientedPoint2d;
   import org.djutils.draw.point.Point2d;
   import org.djutils.exceptions.Throw;
   
   /**
   * Continuous definition of an arc.
   * <p>
   * Copyright (c) 2023-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
   * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
   * </p>
   * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
   */
  public class ContinuousArc implements ContinuousLine
  {
  
      /** Starting point. */
      private final OrientedPoint2d startPoint;
  
      /** Curve radius. */
      private final double radius;
  
      /** Angle of the curve. */
      private final Angle angle;
  
      /** Sign to use for offsets and angles, which depends on the left/right direction. */
      private double sign;
  
      /** Center point of circle, as calculated in constructor. */
      private final Point2d center;
  
      /**
       * Define arc by starting point, radius, curve direction, and length.
       * @param startPoint starting point.
       * @param radius radius (must be positive).
       * @param left left curve, or right.
       * @param length arc length.
       */
      public ContinuousArc(final OrientedPoint2d startPoint, final double radius, final boolean left, final double length)
      {
          this(startPoint, radius, left, Angle.instantiateSI(
                  Throw.when(length, length <= 0.0, IllegalArgumentException.class, "Length must be above 0.") / radius));
      }
  
      /**
       * Define arc by starting point, radius, curve direction, and angle.
       * @param startPoint starting point.
       * @param radius radius (must be positive).
       * @param left left curve, or right.
       * @param angle angle of arc (must be positive).
       */
      public ContinuousArc(final OrientedPoint2d startPoint, final double radius, final boolean left, final Angle angle)
      {
          Throw.whenNull(startPoint, "Start point may not be null.");
          Throw.when(radius < 0.0, IllegalArgumentException.class, "Radius must be positive.");
          Throw.when(angle.si < 0.0, IllegalArgumentException.class, "Angle must be positive.");
          this.startPoint = startPoint;
          this.radius = radius;
          this.sign = left ? 1.0 : -1.0;
          this.angle = angle;
          double dx = Math.cos(startPoint.dirZ) * this.sign * radius;
          double dy = Math.sin(startPoint.dirZ) * this.sign * radius;
  
          this.center = new Point2d(startPoint.x - dy, startPoint.y + dx);
      }
  
      @Override
      public OrientedPoint2d getStartPoint()
      {
          return this.startPoint;
      }
  
      @Override
      public OrientedPoint2d getEndPoint()
      {
          Point2d point = getPoint(1.0, 0.0);
          double dirZ = this.startPoint.dirZ + this.sign * this.angle.si;
          dirZ = dirZ > Math.PI ? dirZ - 2.0 * Math.PI : (dirZ < -Math.PI ? dirZ + 2.0 * Math.PI : 0.0);
          return new OrientedPoint2d(point.x, point.y, dirZ);
      }
  
      @Override
      public double getStartCurvature()
      {
          return 1.0 / this.radius;
      }
  
      @Override
      public double getEndCurvature()
      {
          return getStartCurvature();
      }
  
      @Override
      public double getStartRadius()
      {
         return this.radius;
     }
 
     @Override
     public double getEndRadius()
     {
         return this.radius;
     }
 
     /**
      * Returns a point on the arc at a fraction along the arc.
      * @param fraction fraction along the arc.
      * @param offset offset relative to radius.
      * @return point on the arc at a fraction along the arc.
      */
     private Point2d getPoint(final double fraction, final double offset)
     {
         double len = this.radius - this.sign * offset;
         double ang = this.startPoint.dirZ + this.sign * (this.angle.si * fraction);
         double dx = this.sign * Math.cos(ang) * len;
         double dy = this.sign * Math.sin(ang) * len;
         return new Point2d(this.center.x + dy, this.center.y - dx);
     }
 
     /**
      * Returns the direction at given fraction along the arc.
      * @param fraction fraction along the arc.
      * @return direction at given fraction along the arc.
      */
     private double getDirection(final double fraction)
     {
         double d = this.startPoint.dirZ + this.sign * (this.angle.si) * fraction;
         while (d > Math.PI)
         {
             d -= (2 * Math.PI);
         }
         while (d < -Math.PI)
         {
             d += (2 * Math.PI);
         }
         return d;
     }
 
     @Override
     public PolyLine2d flatten(final Flattener flattener)
     {
         Throw.whenNull(flattener, "Flattener may not be null.");
         return flattener.flatten(new FlattableLine()
         {
             @Override
             public Point2d get(final double fraction)
             {
                 return getPoint(fraction, 0.0);
             }
 
             @Override
             public double getDirection(final double fraction)
             {
                 return ContinuousArc.this.getDirection(fraction);
             }
         });
     }
 
     @Override
     public PolyLine2d flattenOffset(final ContinuousDoubleFunction offset, final Flattener flattener)
     {
         Throw.whenNull(offset, "Offset may not be null.");
         Throw.whenNull(flattener, "Flattener may not be null.");
         return flattener.flatten(new FlattableLine()
         {
             @Override
             public Point2d get(final double fraction)
             {
                 return getPoint(fraction, offset.apply(fraction));
             }
 
             @Override
             public double getDirection(final double fraction)
             {
                 /*-
                  * x = cos(phi) * (r - s(phi))
                  * y = sin(phi) * (r - s(phi)) 
                  * 
                  * with,
                  *   phi    = angle of circle arc point at fraction, relative to circle center
                  *   r      = radius
                  *   s(phi) = offset at phi (or at fraction)
                  * 
                  * then using the product rule: 
                  * 
                  * x' = -sin(phi) * (r - s(phi)) - cos(phi) * s'(phi)
                  * y' = cos(phi) * (r - s(phi)) - sin(phi) * s'(phi)
                  */
                 double phi = (ContinuousArc.this.startPoint.dirZ
                         + ContinuousArc.this.sign * (ContinuousArc.this.angle.si * fraction - Math.PI / 2));
                 double sinPhi = Math.sin(phi);
                 double cosPhi = Math.cos(phi);
                 double sPhi = ContinuousArc.this.sign * offset.apply(fraction);
                 double sPhiD = offset.getDerivative(fraction) / ContinuousArc.this.angle.si;
                 double dx = -sinPhi * (ContinuousArc.this.radius - sPhi) - cosPhi * sPhiD;
                 double dy = cosPhi * (ContinuousArc.this.radius - sPhi) - sinPhi * sPhiD;
                 return Math.atan2(ContinuousArc.this.sign * dy, ContinuousArc.this.sign * dx);
             }
         });
     }
 
     @Override
     public double getLength()
     {
         return this.angle.si * this.radius;
     }
 
     @Override
     public String toString()
     {
         return "ContinuousArc [startPoint=" + this.startPoint + ", radius=" + this.radius + ", angle=" + this.angle + ", left="
                 + (this.sign > 0.0) + "]";
     }
 
 }