using AGXUnity;
using AGXUnity.IO.OpenPLX;
using UnityEngine;
using UnityEngine.InputSystem;

public class WheelVehicleController : MonoBehaviour
{
  InputTarget m_throttleInput;
  InputTarget m_clutchInput;
  InputTarget m_gearBoxInput;
  InputTarget m_leftBrakeInput;
  InputTarget m_rightBrakeInput;
  InputTarget m_steeringInput;

  float currentThrottle = 0;
  float currentClutch = 0;
  float currentBrake = 0;
  float currentSteer = 0;
  int currentGear = 0;

  public InputActionAsset inputActions;
  private InputActionMap m_map;

  // Start is called before the first frame update
  void Start()
  {
    var root = GetComponent<OpenPLXRoot>().GetInitialized();
    var signals = GetComponent<OpenPLXSignals>().GetInitialized();

    var signal_interface = signals.Interfaces[0];

    m_throttleInput   = signal_interface.FindInput( "throttle" );
    m_clutchInput     = signal_interface.FindInput( "clutch" );
    m_gearBoxInput    = signal_interface.FindInput( "gear" );
    m_leftBrakeInput  = signal_interface.FindInput( "brake_left" );
    m_rightBrakeInput = signal_interface.FindInput( "brake_right" );
    m_steeringInput   = signal_interface.FindInput( "steering_angle" );

    m_map = inputActions.FindActionMap( "WheeledVehicle" );
    m_map.Enable();

    Simulation.Instance.StepCallbacks.PreStepForward += Pre;
  }

  void Pre()
  {
    var driveAction = m_map.FindAction( "Drive" );
    var brakeAction = m_map.FindAction( "Brake" );
    var steerAction = m_map.FindAction( "Steer" );

    var drive = driveAction.ReadValue<float>();
    if ( Mathf.Sign( drive ) != Mathf.Sign( currentGear ) ) {
      currentThrottle = 0;
      currentClutch = 0;
    }
    currentGear = (int)Mathf.Sign( drive );
    currentThrottle = drive == 0 ? 0 : Mathf.Min( currentThrottle + 0.01f, 0.9f );
    currentClutch = drive == 0 ? 0 : Mathf.Min( currentClutch + 0.01f, 1 );
    var steer = steerAction.ReadValue<float>();
    currentSteer = steer == 0
      ? Mathf.MoveTowards( currentSteer, 0.0f, 0.01f )
      : Mathf.MoveTowards( currentSteer, Mathf.Sign( steer ) * 0.5f, 0.01f );

    var brake = brakeAction.ReadValue<float>();
    currentBrake = brake == 0 ? 0.0f : Mathf.Min( currentBrake + 0.1f, 1.0f );

    m_throttleInput.SendSignal( currentThrottle );
    m_clutchInput.SendSignal( currentClutch );
    m_gearBoxInput.SendSignal( currentGear );
    m_leftBrakeInput.SendSignal( currentBrake );
    m_rightBrakeInput.SendSignal( currentBrake );
    m_steeringInput.SendSignal( currentSteer );
  }
}
