﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using RootMotion.FinalIK;

public class LoadRPMAvatarIK : LoadRPMAvatar
{
    [Header("VRIK Calibration")]
    public Transform centerEyeAnchor;
    public Vector3 headAnchorPositionOffset;
    public Vector2 headAnchorRotationOffset;

    [Space]
    public Transform leftHandAnchor;
    public Transform rightHandAnchor;
    public Vector3 handAnchorPositionOffset;
    public Vector3 handAnchorRotationOffset;

    [Space]
    [Tooltip("Fixed scale of the avatar.")] public float fixedScale = 0.013f;
    [Tooltip("Scale of upper arm bones.")] public float armScale = 1f;
    [Tooltip("Hand bone scale.")] public float handScale = 0.9f;

    [Header("Limb Stretching")]
    [Tooltip("Stretch arms by this value when target is at arm's length.")] public float armStretchRest = 0.05f;
    [Tooltip("Maximum arm stretch")] public float armStretchMax = 0.25f;
    [Tooltip("Stretch legs by this value when target is at leg's length.")] public float legStretchRest = 0.05f;
    [Tooltip("Maximum leg stretch")] public float legStretchMax = 0.2f;

    private VRIK ik;

    protected override void AvatarLoadedCallback(GameObject avatar)
    {
        // Load the avatar
        base.AvatarLoadedCallback(avatar);

        // Animator
        var animator = avatar.GetComponentInChildren<Animator>();
        animator.enabled = false;

        // Add VRIK
        ik = animator.gameObject.AddComponent<VRIK>();
        ik.AutoDetectReferences();
        ik.references.neck = null; // Less than great neck skinning, leave it static

        // Fix knees
        Quaternion kneeRot = Quaternion.Euler(-1f, 0.0f, 0f);
        ik.references.leftCalf.localRotation = kneeRot;
        ik.references.rightCalf.localRotation = kneeRot;

        // Set hand axes
        ik.solver.leftArm.wristToPalmAxis = Vector3.up;
        ik.solver.leftArm.palmToThumbAxis = Vector3.right;
        ik.solver.rightArm.wristToPalmAxis = Vector3.up;
        ik.solver.rightArm.palmToThumbAxis = -Vector3.right;

        // Set spine solver parameters
        ik.solver.plantFeet = false;
        ik.solver.spine.headClampWeight = 0.3f;
        ik.solver.spine.maintainPelvisPosition = 0.075f;
        ik.solver.spine.bodyPosStiffness = 0.25f;
        ik.solver.spine.bodyRotStiffness = 0f;
        ik.solver.spine.neckStiffness = 0f;

        // Calibrate
        VRIKCalibrator.Calibrate(ik, centerEyeAnchor, leftHandAnchor, rightHandAnchor, headAnchorPositionOffset, headAnchorRotationOffset, handAnchorPositionOffset, handAnchorRotationOffset);

        // Scale arm bones
        ik.transform.localScale = Vector3.one * fixedScale;
        Vector3 armS = Vector3.one * armScale;
        ik.references.leftUpperArm.localScale = armS;
        ik.references.rightUpperArm.localScale = armS;
        Vector3 handS = Vector3.one * handScale;
        ik.references.leftHand.localScale = handS;
        ik.references.rightHand.localScale = handS;

        // Make sure skinned mesh renderers don't get culled when pulled out of bounds by the IK
        var skinnedMeshes = avatar.GetComponentsInChildren<SkinnedMeshRenderer>();
        foreach (SkinnedMeshRenderer s in skinnedMeshes) s.updateWhenOffscreen = true;

        // Leg stretching (helps to avoid tip-toeing)
        AnimationCurve legStretchCurve = MakeStretchCurve(legStretchRest, legStretchMax);
        ik.solver.leftLeg.stretchCurve = legStretchCurve;
        ik.solver.rightLeg.stretchCurve = legStretchCurve;

        // Arm stretching (helps to avoid elbow snapping)
        AnimationCurve armStretchCurve = MakeStretchCurve(armStretchRest, armStretchMax);
        ik.solver.leftArm.stretchCurve = armStretchCurve;
        ik.solver.rightArm.stretchCurve = armStretchCurve;

        // Improves wrist twisting
        var twistRelaxer = ik.gameObject.AddComponent<TwistRelaxer>();
        twistRelaxer.ik = ik;
        twistRelaxer.twistSolvers = new TwistSolver[2];
        twistRelaxer.twistSolvers[0] = MakeTwistSolver(ik.references.leftForearm, ik.references.leftUpperArm, ik.references.leftHand);
        twistRelaxer.twistSolvers[1] = MakeTwistSolver(ik.references.rightForearm, ik.references.rightUpperArm, ik.references.rightHand);
        foreach (TwistSolver s in twistRelaxer.twistSolvers) s.Initiate();
    }

    private TwistSolver MakeTwistSolver(Transform transform, Transform parent, Transform child)
    {
        var t = new TwistSolver();
        t.transform = transform;
        t.parent = parent;
        t.children = new Transform[1] { child };
        return t;
    }

    private AnimationCurve MakeStretchCurve(float restStretch, float maxStretch)
    {
        var keys = new Keyframe[3]
        {
            new Keyframe(0f, 0f),
            new Keyframe(1f, restStretch),
            new Keyframe(1.3f, maxStretch)
        };
        return new AnimationCurve(keys);
    }
}
