--!optimize 2
--!native
--!strict

type Head            = BasePart
type UpperTorso      = BasePart
type Neck            = Motor6D
type Waist           = Motor6D
type NeckC0          = CFrame
type WaistC0         = CFrame
type UDP             = UnreliableRemoteEvent
type CharacterShared = Folder

type struct_SpineMovement = {
	Neck: CFrame,
	Waist: CFrame
}

type ClassConstructor = typeof(setmetatable({} :: Constructor_Return_Props, {} :: Impl_Constructor))
type Impl_Constructor = {
	__index: Impl_Constructor,
	constructor: Constructor_Fun,
	--Class functions
	Move: (self: ClassConstructor, CameraCFrame: CFrame, IsFirstPerson: boolean) -> ()
} & Impl_Static_Props

type Constructor_Fun = (CharacterShared: CharacterShared, Head: Head, UpperTorso: UpperTorso) -> ClassConstructor
type Impl_Static_Props = {
	Enabled: boolean,
	Multiplier: number
}
type Constructor_Return_Props = {	
	Remote: UDP,
	Head: Head,
	UpperTorso: UpperTorso,
	Neck: Neck,
	Waist: Waist,
	NeckC0: NeckC0,
	WaistC0: WaistC0
}

export type SpineKinematicsConstructor = ClassConstructor

local Spine = {} :: Impl_Constructor
Spine.__index = Spine

Spine.Enabled = true
Spine.Multiplier = .5

function Spine.constructor(CharacterShared, Head, UpperTorso)
	local self = {}
	self.Remote = Instance.new("UnreliableRemoteEvent") :: UDP
	self.Remote.Name = "SpineStream"
	self.Remote.Parent = CharacterShared

	self.Head = Head
	self.UpperTorso = UpperTorso
	self.Neck = Head:WaitForChild("Neck") :: Motor6D
	self.Waist = UpperTorso:WaitForChild("Waist") :: Motor6D
	self.NeckC0 = (self.Neck :: Motor6D).C0
	self.WaistC0 = (self.Neck :: Motor6D).C0

	return setmetatable(self, Spine)
end

local function SpineMovement(self: ClassConstructor, CameraCFrame: CFrame, IsFirstPerson: boolean): struct_SpineMovement
	local HeadCFrame:      CFrame  = self.Head.CFrame
	local TorsoPosition:   Vector3 = self.UpperTorso.Position
	local TorsoLookVector: Vector3 = self.UpperTorso.CFrame.LookVector
    local HeadPosition:    Vector3 = HeadCFrame.Position
	local CameraPosition:  Vector3 = CameraCFrame.Position

	local HeadDelta:        Vector3 = HeadPosition-CameraPosition
	local TorsoDeltaNormal: Vector3 = (TorsoPosition-CameraPosition).Unit
	local HeadDeltaNormal:  Vector3 = HeadDelta.Unit
	local HeadDeltaMagnitude: number = HeadDelta.Magnitude

	local arc = Spine.Multiplier*math.asin(HeadDelta.Y/HeadDeltaMagnitude)
	local Neck = CFrame.Angles(arc, 0, 0)
	local Waist = CFrame.Angles(arc, 0, 0)
    
    if not IsFirstPerson then
		--Make this less cringe at some point, combine the dot product into the equation
		local LookingAtSelf = CameraCFrame.LookVector:Dot(HeadCFrame.LookVector)<0 --Make our head face the camera if we are looking at ourself
		local HeadCrossDelta = Spine.Multiplier*HeadDeltaNormal:Cross(TorsoLookVector).Y
		local TorsoCrossDelta = Spine.Multiplier*TorsoDeltaNormal:Cross(TorsoLookVector).Y

		Neck*=CFrame.Angles(0, HeadCrossDelta, 0)
		Waist*=CFrame.Angles(0, LookingAtSelf and TorsoCrossDelta or -TorsoCrossDelta, 0)
    end
	
	return {
		Neck = Neck,
        Waist = Waist
	}
end

function Spine:Move(CameraCFrame, IsFirstPerson)
	local SpineIK = SpineMovement(self, CameraCFrame, IsFirstPerson)

	self.Neck.C0 = self.Neck.C0:Lerp(self.NeckC0*SpineIK.Neck, .9)
	self.Waist.C0 = self.Waist.C0:Lerp(self.WaistC0*SpineIK.Waist, .9)
end

return Spine