using AGXUnity;
using System.Linq;
using UnityEngine;

public class BasicVehicleController : ScriptComponent
{
  // Hinges used for motor
  Constraint[] m_hinges;

  // Cylindrical used for Steering (and suspension)
  Constraint[] m_cylindrical;

  // The speed of the hinges (in radians)
  public float Speed;

  // The angle of the steering 
  public float Steering;


  protected override bool Initialize()
  {
    // Get hinges with the sub-string "Front" in (child GameObjects)
    m_hinges = GetComponentsInChildren<Constraint>().Where( c => c.Type == ConstraintType.Hinge && c.name.Contains( "Front" ) ).ToArray();
    Debug.Assert( m_hinges.Length > 0 );

    // Enable the motors
    foreach ( var c in m_hinges )
      c.GetController<TargetSpeedController>().Enable = true;


    // Get all cylindrical constraints (in child GameObjects)
    m_cylindrical = GetComponentsInChildren<Constraint>().Where( c => c.Type == ConstraintType.CylindricalJoint ).ToArray();
    Debug.Assert( m_cylindrical.Length > 0 );

    return base.Initialize();
  }

  void Update()
  {
    // Set the speed on the hinges
    foreach ( var c in m_hinges )
      c.GetController<TargetSpeedController>().Speed = Speed;

    // Set the target position of the rotational Lock on the Suspension constraint
    m_cylindrical[ 0 ].GetController<LockController>( Constraint.ControllerType.Rotational ).Position = Steering;
    m_cylindrical[ 1 ].GetController<LockController>( Constraint.ControllerType.Rotational ).Position = Steering;

  }
}
