﻿using AGXUnity;
using System.Linq;
using UnityEngine;

/// <summary>
/// Control the rotational angle of two Cylindrical constraints based on 
/// an Ackerman steering mechanism
/// </summary>
public class AckermanSteering : ScriptComponent
{
  // Start is called before the first frame update
  public float Steering;
  Constraint[] m_cylindrical;

  public float AckermanPercentage = 50.0f;
  public float SteeringRatio = 1;


  static (float, float) CalculateSteeringAngles( float delta, float wheelBase, float wheelDistance, float steeringRatio, float ackermanPercentage )
  {
    // Convert delta to radians
    float deltaRad = Mathf.PI / 180 * delta;

    // Calculate the turn radius
    float radius = wheelBase / Mathf.Tan(deltaRad);

    // Calculate the distance bewheelDistanceeen the wheelDistanceo wheels
    float distance = wheelDistance / 2;

    // Calculate the angle by which the inner wheel should turn
    float innerAngle = Mathf.Atan(wheelBase / (radius - distance));

    // Calculate the angle by which the outer wheel should turn
    float outerAngle = Mathf.Atan(wheelBase / (radius + distance));

    // Apply the steering ratio and Ackermann percentage to get the final angles
    float innerSteering = innerAngle / steeringRatio * ackermanPercentage;
    float outerSteering = outerAngle / steeringRatio * ackermanPercentage;

    return (innerSteering, outerSteering);
  }

  float m_wheelBase;
  float m_wheelDistance;

  protected override bool Initialize()
  {

    // Find the Cylindrical constraints as components in the children
    m_cylindrical = GetComponentsInChildren<Constraint>().Where( c => c.Type == ConstraintType.CylindricalJoint ).ToArray();
    Debug.Assert( m_cylindrical.Length > 0 );

    // To calculate the Wheel distance and the Wheel base, we need to find some of the bodies:
    var bodies = GetComponentsInChildren<RigidBody>();
    var leftFrontTire = bodies.Where(b => b.name.Contains("LeftFrontTire")).First();
    Debug.Assert( leftFrontTire != null );

    var rightFrontTire = bodies.Where(b => b.name.Contains("RightFrontTire")).First();
    Debug.Assert( rightFrontTire != null );

    var rightRearTire = bodies.Where(b => b.name.Contains("RightRearTire")).First();
    Debug.Assert( rightRearTire != null );

    // Then we can compute the distance between the tires left/right
    m_wheelDistance = ( leftFrontTire.transform.position - rightFrontTire.transform.position ).magnitude;

    // And the wheel base (distance between front and rear tires.
    m_wheelBase = ( rightRearTire.transform.position - rightFrontTire.transform.position ).magnitude;

    return base.Initialize();
  }

  void Update()
  {
    // Compute the steering angle for each wheel
    (float left, float right) = CalculateSteeringAngles( Steering, m_wheelBase, m_wheelDistance, SteeringRatio, AckermanPercentage );

    // Update the rotational position of the Cylindrical constraints.
    m_cylindrical[ 0 ].GetController<LockController>( Constraint.ControllerType.Rotational ).Position = left;
    m_cylindrical[ 1 ].GetController<LockController>( Constraint.ControllerType.Rotational ).Position = right;
  }
}
