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

public class AutobrakedBoxtruck : MonoBehaviour
{
  class AutobrakeSystem
  {
    private struct LidarData
    {
      public float distance;
      public float intensity;
    }

    private RangeReal m_Range;
    private float m_Cooldown;
    private LidarOutput m_LidarSource;
    private float m_EngageUntil;

    public AutobrakeSystem( RangeReal range, float cooldown, LidarSensor lidarSource )
    {
      m_Range = range;
      m_Cooldown = cooldown;
      m_EngageUntil = 0.0f;

      m_LidarSource = new LidarOutput()
      {
        agxSensor.RtOutput.Field.DISTANCE_F32,
        agxSensor.RtOutput.Field.INTENSITY_F32
      };
      lidarSource.Add( m_LidarSource );
    }

    public void Update()
    {
      var points = m_LidarSource.View<LidarData>( out uint count );

      bool hit = false;
      for ( int i = 0; i < count; i++ ) {
        var distance = points[i].distance;
        if (
          distance > m_Range.Min && distance < m_Range.Max &&
          points[ i ].intensity > 0.01f
          ) {
          hit = true;
          break;
        }
      }

      if ( hit )
        m_EngageUntil = (float)( Simulation.Instance.Native.getTimeStamp() + m_Cooldown );
    }

    public bool IsEngaged => Simulation.Instance.Native.getTimeStamp() < m_EngageUntil;
  }

  private AutobrakeSystem m_autobrake;
  private InputTarget m_leftDriveInput;
  private InputTarget m_rightDriveInput;

  public InputActionAsset InputActions;

  private InputActionMap m_actionMap;

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

    // TODO: This should use the OpenPLX signal API when sensors are supported in AGXUnity
    var openplxLidar = root.Native.getObject( "box_truck.lidar.logic.ray_source" ) as openplx.Sensors.Optics.HorizontalSweepRaySource;
    var mappedLidar = root.FindMappedObject( "box_truck.lidar.logic" ).GetComponent<LidarSensor>();

    m_autobrake = new AutobrakeSystem(
      new RangeReal( 0.7f, 2.0f ),
      (float)( 1.0 / openplxLidar.frequency() * 8.0f ),
      mappedLidar
    );

    var rootName = root.Native.getName();
    m_leftDriveInput = signals.FindInputTarget( $"{rootName}.box_truck.front_left_drive_motor.target_angular_velocity_input" );
    m_rightDriveInput = signals.FindInputTarget( $"{rootName}.box_truck.front_right_drive_motor.target_angular_velocity_input" );

    m_actionMap = InputActions.FindActionMap( "BoxTruck" );
    m_actionMap.Enable();

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

  void Pre()
  {
    float drive = m_actionMap["Drive"].ReadValue<float>();
    m_autobrake.Update();
    if ( m_autobrake.IsEngaged )
      drive = Mathf.Min( drive, 0.0f );

    drive *= 6;

    m_leftDriveInput.SendSignal( drive );
    m_rightDriveInput.SendSignal( -drive );
  }
}
