using MMICSharp.Common;
using MMIStandard;
using MMIUnity;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

public class WaveMMU : UnityMMUBase
{
   private Animator animator;

   private MInstruction instruction;

   private MPostureBlendingService.Iface blending_service;
   private float start_percentage = 0.15f;
   private float end_percentage = 0.15f;

   private List<MJointType> partialList = new List<MJointType>() { MJointType.S1L5Joint, MJointType.T12L1Joint, MJointType.T1T2Joint, MJointType.C4C5Joint, MJointType.HeadJoint, MJointType.HeadTip, MJointType.RightShoulder, MJointType.RightElbow, MJointType.RightWrist };


   protected override void Awake()
   {
      // Assign the name of the MMU. This name should correspond to the GameObject name, the prefab asset, and the MMU folder. 
      this.Name = "WaveMMU";

      // Assign the motion type of the MMU. This motion type will be utilized to assign instructions. 
      this.MotionType = "Pose/Wave";

      // Auto generated source code for assignemnt of root transform and root bone:
      this.Pelvis = this.gameObject.GetComponentsInChildren<Transform>().First(s => s.name == "pelvis");
      this.RootTransform = this.transform;

      // Get the animator (needs to be added in before)
      this.animator = this.GetComponent<Animator>();

      // Disable the animator at the beginning (otherwise retargeting won't work)
      this.animator.enabled = false;
      this.animator.cullingMode = AnimatorCullingMode.AlwaysAnimate;

      // It is important that the bone assignment is done before the base class awake is called
      base.Awake();
   }


   // Start is called before the first frame update
   protected override void Start()
   {
      //All required scripts should be added in here

      //Auto generated source code for script initialization.

      base.Start();

   }

   /// <summary>
   /// Basic initialize routine. This routine is called when the MMU is initialized. 
   /// The method must be implemented by the MMU developer.
   /// </summary>
   /// <param name="avatarDescription"></param>
   /// <param name="properties"></param>
   /// <returns></returns>
   public override MBoolResponse Initialize(MAvatarDescription avatarDescription, Dictionary<string, string> properties)
   {
      //Execute instructions on main thread
      this.ExecuteOnMainThread(() =>
      {
         //Call the base class initialization
         base.Initialize(avatarDescription, properties);
      });

      return new MBoolResponse(true);
   }

   /// <summary>
   /// Method to assign a MInstriction to the MMU:
   /// The method must be provided by the MMU developer.
   /// </summary>
   /// <param name="motionCommand"></param>
   /// <param name="avatarState"></param>
   /// <returns></returns>
   public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState state)
   {
      //Assign the instruction to the class variable
      this.instruction = instruction;

      Vector3 target_position = new Vector3(0, 0, 0);
      bool wasConstraint = false;

      // Check if the target id is a constraint
      foreach (MConstraint c in instruction.Constraints)
      {
         if (c.ID == instruction.Properties["TargetID"] && c.GeometryConstraint != null)
         {
            // Constraint found
            Quaternion parentRot = Quaternion.identity;
            Vector3 parentPos = Vector3.zero;
            if (!string.IsNullOrEmpty(c.GeometryConstraint.ParentObjectID))
            {
            // has a parent
            var parent = this.SceneAccess.GetSceneObjectByID(c.GeometryConstraint.ParentObjectID);
               if (parent != null)
               {
                  parentRot = parent.Transform.Rotation.ToQuaternion();
                  parentPos = parent.Transform.Position.ToVector3();
               }
            }
            // Transform to global space
            target_position = parentRot * c.GeometryConstraint.ParentToConstraint.Position.ToVector3() + parentPos;
            wasConstraint = true;
            break;
         }
      }
      // If no constraint, check if there is a suitable scene object
      if (!wasConstraint)
      {
         var mobj = this.SceneAccess.GetSceneObjectByID(this.instruction.Properties["TargetID"]);
         if (mobj == null)
         {
            MBoolResponse result = new MBoolResponse(false);
            result.LogData = new List<string>() { "Target Constraint or Object not found with ID: " + this.instruction.Properties["TargetID"] };
            return result;
         }
         target_position = mobj.Transform.Position.ToVector3();
      }


      this.ExecuteOnMainThread(() =>
      {
         this.AssignPostureValues(state.Current);

         var dir = this.transform.TransformDirection(target_position - this.transform.position).normalized;

         dir = Vector3.ProjectOnPlane(dir, Vector3.up);
         float angle = Mathf.Clamp(Vector2.SignedAngle(new Vector2(0, 1), new Vector2(dir.x, dir.z)) / 90, -1, 1);

         animator.SetFloat("WaveBlend", angle);
         animator.SetLayerWeight(1, 1);
         animator.SetTrigger("Wave");

         // Initial animator update to start the trigger
         this.animator.Update(0.01f);

      });

      return new MBoolResponse(true);
   }

   /// <summary>
   /// Basic do step routine. This method must be implemented by the MMU developer.
   /// </summary>
   /// <param name="time"></param>
   /// <param name="avatarState"></param>
   /// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState state)
{
   //Create a new simulation result
   MSimulationResult result = new MSimulationResult()
   {
      Posture = state.Current,
      Constraints = state.Constraints != null ? state.Constraints : new List<MConstraint>(),
      Events = state.Events != null ? state.Events : new List<MSimulationEvent>(),
      SceneManipulations = state.SceneManipulations != null ? state.SceneManipulations : new List<MSceneManipulation>(),
   };

   //Execute the instruction on the main thread (required in order to access unity functionality)
   this.ExecuteOnMainThread(() =>
   {
      this.AssignPostureValues(state.Current);
      this.animator.Update((float)time);

      if (this.animator.GetCurrentAnimatorStateInfo(0).IsName("Idle"))
      {
         result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
      }

      MAvatarPostureValues simVals = this.GetRetargetedPosture();
      result.Posture = simVals;
   });

   //To do -> insert your do step code in here

   return result;
}


   /// <summary>
   /// Method to return the boundary constraints.
   /// Method can be optionally implemented by the developers.
   /// </summary>
   /// <param name="instruction"></param>
   /// <returns></returns>
   public override List<MConstraint> GetBoundaryConstraints(MInstruction instruction)
   {
      return new List<MConstraint>();
   }

   /// <summary>
   /// Method checks if the prerequisites for starting the instruction are fulfilled.
   /// </summary>
   /// <param name="instruction"></param>
   /// <returns></returns>
   public override MBoolResponse CheckPrerequisites(MInstruction instruction)
   {
      return new MBoolResponse(true);
   }

}