FtD Submarine Control

TerrainAdjustmentTime = 10;
CruiseDepth = 150;
MaxDepth = 400;
Clearance = 30;

function Update(I)
    I:ClearLogs();
    DepthCorrect(I);
    PitchCorrect(I);
end

function TerrainPrediction(I)
    -- Set return value to lowest possible altitude
    local ReturnValue = 500;
    if I:GetForwardsVelocityMagnitude() > 0 then
        for zIndex = I:GetConstructMinDimensions().z, I:GetConstructMaxDimensions().z + (I:GetForwardsVelocityMagnitude()*TerrainAdjustmentTime), 1 do
            -- Use highest terrain from front of ship to speed*TerrainAdjustmentTime distance ahead
            if ReturnValue > -I:GetTerrainAltitudeForLocalPosition(0,I:GetConstructPosition().y,zIndex) then
                ReturnValue = -I:GetTerrainAltitudeForLocalPosition(0,I:GetConstructPosition().y,zIndex);
            end
        end
    else
        for zIndex = I:GetConstructMaxDimensions().z, I:GetConstructMinDimensions().z - (I:GetForwardsVelocityMagnitude()*TerrainAdjustmentTime), -1 do
            -- Use highest terrain from rear of ship to speed*TerrainAdjustmentTime distance behind
            if ReturnValue > -I:GetTerrainAltitudeForLocalPosition(0,I:GetConstructPosition().y,zIndex) then
                ReturnValue = -I:GetTerrainAltitudeForLocalPosition(0,I:GetConstructPosition().y,zIndex);
            end
        end
    end
    return ReturnValue;
end

function DepthCorrect(I)
    local maxDepth = TerrainPrediction(I);
    I:Log('Terrain depth at prediction point: '.. math.floor(maxDepth));
    I:LogToHud('Terrain depth at prediction point: '.. math.floor(maxDepth));
    local CClearance = 500;
    if CruiseDepth + I:GetConstructPosition().y > maxDepth + I:GetConstructPosition().y then
        CClearance = maxDepth + I:GetConstructPosition().y;
    else
        CClearance = CruiseDepth + I:GetConstructPosition().y;
        Clearance = 0;
    end

    if CClearance < Clearance then
        I:RequestComplexControllerStimulus(1);
        I:Log('Clearance too small! '.. math.floor(CClearance) ..'<'..Clearance ..' --> Engaging thrusters!');
        I:LogToHud('Clearance too small! Engaging thrusters!');
    end

    if CClearance > Clearance then
        I:RequestComplexControllerStimulus(2);
        I:Log('Clearance OK! Distance to terrain: '.. math.floor(CClearance)..'>'..Clearance);
        I:LogToHud('Clearance OK! Descending!');
    else
        I:Log('Desired depth reached! Depth ' .. math.floor(-I:GetConstructPosition().y));
        I:LogToHud('Desired depth reached!');
    end
end

function PitchCorrect(I)
    pitch = I:GetConstructPitch();
    if pitch > 180 then
        pitch = pitch - 360;
    end
    --I:Log(pitch);

    if pitch > 1 then
        I:RequestComplexControllerStimulus(11);
    end

    if pitch < -1 then
        I:RequestComplexControllerStimulus(12);
    end
end