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 = "WaveMMU";

        //Assign the motion type of the MMU
        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);


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

            // Setup Blending Service
            blending_service = this.ServiceAccess.PostureBlendingService;
            blending_service.Setup(avatarDescription, new Dictionary<string, string>());

        });

        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();
            // Make Partial
            simVals = result.Posture.OverwriteWithPartial(simVals.MakePartial(this.partialList));

            var simTime = this.animator.GetCurrentAnimatorStateInfo(0).normalizedTime % 1.0f;
            Debug.Log("SimTime: " + simTime);
            if (simTime < this.start_percentage)
            {
                var blend_weight = simTime / this.start_percentage;
                //Debug.Log("blend from idle: " + blend_weight);
                result.Posture = this.blending_service.Blend(result.Posture, simVals, blend_weight, new Dictionary<MJointType, double>(), new Dictionary<string, string>());
            }
            else if (simTime > 1 - this.end_percentage)
            {
                var blend_weight = 1 - (1 - simTime) / this.end_percentage;
                //Debug.Log("blend to idle: " + blend_weight);
                result.Posture = this.blending_service.Blend(simVals, result.Posture, blend_weight, new Dictionary<MJointType, double>(), new Dictionary<string, string>());
            }
            else
            {
                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);
    }

}

public static class TransformExtensionsWaveMMU
{

    public static MAvatarPostureValues MakePartial(this MAvatarPostureValues vals, List<MJointType> PartialJointList)
    {
        List<MJoint> defaultList = ISDescription.GetDefaultJointList();
        MAvatarPostureValues ret = new MAvatarPostureValues(vals.AvatarID, new List<double>());
        ret.PartialJointList = PartialJointList;
        int id = 0;
        foreach (MJoint joint in defaultList)
        {
            if (PartialJointList.Contains(joint.Type))
            {
                foreach (var x in joint.Channels)
                {
                    ret.PostureData.Add(vals.PostureData[id]);
                    id++;
                }
            }
            else
            {
                id += joint.Channels.Count;
            }
        }

        return ret;
    }

    public static MAvatarPostureValues OverwriteWithPartial(this MAvatarPostureValues vals, MAvatarPostureValues other)
    {
        List<MJoint> defaultList = ISDescription.GetDefaultJointList();
        MAvatarPostureValues ret = new MAvatarPostureValues(vals.AvatarID, new List<double>());

        int id = 0;
        int idPartial = 0;
        foreach (MJoint joint in defaultList)
        {
            if (other.PartialJointList.Contains(joint.Type))
            {
                foreach (var x in joint.Channels)
                {
                    ret.PostureData.Add(other.PostureData[idPartial]);
                    id++;
                    idPartial++;
                }
            }
            else
            {
                foreach (var x in joint.Channels)
                {
                    ret.PostureData.Add(vals.PostureData[id]);
                    id++;
                }
            }
        }

        return ret;

    }

    public static string[] GetChildNameList(this Transform t)
    {
        string[] acc = new string[] { t.name };
        for (int c = 0; c < t.childCount; c++)
        {
            string[] addAcc = t.GetChild(c).GetChildNameList();
            acc = acc.Concat(addAcc).ToArray();
        }
        return acc;
    }

}