/*******************************************************************************

File: CreatureStandup.cs
Author: Felix Knight
DP Email: felix.knight@digipen.edu
Date: 3/29/2021
Course: CS199
Section: A
Description:
Automatically creates IKTargets for any attached legs that don't already have
one, allows the creature to respond to gravity and tarrain variation, and
forces its legs to step in a natural, alternating pattern.

IKTargets are placed at the end of each leg so the user may control their
stance and gait simply by adjusting the starting rotation of the legs, down
to their joints.

Upward force is applied based on how many legs are grounded and how extended
they are.

Legs are sorted into pairs and forced to step in an alternating pattern as
they are extended.
*******************************************************************************/

using System.Collections.Generic;
using UnityEngine;

public class CreatureStandup : MonoBehaviour

{
[Tooltip("IKTarget prefab to spawn at the end of each leg, if one isn't already set")]
public GameObject IKTargetPrefab;

[Tooltip("How much strength a leg can exert based on its current extension. Should max out near a 90-degree angle.")]
public AnimationCurve LegStrengthByExtension;

[Tooltip("How much force the body should be able to use based on how many legs are currently grounded. Roughly half the
legs should be able to keep it aloft.")]
public AnimationCurve LegStrengthByProportionGrounded;

[Tooltip("Overall force the legs can exert upward to counteract gravity")]

public float LegLiftForce = 1f;

[Tooltip("Maximum difference in relative Z positions for legs to still be paired together")]

public float LegPairZThresh = 0.15f;

[Tooltip("Minimum extension threshold both legs in a pair must reach before one is forced to step")]
public float PairStepExtensionThresh = 0.5f;

[Tooltip("Minimum velocity for the body to move before legs will be forced to step alternating")]
public float PairStepVelocityThresh = 0.5f;

[Tooltip("Minimum distance threshold both legs in a pair must reach from their step target position before one is forced to
step")]
public float PairStepTargetDistThresh = 0.1f;

//attached objects with LegIK and IKtargetMover components

[HideInInspector]
public List<LegIK> Legs;

[HideInInspector]
public List<IKTargetMover> IKTargets;

//torque vector to apply to the body to align it with the target determined
//by a BodyRotator component
[HideInInspector]
public Vector3 GroundAlignTorque = new Vector3();
//whether at least one leg is currently grounded
[HideInInspector]
public bool Grounded;

//current upward force being exerted by the legs

private float CurrentLegForce;

//list of all pairs of IKTargets

private List<TargetPair> TargetPairs;

private Rigidbody RB;

private BodyRotator RotatorComp;

private void Awake()

{
RB = GetComponent<Rigidbody>();
RotatorComp = GetComponent<BodyRotator>();

//get attached legs and IKTargets

GetComponentsInChildren<LegIK>(Legs);
GetIKTargets();

//find pairs of targets

TargetPairs = GetTargetPairs();
}

private void Start()

{
//use gravity automatically if this component is enabled
RB.useGravity = true;
}

// Update is called once per frame

void Update()
{
//update CurrentLegForce
CurrentLegForce = GetTotalLegStrength(LegStrengthByExtension);

//check if any leg pairs need to step and force them to step if necessary
UpdateLegPairsStep();
}

private void FixedUpdate()

{
//add upward force to counteract gravity
RB.AddForce(Vector3.up * CurrentLegForce * LegLiftForce);

//add torque set by a BodyRotator component to keep the body aligned with the ground
RB.AddTorque(GroundAlignTorque);
}

//get any existing IKTargets, and make new ones for any Legs that don't have one.
private void GetIKTargets()
{
//get any existing IKTargets
GetComponentsInChildren<IKTargetMover>(IKTargets);

//list of legs that don't have targets yet- default to list of all legs
List<LegIK> legsWithoutTargets = Legs;

//for every existing target- remove its assigned leg from the list of legs to create new targets for
foreach (IKTargetMover target in IKTargets)
{
legsWithoutTargets.Remove(target.Leg);
}
 //make an IKTarget for each leg without one
foreach (LegIK leg in legsWithoutTargets)
{
//create a new IKTargetPrefab at the foot position of the given leg
Vector3 pos = leg.GetFootPosition();
Quaternion rot = Quaternion.identity;
GameObject targetObj = Instantiate(IKTargetPrefab, pos, rot);

//child the target object to this transform

targetObj.transform.SetParent(this.transform);

//get the target mover script and add it to the list of IKTargets
IKTargetMover target = targetObj.GetComponent<IKTargetMover>();
IKTargets.Add(target);

//set the leg on the target

target.SetLeg(leg);
}

//set the target's standup component to this

foreach (IKTargetMover target in IKTargets)
{
target.Standup = this;
}
}

//gets the extension strength of all given legs divided by half their amount.
//this allows calibration of the lift force so that half the legs can support the body.
private float GetTotalLegStrength(AnimationCurve strengthCurve)
{
//maximum strength of all combined legs
float sumStrength = 0f;

int legsGrounded = 0;

//iterate through each IKTarget

foreach (IKTargetMover target in IKTargets)
{
//if the leg is grounded, add its strength based on its
//current extension and the passed strength curve
if (target.GetGrounded())
{
float extension = target.Leg.GetCurrentLengthRatio;
sumStrength += strengthCurve.Evaluate(extension);

//add to the number of legs grounded

++legsGrounded;
}
}

//proceed if any legs are grounded- ensures no division by 0

if(legsGrounded > 0)
{
Grounded = true;

//average maximum strength of all grounded legs

float avgLegStrength = sumStrength / legsGrounded;

//calculate proportion of legs grounded

float proportionGrounded = (float)legsGrounded / IKTargets.Count;

//factor for leg strength by how many are grounded

float amountStrength = LegStrengthByProportionGrounded.Evaluate(proportionGrounded);

return (amountStrength * avgLegStrength);

}
 //otherwise, return 0 to not apply any force
else
{
Grounded = false;

return 0f;
}
}

//gets pairs of IK by closeness of their legs on the Z axis

private List<TargetPair> GetTargetPairs()
{
List<TargetPair> targetPairs = new List<TargetPair>();

//paired targets to not be evaluated for more pairs

List<IKTargetMover> pairedTargets = new List<IKTargetMover>();

//iterate through all targets

foreach (IKTargetMover target1 in IKTargets)
{
//continue if this target hasn't been paired yet
if (!pairedTargets.Contains(target1))
{
//get this target's leg's relative Z
float relativeZ = target1.Leg.transform.localPosition.z;

//check all other targets that haven't been paired and aren't this target
foreach (IKTargetMover target2 in IKTargets)
{
if (!pairedTargets.Contains(target1) && target2 != target1)
{
//get the second target's leg's relative Z
float RelativeZ2 = target2.Leg.transform.localPosition.z;

//if the Z distance between the legs is less than the

//Z threshold, pair them
if (Mathf.Abs(relativeZ - RelativeZ2) < LegPairZThresh)
{
//create a pair with these targets, and that pair to the list of pairs
targetPairs.Add(new TargetPair(target1, target2));

//add these targets to the paired targets list

//so they won't be evaluated any more
pairedTargets.Add(target1);
pairedTargets.Add(target2);

//pair found, stop searching for this pair (but continue searching for pairs)
break;
}

}
}
}
}

//sort the leg pairs by relative z first

targetPairs.Sort(new FrontTargetCompare());

return targetPairs;
}

//gets the horizontal velocity of the body

public float GetXZVelocity()
{
//copy velocity, set the y axis to 0, and gets its magnitude to calc XZ Velocity
Vector3 XZVector = RB.velocity;
 XZVector.y = 0f;
return Vector3.Magnitude(XZVector);
}

//go through all target pairs, forcing one leg in a pair to step if they're both extended
//beyond a threshold and grounded.
private void UpdateLegPairsStep()
{
//continue if the velocity is above the threshold
if (GetXZVelocity() > PairStepVelocityThresh)
{
//iterate through all leg pairs
for (int i = 0; i < TargetPairs.Count; i++)
{
//number of legs in the pair that have met the conditions to step
int legsCanStep = 0;

//check both legs in the pair

for (int j = 0; j < TargetPairs[i].IKTargets.Count; j++)
{
//get the current target
IKTargetMover currentTarget = TargetPairs[i].IKTargets[j];

//whether the leg's extension has passed the step threshold

bool extendThresh = currentTarget.Leg.GetCurrentLengthRatio > PairStepExtensionThresh;

//whether the distance between the targets has passed the threshold
bool distThresh = currentTarget.TargetsAreDistant(PairStepTargetDistThresh);

//conditions- leg's not stepping, extended beyond the threshold, and

//the distance between the targets is beyond the threshold
if (!currentTarget.GetStepping() && extendThresh && distThresh)
{
//if it's not stepping and past the threshold, add to the legs that meet conditions
++legsCanStep;
}
}

//if both legs in the pair meet the conditions, force one to step
if (legsCanStep >= 2)
{
int indexToStep = 0;

//step with the leg opposite of the front pair's last step
if (ForwardPairLastStep(i) == 0)
{
indexToStep = 1;
}
else
{
indexToStep = 0;
}

//force this leg to step

TargetPairs[i].IKTargets[indexToStep].Step();
}
}
}
}

//get the last step index taken by the pair in front of this pair
private int ForwardPairLastStep(int origPairIndex)
{
//if target pairs includes an index in front of this pair, check its IKTarget
//for the last step it took
if ((TargetPairs.Count - 1) >= origPairIndex + 1)
 {
return TargetPairs[origPairIndex + 1].LastStepIndex;
}
else
{
return 0;
}
}

//iterate through target pairs until the given target is found, then
//set its index as the LastStepIndex for its pair
public void UpdateTargetStepped(IKTargetMover target)
{
//iterate through pairs
for (int i = 0; i < TargetPairs.Count; i++)
{
//iterate through targets in pair
for (int j = 0; j < TargetPairs[i].IKTargets.Count; j++)
{
//if this is the given target, set its index as the last step index.
if(TargetPairs[i].IKTargets[j] == target)
{
TargetPairs[i].LastStepIndex = j;
return;
}
}
}
}

//force all IKTargets and BodyRotator to set the visibility of their debug meshes
public void ShowDebugMeshes(bool show)
{
foreach (IKTargetMover target in IKTargets)
{
target.ShowDebugMeshes(show);
}

RotatorComp.ShowDebugMeshes(show);
}
}

//pair of leg targets and the index for the last step they took
public class TargetPair
{
//constructor only takes IKtargets, last step defaults to 0 and is changed later
public TargetPair(IKTargetMover target1, IKTargetMover target2)
{
IKTargets = new List<IKTargetMover>();
IKTargets.Add(target1);
IKTargets.Add(target2);

IKTargets.Sort(new LeftTargetCompare());
}

public List<IKTargetMover> IKTargets;

public int LastStepIndex = 0;
}

//comparer to sort leg pairs, higher value for the relative z position first
public class FrontTargetCompare : IComparer<TargetPair>
{
public int Compare(TargetPair pair1, TargetPair pair2)
{
float pair1z = pair1.IKTargets[0].Leg.transform.localPosition.z;
float pair2z = pair2.IKTargets[0].Leg.transform.localPosition.z;
 return pair1z.CompareTo(pair2z);
}
}

//comparer to sort targets, higher value for the relative negative x position first
public class LeftTargetCompare : IComparer<IKTargetMover>
{
public int Compare(IKTargetMover target1, IKTargetMover target2)
{
//negate positions to order them negative first
float target1x = -target1.Leg.transform.localPosition.x;
float target2x = -target2.Leg.transform.localPosition.x;

return target1x.CompareTo(target2x);
}
}