/**
Base class for six-legged walker vehicles.

Copyright (c) 2016, Wormbo

(1) This source code and any binaries compiled from it are provided "as-is",
without warranty of any kind. (In other words, if it breaks something for you,
that's entirely your problem, not mine.)
(2) You are allowed to reuse parts of this source code and binaries compiled
from it in any way that does not involve making money, breaking applicable laws
or restricting anyone's human or civil rights.
(3) You are allowed to distribute binaries compiled from modified versions of
this source code only if you make the modified sources available as well. I'd
prefer being mentioned in the credits for such binaries, but please do not make
it seem like I endorse them in any way.
(also see http://wormbo.de/license/)
*/

class SpiderTank extends ONSVehicle;


//=============================================================================
// Imports
//=============================================================================

#exec obj load file=SpiderTankMeshes.ukx


//=============================================================================
// Constants
//=============================================================================

const NUM_LEGS = 6;


//=============================================================================
// Structs
//=============================================================================

struct TSpiderLeg {
	var name LegTurnBone;
	var name LegLiftBone;
	var name KneeBone;
	var name FootBone;
	var name TipBone;
};

struct TSpiderLegState {
	var bool bFootOnGround;
	var vector FootWorldLocation;
	var vector FootLocalLocation;
	var Actor FootBase;
};

struct TSpiderTankState {
	var vector ChassisPosition;
	var Quat ChassisQuaternion;
	var vector ChassisLinVel;
	var vector ChassisAngVel;

	var rotator ServerTSR; // Thrust, Strafe, Rise (exploiting rotator compression properties)
	var int ServerView; // pitch and yaw
	
	var TSpiderLegState LegStates[NUM_LEGS];
};


//=============================================================================
// Properties
//=============================================================================

var() TSpiderLeg SpiderLegs[NUM_LEGS];
var() editconst editinline SpiderTankLegController LegControllers[NUM_LEGS];

var() float HoverSoftness;
var() float HoverPenScale;
var() float HoverCheckDist;

var() float UprightStiffness;
var() float UprightDamping;

var() float FootSoftness;
var() float FootPenScale;
var() float FootCheckDist;

var() float GroundLinearDamping, GroundAngularDamping, GroundMaxSpeed;
var() float AirLinearDamping, AirAngularDamping, AirMaxSpeed;

var() float MaxThrustForce;
var() float LongDamping;

var() float MaxStrafeForce;
var() float LatDamping;

var() float TurnTorqueFactor;
var() float TurnTorqueMax;
var() float MaxYawRate;


//=============================================================================
// Variables
//=============================================================================

var float OutputThrust;
var float OutputStrafe;
var float OutputRise;

var TSpiderTankState SpiderTankState;
var TSpiderTankState OldSpiderTankState;
var KRigidBodyState ChassisState;
var bool bNewSpiderTankState;

var bool bOnGround;

var float TargetHeading;
var bool bHeadingInitialized;

var bool bUpdateForces;
var vector UpdatedForce, UpdatedTorque;


//=============================================================================
// Replication
//=============================================================================

replication
{
	reliable if (Role == ROLE_Authority)
		SpiderTankState;
}


simulated function DrawHUD(Canvas Canvas)
{
	Super.DrawHUD(Canvas);
}

simulated function PostNetBeginPlay()
{
	InitializeLegs();
	
	Super.PostNetBeginPlay();

	if (Role == ROLE_Authority)
	{
		if (Weapons.Length == 2 && ONSLinkableWeapon(Weapons[0]) != None)
			ONSLinkableWeapon(Weapons[0]).ChildWeapon = Weapons[1];
	}
}

function DriverLeft()
{
	Super.DriverLeft();
	Weapons[1].bActive = False;
}

simulated function ClientKDriverEnter(PlayerController PC)
{
	bHeadingInitialized = False;

	Super.ClientKDriverEnter(PC);
}

function KDriverEnter(Pawn P)
{
	Super.KDriverEnter(P);

	bHeadingInitialized = False;
	if (Weapons.Length == 2 && Weapons[1] != None)
		Weapons[1].bActive = True;
}

simulated event SetInitialState()
{
	Super.SetInitialState();

	Enable('Tick'); // make sure it stays enabled!
}

simulated function InitializeLegs()
{
	local int i;
	local KRepulsor Rep;
	local KarmaParams KP;
	local Coords FootPos;
	
	KP = KarmaParams(KParams);
	
	if (KP != None) {
		Rep = Spawn(class'KRepulsor', Self, '', Location);
		if (Rep != None) {
			Rep.SetBase(Self);
			Rep.bHidden = True;
			KP.Repulsors[0] = Rep;
		}
	}
	
	for (i = 0; i < NUM_LEGS; i++)
	{
		FootPos = GetBoneCoords(SpiderLegs[i].FootBone);
		
		if (KP != None) {
			Rep = Spawn(class'KRepulsor', Self, '', FootPos.Origin);
			if (Rep != None) {
				Rep.bEnableRepulsion = True;
				AttachToBone(Rep, SpiderLegs[i].FootBone);
				Rep.bHidden = True;
				KP.Repulsors[KP.Repulsors.Length] = Rep;
			}
		}
		
		LegControllers[i] = new(Self) class'SpiderTankLegController';
		LegControllers[i].Initialize(i, Rep);
	}
}

simulated function Destroyed()
{
	local KarmaParams KP;
	local int i;
	
	Super.Destroyed();
	
	KP = KarmaParams(KParams);
	if (KP != None) {
		for (i = 0; i < KP.Repulsors.Length; i++) {
			KP.Repulsors[i].Destroy();
		}
		KP.Repulsors.Length = 0;
	}
}

simulated event SVehicleUpdateParams()
{
	local int i;
	local KarmaParams KP;

	Super.SVehicleUpdateParams();
	
	KSetStayUprightParams(UprightStiffness, UprightDamping);
	
	KP = KarmaParams(KParams);

	if (KP != None && KP.Repulsors.Length > 0) {
		KP.Repulsors[0].Softness = HoverSoftness;
		KP.Repulsors[0].PenScale = HoverPenScale;
		KP.Repulsors[0].CheckDist = HoverCheckDist;
		
		for (i = 1; i < KP.Repulsors.Length; i++) {
			KP.Repulsors[i].Softness = FootSoftness;
			KP.Repulsors[i].PenScale = FootPenScale;
			KP.Repulsors[i].CheckDist = FootCheckDist;
		}
	}
}


simulated function UpdateVehicle(float DeltaTime)
{
	local KarmaParams KP;
	local rotator FlatRot, LookRot;
	local vector DirX, DirY, DirZ, Forward, Right, Up;
	local KRigidBodyState RBState;
	local vector LinVel, AngVel;
	local vector Force, Torque;
	local vector LookDir, PlaneLookDir;
	local float ForwardVelMag, RightVelMag, AirControlModifier;
	local float CurrentHeading, DesiredHeading, DeltaTargetHeading, MaxDeltaHeading, DeltaHeading, TurnTorqueMag;

	if (!KIsAwake())
		return; // no point doing this if Karma model not awake
	
	KP = KarmaParams(KParams);
	if (KP == None)
		return; // need Karma parameters to do stuff
	
	if (Controller != None) {
		
		GetAxes(Rotation, DirX, DirY, DirZ);
		FlatRot.Yaw = Rotation.Yaw; // only use yaw to calculate directional input
		GetAxes(FlatRot, Forward, Right, Up);
		
		KGetRigidBodyState(RBState);
		AngVel = KRBVecToVector(RBState.AngVel);
		LinVel = KRBVecToVector(RBState.LinVel);
		
		ForwardVelMag = Velocity dot Forward;
		RightVelMag = Velocity dot Right;
		
		if (bOnGround)
			AirControlModifier = 1.0;
		else
			AirControlModifier = 0.1;
		
		// Thrust
		Force += OutputThrust * MaxThrustForce * AirControlModifier * Forward;
		Force -= (1.0f - Abs(OutputThrust)) * AirControlModifier * LongDamping * ForwardVelMag * Forward;
		
		// Strafe
		Force += -OutputStrafe * MaxStrafeForce * AirControlModifier * Right;
		Force -= (1.0f - Abs(OutputStrafe)) * AirControlModifier * LatDamping * RightVelMag * Right;
		
		LookRot.Pitch = DriverViewPitch;
		LookRot.Yaw = DriverViewYaw;
		LookRot.Roll = 0;
		LookDir = vector(LookRot);
		
		PlaneLookDir = Normal(LookDir * vect(1,1,0));
		CurrentHeading = HeadingAngle(Forward);
		DesiredHeading = HeadingAngle(PlaneLookDir);
		
		if (!bHeadingInitialized)
		{
			TargetHeading = CurrentHeading;
			bHeadingInitialized = true;
		}
		
		// Move 'target heading' towards 'desired heading' as fast as MaxYawRate allows.
		DeltaTargetHeading = FindDeltaAngle(TargetHeading, DesiredHeading);
		MaxDeltaHeading = DeltaTime * MaxYawRate;
		DeltaTargetHeading = FClamp(DeltaTargetHeading, -MaxDeltaHeading, MaxDeltaHeading);
		TargetHeading = UnwindHeading(TargetHeading + DeltaTargetHeading);
		
		// Then put a 'spring' on the copter to target heading.
		DeltaHeading = FindDeltaAngle(CurrentHeading, TargetHeading);
		TurnTorqueMag = (DeltaHeading / Pi) * TurnTorqueFactor;
		TurnTorqueMag = FClamp(TurnTorqueMag, -TurnTorqueMax, TurnTorqueMax);
		Torque += TurnTorqueMag * Up;
		
		UpdatedForce = Force;
		UpdatedTorque = Torque;
		bUpdateForces = True;
	}
}


simulated event KApplyForce(out vector Force, out vector Torque)
{
	if (bUpdateForces) {
		Force = UpdatedForce;
		Torque = UpdatedTorque;
		bUpdateForces = False;
	}
}


simulated function Tick(float DeltaTime)
{
	local int i;
	local KarmaParams KP;
	local int NumOnGround;

	Super.Tick(DeltaTime);
	
	Enable('Tick'); // make sure it stays enabled!
	
	for (i = 0; i < NUM_LEGS; i++) {
		LegControllers[i].CalculateLegState(DeltaTime);
		if (LegControllers[i].bIsOnGround)
			NumOnGround++;
	}
	
	KP = KarmaParams(KParams);
	if (KP != None) {
		if (!bOnGround && NumOnGround > 1) {
			bOnGround = True;
			KP.KMaxSpeed = GroundMaxSpeed;
			KP.KLinearDamping = GroundLinearDamping;
			KP.KAngularDamping = GroundAngularDamping;
		}
		else if (bOnGround && NumOnGround <= 1) {
			bOnGround = False;
			KP.KMaxSpeed = AirMaxSpeed;
			KP.KLinearDamping = AirLinearDamping;
			KP.KAngularDamping = AirAngularDamping;
		}
	}
	
	for (i = 0; i < NUM_LEGS; i++) {
		LegControllers[i].UpdateLegState(DeltaTime);
	}
	
	KWake();
	
	if (Role == ROLE_Authority)
	{
		if (Driver != None)
		{
			OutputThrust = Throttle;
			OutputStrafe = Steering;

		}

		PackState();
	}
}

function PackState()
{
	local KRigidBodyState RBState;
	local TSpiderTankState NewSpiderTankState;
	local rotator ViewRot;
	local int i;
	
	KGetRigidBodyState(RBState);
	
	// will be rounded anyway during replication, so might as well do it now
	NewSpiderTankState.ChassisPosition.X = Round(RBState.Position.X);
	NewSpiderTankState.ChassisPosition.Y = Round(RBState.Position.Y);
	NewSpiderTankState.ChassisPosition.Z = Round(RBState.Position.Z);
	
	// simulate the replication compression of quaternions
	NewSpiderTankState.ChassisQuaternion = NormalizeQuat(RBState.Quaternion);
	
	NewSpiderTankState.ChassisLinVel.X = Round(10.0 * RBState.LinVel.X);
	NewSpiderTankState.ChassisLinVel.Y = Round(10.0 * RBState.LinVel.Y);
	NewSpiderTankState.ChassisLinVel.Z = Round(10.0 * RBState.LinVel.Z);
	
	NewSpiderTankState.ChassisAngVel.X = Round(1000.0 * RBState.AngVel.X);
	NewSpiderTankState.ChassisAngVel.Y = Round(1000.0 * RBState.AngVel.Y);
	NewSpiderTankState.ChassisAngVel.Z = Round(1000.0 * RBState.AngVel.Z);
	
	NewSpiderTankState.ServerTSR.Pitch = FloatToRangeByte(OutputThrust) << 8;
	NewSpiderTankState.ServerTSR.Yaw   = FloatToRangeByte(OutputStrafe) << 8;
	NewSpiderTankState.ServerTSR.Roll  = FloatToRangeByte(OutputRise)	<< 8;
	
	if (Controller != None)
	{
		if (IsHumanControlled())
			ViewRot = Controller.Rotation;
		else
			ViewRot = rotator(Controller.FocalPoint - Location);
	}
	else
	{
		ViewRot = Rotation;
	}

	DriverViewPitch = ViewRot.Pitch;
	DriverViewYaw = ViewRot.Yaw; 
	NewSpiderTankState.ServerView = (DriverViewYaw & 0xffff) | DriverViewPitch << 16;
	
	for (i = 0; i < NUM_LEGS; i++) {
		if (LegControllers[i] != None) {
			NewSpiderTankState.LegStates[i].bFootOnGround = LegControllers[i].bIsOnGround;
			NewSpiderTankState.LegStates[i].FootWorldLocation = RoundVector(LegControllers[i].FootWorldLocation);
			NewSpiderTankState.LegStates[i].FootLocalLocation = RoundVector(LegControllers[i].FootLocalLocation);
			NewSpiderTankState.LegStates[i].FootBase = LegControllers[i].RepulsorGroundActor;
		}
	}
	
	if (SpiderTankState != NewSpiderTankState)
		SpiderTankState = NewSpiderTankState;
}

simulated function PostNetReceive()
{
	Super.PostNetReceive();
	
	if (OldSpiderTankState != SpiderTankState) {
		ChassisState.Position.X = SpiderTankState.ChassisPosition.X;
		ChassisState.Position.Y = SpiderTankState.ChassisPosition.Y;
		ChassisState.Position.Z = SpiderTankState.ChassisPosition.Z;
		
		ChassisState.Quaternion = SpiderTankState.ChassisQuaternion;
		
		ChassisState.LinVel.X = 0.1 * SpiderTankState.ChassisLinVel.X;
		ChassisState.LinVel.Y = 0.1 * SpiderTankState.ChassisLinVel.Y;
		ChassisState.LinVel.Z = 0.1 * SpiderTankState.ChassisLinVel.Z;
		
		ChassisState.AngVel.X = 0.001 * SpiderTankState.ChassisAngVel.X;
		ChassisState.AngVel.Y = 0.001 * SpiderTankState.ChassisAngVel.Y;
		ChassisState.AngVel.Z = 0.001 * SpiderTankState.ChassisAngVel.Z;
		
		OutputThrust = RangeByteToFloat(SpiderTankState.ServerTSR.Pitch >> 8);
		OutputStrafe = RangeByteToFloat(SpiderTankState.ServerTSR.Yaw >> 8);
		OutputRise	 = RangeByteToFloat(SpiderTankState.ServerTSR.Roll >> 8);
		DriverViewPitch = SpiderTankState.ServerView >> 16;
		DriverViewYaw	= SpiderTankState.ServerView & 0xffff;
		
		OldSpiderTankState = SpiderTankState;
		bNewSpiderTankState = True;
	}
}


simulated event bool KUpdateState(out KRigidBodyState NewState)
{
	local int i;
	
	if (bNewSpiderTankState) {
		for (i = 0; i < NUM_LEGS; i++) {
			if (LegControllers[i] != None) {
				LegControllers[i].bIsOnGround = SpiderTankState.LegStates[i].bFootOnGround;
				LegControllers[i].FootWorldLocation = SpiderTankState.LegStates[i].FootWorldLocation;
				LegControllers[i].FootLocalLocation = SpiderTankState.LegStates[i].FootLocalLocation;
				LegControllers[i].RepulsorGroundActor = SpiderTankState.LegStates[i].FootBase;
				LegControllers[i].ReceivedLegState();
			}
		}
		
		NewState = ChassisState;
		bNewSpiderTankState = False;
		return true;
	}
	
	return false;
}


function TakeDamage(int Damage, Pawn InstigatedBy, Vector HitLocation, Vector Momentum, class<DamageType> DamageType)
{
	local float MomentumScale;
	
	if (HitLocation == Location) {
		MomentumScale = 1.0;
	}
	else {
		MomentumScale = 0.01 + 0.99 * Square(Normal(Momentum) dot Normal(Location - HitLocation));
	}
	
	// don't destroy immediately when upside-down; scale momentum when applied off-center
	Super(SVehicle).TakeDamage(Damage, InstigatedBy, HitLocation, Momentum * MomentumScale, DamageType);
}


static function byte FloatToRangeByte(float f)
{
	// Maps 0.0 to 0, [-1.0;0.0) to [129..255] and (0.0;1.0] to [1..127].
	return int(Round(127 * FClamp(f, -1, 1))); // exploits binary truncation of float->int->byte
}

static function float RangeByteToFloat(byte b)
{
	// maps byte values [129..255, 0..127] to int values [-127..127], then scales to float range [-1.0;1.0]
	return (b << 24 >> 24) / 127.0;
}

static function vector RoundVector(vector v)
{
	v.X = Round(v.X);
	v.Y = Round(v.Y);
	v.Z = Round(v.Z);
	return v;
}

static function Quat NormalizeQuat(Quat q)
{
	if (q.W < 0.0) {
		// replication normalizes to non-negative W (q and -q represent the same orientation)
		q.W = -q.W;
		q.X = -q.X;
		q.Y = -q.Y;
		q.Z = -q.Z;
	}
	
	// W will be regenerated like this during replication
	q.W = Sqrt(1.0 - Square(q.X) - Square(q.Y) - Square(q.Z));
	
	return q;
}


static final function float HeadingAngle(vector Dir)
{
	local float Angle;
	
	Angle = ACos(Dir.X);
	
	if (Dir.Y < 0)
		return -Angle;
	
	return angle;
}

static final function float FindDeltaAngle(float a1, float a2)
{
	local float delta;
	
	delta = a2 - a1;
	
	if (delta > Pi)
		delta = delta - Pi * 2.0;
	else if (delta < -Pi)
		delta = delta + Pi * 2.0;
	
	return delta;
}

static final function float UnwindHeading(float a)
{
	while(a > Pi)
		a -= Pi * 2.0;
	
	while(a < -Pi)
		a += Pi * 2.0;
	
	return a;
}


//=============================================================================
// Default values
//=============================================================================

defaultproperties
{
	Mesh = SkeletalMesh'SpiderTankChassis'
	CollisionRadius = 170.0
	CollisionHeight =  75.0
	
	// TODO
	RedSkin = Texture'SpiderMineTEX'
	BlueSkin = Texture'SpiderMineBLUETEX'

	bNetNotify = True
	
	Health = 10000
	HealthMax = 10000
	
	EntryRadius = 200.0
	ExitPositions(0) = (X=50,Y=190,Z=0)
	ExitPositions(1) = (X=50,Y=-190,Z=0)
	ExitPositions(2) = (X=-150,Y=150,Z=0)
	ExitPositions(3) = (X=-150,Y=-150,Z=0)
	ExitPositions(4) = (X=-150,Y=0,Z=70)
	
	SpiderLegs(0) = (LegTurnBone=LegAttachLF,LegLiftBone=FemurLF,KneeBone=TibiaLF,FootBone=TarsusLF,TipBone=TipLF)
	SpiderLegs(1) = (LegTurnBone=LegAttachLM,LegLiftBone=FemurLM,KneeBone=TibiaLM,FootBone=TarsusLM,TipBone=TipLM)
	SpiderLegs(2) = (LegTurnBone=LegAttachLR,LegLiftBone=FemurLR,KneeBone=TibiaLR,FootBone=TarsusLR,TipBone=TipLR)
	SpiderLegs(3) = (LegTurnBone=LegAttachRF,LegLiftBone=FemurRF,KneeBone=TibiaRF,FootBone=TarsusRF,TipBone=TipRF)
	SpiderLegs(4) = (LegTurnBone=LegAttachRM,LegLiftBone=FemurRM,KneeBone=TibiaRM,FootBone=TarsusRM,TipBone=TipRM)
	SpiderLegs(5) = (LegTurnBone=LegAttachRR,LegLiftBone=FemurRR,KneeBone=TibiaRR,FootBone=TarsusRR,TipBone=TipRR)
	
	HoverSoftness = 0.1
	HoverPenScale = 1.0
	HoverCheckDist = 120.0
	
	UprightStiffness = 100
	UprightDamping = 100
	
	FootSoftness = 0.01
	FootPenScale = 2.5
	FootCheckDist = 25.0
	
	GroundMaxSpeed = 700.0
	GroundLinearDamping = 5.0
	GroundAngularDamping = 5.0
	AirMaxSpeed = 2000.0
	AirLinearDamping = 1.0
	AirAngularDamping = 1.0
	
	MaxThrustForce=100
	LongDamping=0.02
	
	MaxStrafeForce=100
	LatDamping=0.02
	
	TurnTorqueFactor=500.0
	TurnTorqueMax=75.0
	MaxYawRate=0.5
	
	VehicleMass = 4.0
	
	Begin Object Class=KarmaParamsRBFull Name=SpiderTankKParams
		KStartEnabled=True
		KFriction=0.5
		KLinearDamping=5.0
		KAngularDamping=3.0
		KImpactThreshold=700
		bKNonSphericalInertia=False
		bHighDetailOnly=False
		bClientOnly=False
		bKDoubleTickRate=True
		bKStayUpright=True
		bKAllowRotate=True
		KMaxSpeed=700.0
		KMaxAngularSpeed=2.0
		KInertiaTensor(0)=2.0
		KInertiaTensor(1)=0.0
		KInertiaTensor(2)=0.0
		KInertiaTensor(3)=2.5
		KInertiaTensor(4)=0.0
		KInertiaTensor(5)=4.0
		KCOMOffset=(X=0.0,Y=0.0,Z=0.0)
		bDestroyOnWorldPenetrate=True
		bDoSafetime=True
	End Object
	KParams=SpiderTankKParams
	
	DriverWeapons(0) = (WeaponClass=class'SpiderTankWebLauncherFront',WeaponBone=FrontGunAttack)
	DriverWeapons(1) = (WeaponClass=class'SpiderTankWebLauncherRear',WeaponBone=RearGunAttach)
}
