using AGXUnity;
using UnityEngine;
using UnityEngine.UI;

public class CarDrivetrain : ScriptComponent
{
  // Start is called before the first frame update

  public Constraint LeftFrontHinge;
  public Constraint RightFrontHinge;

  public float MaxMotorTorque = 365; // at 4175 rpm
  public bool Clutch;
  public float TargetSpeed = 0;
  public bool LockDifferential = false;


  public float MotorRPM;
  public float TireRPM;
  public float TireSpeed = 0;
  public float CurrentMotorTorque = 0;



  private const float RadToRpm = 9.5492968f;

  public enum GearSelection
  {
    Reverse,
    Neutral,
    First,
    Second,
    Third,
    Fourth,
    Fifth,
    Sixth,
    Seventh,
    Eight
  }
  public GearSelection Gear;


  agx.Hinge m_motorHinge;
  agxDriveTrain.DryClutch m_dryClutch;
  agxDriveTrain.Differential m_differential;
  agxDriveTrain.GearBox m_gearBox;
  agxPowerLine.PowerLine m_powerLine;

  Text m_infoText;

  protected override bool Initialize()
  {
    m_infoText = GameObject.Find( "Canvas/Information" ).GetComponent<Text>();
    Debug.Assert( m_infoText );

    m_powerLine = new agxPowerLine.PowerLine();
    GetSimulation().add( m_powerLine );

    // Create the drivetrain
    CreateDrivetrain( m_powerLine );

    return base.Initialize();
  }

  public void NextGear()
  {
    int gear = Mathf.Clamp(m_gearBox.getGear() + 1, 0, (int)m_gearBox.getNumGears() - 1);
    Gear = (GearSelection)gear;

    m_gearBox.setGear( gear );
  }
  public void PreviousGear()
  {
    int gear = Mathf.Clamp(m_gearBox.getGear() - 1, 0, (int)m_gearBox.getNumGears() - 1);
    m_gearBox.setGear( gear );
    Gear = (GearSelection)gear;
  }


  void CreateDrivetrain( agxPowerLine.PowerLine powerLine )
  {
    /*
    

    hinge -> motorShaft -> dryClutch -> inputShaft -> gearBox -> outputShaft -> frontDifferential -> leftRotationalActuactor -> leftHinge
                                                                                                 |
                                                                                                  -> rightRotationalActuactor -> rightHinge   

    */

    var nativeLeftFrontHinge = LeftFrontHinge.GetInitialized<AGXUnity.Constraint>().Native.asHinge();
    var nativeRightFrontHinge = RightFrontHinge.GetInitialized<AGXUnity.Constraint>().Native.asHinge();

    LeftFrontHinge.GetController<TargetSpeedController>().Enable = false;
    RightFrontHinge.GetController<TargetSpeedController>().Enable = false;

    var motorShaft = new agxDriveTrain.Shaft();
    motorShaft.getRotationalDimension().setName( "motorShaft" );


    // Create a new Hinge with a motor that will act as our Engine
    var frame = new agx.Frame();
    frame.setLocalRotate( new agx.Quat( agx.Vec3.Z_AXIS(), motorShaft.getRotationalDimension().getWorldDirection() ) );
    m_motorHinge = new agx.Hinge( motorShaft.getRotationalDimension().getOrReserveBody(), frame );

    m_motorHinge.getMotor1D().setEnable( true );
    m_motorHinge.getMotor1D().setSpeed( 0 );
    GetSimulation().add( m_motorHinge );

    // Create a clutch
    m_dryClutch = new agxDriveTrain.DryClutch();
    m_dryClutch.setDisengageTimeConstant( 0.5 );
    m_dryClutch.setTorqueCapacity( 500 );
    m_dryClutch.setAutoLock( true );
    powerLine.add( m_dryClutch );

    var inputShaft = new agxDriveTrain.Shaft();
    inputShaft.getRotationalDimension().setName( "inputShaft" );
    powerLine.add( inputShaft );

    // Create a gearbox
    m_gearBox = new agxDriveTrain.GearBox();
    powerLine.add( m_gearBox );
    var gearRatios = new agx.RealVector();
    gearRatios.Add( -3.3 );
    gearRatios.Add( 0 );
    gearRatios.Add( 4.71 );
    gearRatios.Add( 3.14 );
    gearRatios.Add( 2.1 );
    gearRatios.Add( 1.67 );
    gearRatios.Add( 1.29 );
    gearRatios.Add( 1.0 );
    gearRatios.Add( 0.84 );
    gearRatios.Add( 0.67 );

    // Set the gear ratios
    m_gearBox.setGearRatios( gearRatios );

    // Create a differential between left and right wheel
    m_differential = new agxDriveTrain.Differential();

    double axleGearRatio = 3.21;
    m_differential.setGearRatio( axleGearRatio );
    powerLine.add( m_differential );
    m_differential.setLock( LockDifferential );

    // Create a connector between the drivetrain and the left hinge
    var leftWheel = new agxPowerLine.RotationalActuator(nativeLeftFrontHinge);
    powerLine.add( leftWheel );
    leftWheel.setUseHighSpeedMode( true );

    // Create a connector between the drivetrain and the right hinge
    var rightWheel = new agxPowerLine.RotationalActuator(nativeRightFrontHinge);
    powerLine.add( rightWheel );
    rightWheel.setUseHighSpeedMode( true );

    var outputShaft = new agxDriveTrain.Shaft();
    outputShaft.getRotationalDimension().setName( "outputShaft" );
    powerLine.add( outputShaft );


    // Connect motor to clutch
    Debug.Assert( motorShaft.connect( m_dryClutch ) );

    // Connect clutch to the gear shaft
    Debug.Assert( m_dryClutch.connect( inputShaft ) );

    // Connect the gear shaft to the gearbox
    Debug.Assert( inputShaft.connect( m_gearBox ) );

    // Connect the gearbox to the output shaft
    Debug.Assert( m_gearBox.connect( outputShaft ) );

    // Connect the output shaft to the differential
    Debug.Assert( outputShaft.connect( m_differential ) );

    // Connect left hinge to the differential
    Debug.Assert( m_differential.connect( leftWheel ) );

    // Connect right hinge to the differential
    Debug.Assert( m_differential.connect( rightWheel ) );

    //m_powerLine.writeDimensionsGraph("c:\\temp\\driveline.dot");
  }


  // Update is called once per frame
  void Update()
  {
    float speed = -TargetSpeed/9.5492968f;
    m_motorHinge.getMotor1D().setSpeed( speed );
    m_motorHinge.getMotor1D().setForceRange( -MaxMotorTorque, MaxMotorTorque );
    MotorRPM = (float)( m_motorHinge.getCurrentSpeed() * RadToRpm );

    m_dryClutch.setEngage( Clutch );
    m_differential.setLock( LockDifferential );

    int g = (int)Mathf.Clamp(((int)Gear), 0, m_gearBox.getNumGears() - 1);

    m_gearBox.setGear( g );

    // This is the radius of the cylinders used as a tire.
    const float tireRadius = 0.4205979f;
    TireRPM = (float)LeftFrontHinge.Native.asHinge().getCurrentSpeed() * RadToRpm;
    TireSpeed = (float)LeftFrontHinge.Native.asHinge().getCurrentSpeed() * tireRadius;

    CurrentMotorTorque = (float)m_motorHinge.getMotor1D().getCurrentForce();

    string infoText = "";
    infoText += $"Speed:\t\t{TireSpeed * -3.6:0.0} km/h\n";
    infoText += $"Gear:\t\t{(GearSelection)g}\n";
    infoText += $"Gear Ratio:\t{m_gearBox.getCurrentGearRatio()}\n";
    m_infoText.text = infoText;
  }
}
