using agxSensor;
using AGXUnity;
using AGXUnity.Model;
using AGXUnity.Sensor;
using UnityEngine;

[RequireComponent( typeof( LidarSensor ) )]
public class LiDARReverseBrake : ScriptComponent
{
  private LidarSensor m_lidar;
  private LidarOutput m_output;
  private RigidBody m_rigidBody;
  private WheelLoader m_wheelLoader;

  protected override bool Initialize()
  {
    m_lidar = GetComponent<LidarSensor>();
    m_output = new LidarOutput { RtOutput.Field.DISTANCE_F32 };
    m_lidar.Add( m_output );

    m_rigidBody = GetComponentInParent<RigidBody>();
    m_wheelLoader = m_rigidBody.GetComponentInParent<WheelLoader>();

    if ( m_rigidBody == null || m_wheelLoader == null ) {
      Debug.LogError( "Couldn't find RB and wheel loader components to attach to." );
      return false;
    }

    Simulation.Instance.StepCallbacks.PreStepForward += CheckLidar;
    return true;
  }

  void CheckLidar()
  {
    var data = m_output.View<float>(out uint _);
    bool brake = false;
    foreach ( var f in data )
      if ( f < 1 )
        brake = true;
    var relVel = transform.TransformDirection(m_rigidBody.LinearVelocity);
    if ( brake && relVel.z > 0.2f ) {
      Debug.Log( "Emergency Brake!" );
      m_wheelLoader.BrakeHinge.getMotor1D().setEnable( true );
      m_wheelLoader.BrakeHinge.getMotor1D().setSpeed( 0.0f );
      m_wheelLoader.BrakeHinge.getMotor1D().setForceRange( -1e5, 1e5 );
    }
  }
}
