﻿using AGXUnity;
using agx;

namespace AGXUnity_RobotML.Scripts
{
  public class RobotMotor : ScriptComponent
  {
    agxPowerLine.PowerLine powerLine;
    agxDriveTrain.Engine engine;
    agxPowerLine.PowerTimeIntegralLookupTable lookupTable;
    agxPowerLine.RotationalActuator actuator;

    public double motorInertia = 0.27;
    public double gearRatio = 125.0;
    public double torque = 0.0;

    // Start is called before the first frame update
    protected override bool Initialize()
    {
      powerLine = new agxPowerLine.PowerLine();
      engine = new agxDriveTrain.Engine();

      Hinge hinge = GetComponent<AGXUnity.Constraint>().GetInitialized<AGXUnity.Constraint>().Native.asHinge();
      Simulation.Instance.Native.add( powerLine );

      hinge.getMotor1D().setEnable( false );
      engine.setPowerGenerator( new agxPowerLine.TorqueGenerator( engine.getRotationalDimension() ) );
      lookupTable = engine.getPowerGenerator().getPowerTimeIntegralLookupTable();
      engine.setThrottle( 1.0 );
      engine.setInertia( motorInertia );
      powerLine.add( engine );

      actuator = new agxPowerLine.RotationalActuator( hinge );
      agxDriveTrain.HolonomicGear gear = new agxDriveTrain.HolonomicGear();
      gear.setGearRatio( gearRatio );
      engine.connect( gear );
      gear.connect( actuator );

      Simulation.Instance.StepCallbacks.PreStepForward += OnPreStepForward;

      return true;
    }

    protected override void OnDestroy()
    {
      if ( Simulation.HasInstance ) {
        Simulation.Instance.Native.remove( powerLine );
        Simulation.Instance.StepCallbacks.PreStepForward -= OnPreStepForward;
      }
    }

    private void OnPreStepForward()
    {
      // Hinge hinge = GetComponent<AGXUnity.Constraint>().GetInitialized<AGXUnity.Constraint>().Native.asHinge();
      // Debug.Log(hinge.getName());
      // Debug.Log("enabled " + hinge.getRange1D().getEnable());
      // Debug.Log("enabled " + hinge.getRange1D().getRange().lower() + " : " + hinge.getRange1D().getRange().upper());
      // Debug.Log("angle " + hinge.getAngle());

      lookupTable.clear();
      lookupTable.insertValue( 0.0, torque );
    }
  }
}