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

local Spine = {
	Enabled = true,
	Multiplier = .5
}
Spine.__index = Spine

type Head = BasePart
type UpperTorso = BasePart
type Neck = Motor6D
type Waist = Motor6D

type NeckC0 = CFrame
type WaistC0 = CFrame

type UDP = UnreliableRemoteEvent

type struct_Spine = {
	Remote: UDP,
	Head: Head,
	UpperTorso: UpperTorso,
	Neck: Neck,
	Waist: Waist,
	NeckC0: NeckC0,
	WaistC0: WaistC0
}

local Remote = Instance.new("UnreliableRemoteEvent")
Remote.Name = "SpineStream"
Remote.Parent = _G.include(script, "CharacterShared")

function Spine.constructor(Head: Head, UpperTorso: UpperTorso)
	local self = {} :: struct_Spine
	self.Head = Head
	self.UpperTorso = UpperTorso
	self.Neck = Head:WaitForChild("Neck") :: Neck
	self.Waist = UpperTorso:WaitForChild("Waist") :: Waist
	self.Remote = Remote :: UDP

	self.NeckC0 = self.Neck.C0
	self.WaistC0 = self.Waist.C0
	return setmetatable(self, Spine)
end

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

local function SpineMovement(self, 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: CFrame, IsFirstPerson: boolean)
	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