using AGXUnity;
using System.Linq;
using UnityEngine;

namespace AGXUnity_TwistedCables
{
  public class TwistedCables : ScriptComponent
  {
    public float CableStiffness = 1E7f;

    private AGXUnity.Constraint m_cylindrical;
    private AGXUnity.CableProperties m_cableProperties;

    public UnityEngine.UI.Text speedLabel;
    public UnityEngine.UI.Text torqueLabel;
    public UnityEngine.UI.Text stiffnessLabel;

    public RigidBody disc1;
    public RigidBody disc2;

    void RouteCable( int i, AGXUnity.Cable cable, float radius, float thickness, AGXUnity.RigidBody disc1, AGXUnity.RigidBody disc2, int n )
    {
      double step = Mathf.PI * 2 / n;
      var x = Mathf.Sin((float)(i * step)) * (radius - cable.Radius);
      var z = Mathf.Cos((float)(i * step)) * (radius - cable.Radius);
      var position = new Vector3(x, -thickness, z);

      cable.Route.Add( AGXUnity.Cable.NodeType.BodyFixedNode, disc1.gameObject, position, m_cylindrical.AttachmentPair.ReferenceFrame.LocalRotation );
      position.y = thickness;
      cable.Route.Add( AGXUnity.Cable.NodeType.BodyFixedNode, disc2.gameObject, position, m_cylindrical.AttachmentPair.ReferenceFrame.LocalRotation );
    }

    // Start is called before the first frame update
    protected override bool Initialize()
    {
      var cables = GetComponentsInChildren<AGXUnity.Cable>();

      // Create new cable properties which we will assign to the cables
      m_cableProperties = AGXUnity.ScriptAsset.Create<AGXUnity.CableProperties>();

      // Find the first cylinder in the scene (one of the disks)
      var cylinder = GetComponentInChildren<AGXUnity.Collide.Cylinder>();
      var radius = cylinder.Radius;
      var height = cylinder.Height;

      // Get the cylindrical constraint which we will use for rotating and moving sideways the Disc1 rigid body
      var constraints = from c in GetComponentsInChildren<AGXUnity.Constraint>() where c.Type == AGXUnity.ConstraintType.CylindricalJoint select c;
      m_cylindrical = constraints.First();

      // For each cable, assign cable properties
      // And Route the cable from one of the discs to the other
      int i = 0;
      foreach ( var c in cables ) {
        RouteCable( i++, c, radius, height, disc1, disc2, cables.Length );
        c.Properties = m_cableProperties;
      }

      // Set the default stiffness
      m_cableProperties[ CableProperties.Direction.Stretch ].YoungsModulus = CableStiffness;
      m_cableProperties[ CableProperties.Direction.Bend ].YoungsModulus = CableStiffness;
      m_cableProperties[ CableProperties.Direction.Twist ].YoungsModulus = CableStiffness;

      return base.Initialize();
    }

    /// <summary>
    /// Set the stiffness of the cable as a ratio change (> or < than 1)
    /// </summary>
    /// <param name="change"></param>
    void SetStiffness( float ratio )
    {
      CableStiffness = Mathf.Clamp( CableStiffness * ratio, 1E5f, 1E10f );
      m_cableProperties[ AGXUnity.CableProperties.Direction.Bend ].YoungsModulus = CableStiffness;
      m_cableProperties[ AGXUnity.CableProperties.Direction.Stretch ].YoungsModulus = CableStiffness;
      m_cableProperties[ AGXUnity.CableProperties.Direction.Twist ].YoungsModulus = CableStiffness;
    }


    void UpdateText()
    {
      var speedController = m_cylindrical.GetController<AGXUnity.TargetSpeedController>(Constraint.ControllerType.Rotational);

      var s = CableStiffness / 1E9f;

      speedLabel.text     = $"{speedController.Speed} rad/s";
      torqueLabel.text    = $"{speedController.Native.getCurrentForce():f} Nm";
      stiffnessLabel.text = $"{s:f3} GPa";
    }

    void FixedUpdate()
    {
      float changeInStiffness = 0.95f;

      // Handle keyboard events
      float speed = 1.0f;
      if ( Input.GetKey( KeyCode.UpArrow ) ) {
        // Start rotating
        var speedController = m_cylindrical.GetController<AGXUnity.TargetSpeedController>(Constraint.ControllerType.Rotational);
        speedController.Speed = -speed;
      }
      else if ( Input.GetKey( KeyCode.DownArrow ) ) {
        // Start rotating
        var speedController = m_cylindrical.GetController<AGXUnity.TargetSpeedController>(Constraint.ControllerType.Rotational);
        speedController.Speed = speed;
      }
      else if ( Input.GetKey( KeyCode.Home ) ) {
        // Stop rotating and moving
        var speedController = m_cylindrical.GetController<AGXUnity.TargetSpeedController>(Constraint.ControllerType.Rotational);
        speedController.Speed = 0;
        speedController = m_cylindrical.GetController<AGXUnity.TargetSpeedController>( Constraint.ControllerType.Translational );
        speedController.Speed = 0;

      }
      else if ( Input.GetKey( KeyCode.LeftArrow ) ) {
        // Start moving left
        var speedController = m_cylindrical.GetController<AGXUnity.TargetSpeedController>(Constraint.ControllerType.Translational);
        speedController.Speed = -0.2f;
      }
      else if ( Input.GetKey( KeyCode.RightArrow ) ) {
        // Start moving right
        var speedController = m_cylindrical.GetController<AGXUnity.TargetSpeedController>(Constraint.ControllerType.Translational);
        speedController.Speed = 0.2f;
      }
      else if ( Input.GetKey( KeyCode.PageDown ) ) {
        // Decrease stiffness
        SetStiffness( changeInStiffness );
      }
      else if ( Input.GetKey( KeyCode.PageUp ) ) {
        // Increase stiffness
        SetStiffness( 1 / changeInStiffness );
      }

      UpdateText();
    }
  }
}