FtD Sea Vessel Automatic Stabilization

--Automatic balancing and compensation
vesselPitchCorrectTolerance = 5
vesselRollCorrectTolerance = 5
--How much tolerance you wish to have in your vessel pitch/roll, 5 meaning it'll deviate at most 5 degrees from level pitch/roll.

vesselHydrofoilPitchSpeed = 90
--How fast you want your vessel's hydrofoils to pitch per GAME SECOND (it will slow down when game time slows down.)
--This is in degrees per game second. Hydrofoils can be at most -45 or +45 degrees pitch.
vesselPitchComp = true --Don't touch this. It's needed for calculation inside the code.
function PitchCorrect(I)

	currPitch = I:GetConstructPitch()

	hydrofoilAftArray = {}
	hydrofoilForeArray = {}
	numHydrofoils = I:Component_GetCount(8)
	hydrofoilSpeed = (vesselHydrofoilPitchSpeed / 40)

	j, k = 0, 0

	for i=0,numHydrofoils do
	currHydrofoilPosition = I:Component_GetBlockInfo(8,i).LocalPositionRelativeToCom

	if currHydrofoilPosition.z < 0 then
	hydrofoilAftArray[j] = i
	j = j + 1
	end

	if currHydrofoilPosition.z > 0 then
	hydrofoilForeArray[k] = i
	k = k + 1
	end

	end
	currHydrofoilAft = 0
	currHydrofoilFore = 0
	if table.getn(hydrofoilAftArray) > 0 then currHydrofoilAft = I:Component_GetFloatLogic(8, hydrofoilAftArray[1]) end
	if table.getn(hydrofoilForeArray) > 0 then currHydrofoilFore = I:Component_GetFloatLogic(8, hydrofoilForeArray[1]) end

	if (currPitch > vesselPitchCorrectTolerance) and (currPitch <= 225) then
	I:RequestThrustControl(10)
	BouyancyCorrect(true, I)
	vesselPitchComp = false

	if table.getn(hydrofoilAftArray) > 0 then
		currHydrofoilAft = currHydrofoilAft - hydrofoilSpeed
		for i=0,table.getn(hydrofoilAftArray) do

		I:Component_SetFloatLogic(8, hydrofoilAftArray[i], currHydrofoilAft)

		end
	end

	if table.getn(hydrofoilForeArray) > 0 then
		currHydrofoilFore = currHydrofoilFore + hydrofoilSpeed
		for i=0,table.getn(hydrofoilForeArray) do

		I:Component_SetFloatLogic(8, hydrofoilForeArray[i], currHydrofoilFore)

		end
	end


	elseif (currPitch < (360 - vesselPitchCorrectTolerance)) and (currPitch > 225) then
	I:RequestThrustControl(11)
	BouyancyCorrect(true, I)
	vesselPitchComp = false
	if table.getn(hydrofoilAftArray) > 0 then
		currHydrofoilAft = currHydrofoilAft + hydrofoilSpeed
		for i=0,table.getn(hydrofoilAftArray) do

		I:Component_SetFloatLogic(8, hydrofoilAftArray[i], currHydrofoilAft)

		end
	end

	if table.getn(hydrofoilForeArray) > 0 then
		currHydrofoilFore = currHydrofoilFore - hydrofoilSpeed
		for i=0,table.getn(hydrofoilForeArray) do

		I:Component_SetFloatLogic(8, hydrofoilForeArray[i], currHydrofoilFore)

		end
	end

	else
	I:Component_SetFloatLogicAll(8,0)
	end

end

function RollCorrect(I)

	currRoll = I:GetConstructRoll()

	hydrofoilLeftArray = {}
	hydrofoilRightArray = {}
	numHydrofoils = I:Component_GetCount(8)
	hydrofoilSpeed = (vesselHydrofoilPitchSpeed / 40)

	j, k = 0, 0
	for i=0,numHydrofoils do
	currHydrofoilPosition = I:Component_GetBlockInfo(8,i).LocalPositionRelativeToCom

	if currHydrofoilPosition.x < 0 then
	hydrofoilLeftArray[j] = i
	j = j + 1
	end

	if currHydrofoilPosition.x > 0 then
	hydrofoilRightArray[k] = i
	k = k + 1
	end

	end
	currHydrofoilLeft = 0
	currHydrofoilRight = 0
	if table.getn(hydrofoilLeftArray) > 0 then currHydrofoilLeft = I:Component_GetFloatLogic(8, hydrofoilLeftArray[0]) end
	if table.getn(hydrofoilRightArray) > 0 then currHydrofoilRight = I:Component_GetFloatLogic(8, hydrofoilRightArray[0]) end
	if (currRoll > vesselRollCorrectTolerance) and (currRoll <= 180) then
	I:RequestThrustControl(6)

	if table.getn(hydrofoilLeftArray) > 0 then
		currHydrofoilLeft = currHydrofoilLeft + hydrofoilSpeed
		for i=0,table.getn(hydrofoilLeftArray) do

		I:Component_SetFloatLogic(8, hydrofoilLeftArray[i], currHydrofoilLeft)

		end
	end

	if table.getn(hydrofoilRightArray) > 0 then
		currHydrofoilRight = currHydrofoilRight - hydrofoilSpeed
		for i=0,table.getn(hydrofoilRightArray) do

		I:Component_SetFloatLogic(8, hydrofoilRightArray[i], currHydrofoilRight)

		end
	end


	elseif (currRoll < (360 - vesselRollCorrectTolerance)) and (currRoll > 180) then
	I:RequestThrustControl(7)

	if table.getn(hydrofoilLeftArray) > 0 then
		currHydrofoilLeft = currHydrofoilLeft - hydrofoilSpeed
		for i=0,table.getn(hydrofoilLeftArray) do

		I:Component_SetFloatLogic(8, hydrofoilLeftArray[i], currHydrofoilLeft)

		end


	end

	if table.getn(hydrofoilRightArray) > 0 then
		currHydrofoilRight = currHydrofoilRight + hydrofoilSpeed
		for i=0,table.getn(hydrofoilRightArray) do

		I:Component_SetFloatLogic(8, hydrofoilRightArray[i], currHydrofoilRight)

		end
	end

	else

	I:Component_SetFloatLogicAll(8,0)

	end

	end

	function BouyancyCorrect(pitchComp, I)

	currVertVelo = I:GetVelocityVector().y
	currBouyFactor = I:Component_GetFloatLogic(2,0)

	if (currVertVelo < -0.1) and pitchComp then

		currBouyFactor = currBouyFactor + 0.005

	elseif (currVertVelo > 0.1) and pitchComp then

		currBouyFactor = currBouyFactor - 0.005

	elseif (currVertVelo < -0.1) and not pitchComp then

		currBouyFactor = currBouyFactor - 0.005

	elseif (currVertVelo > 0.1) and not pitchComp then

		currBouyFactor = currBouyFactor + 0.005

	end

	I:Component_SetFloatLogicAll(2,currBouyFactor)

end


function Update(I)
	vesselPitchComp = true
	PitchCorrect(I)
	RollCorrect(I)
	if vesselPitchComp == true then BouyancyCorrect(true, I) end

end

I:BindUpdateFunction(MyUpdateFunction)