USARBot.UnderwaterRobot

class UnderwaterRobot extends NauticVehicle config(USARBot);

// Structure to hold both the part index and the part's force
struct PartData
{
    var int PartNumber;
    var float Force;
};

// Configuration variable to be set in the defaultproperties section of the vehicle's class
var float waterDensity;

// Programming variables (these variables are transparent to the users)
var float currentVel, totalVel, currentTime, oldTime;
var bool atSurface, initialized;
var array<PartData> Propellers, Rudders, SternPlanes;

function ProcessCarInput()
{
    // Programming variables
    local int i;
    local float maxSpeed, maxAngle, ControllerSpeed, ControllerRudder, ControllerSternPlane, InputSpeed, InputRudder, InputSternPlane;
    local bool isCommandNormalized;
    
    Super.ProcessCarInput();

    // Initializes various variables and fills up arrays. Note: this section is only executed once.
    if(!initialized)
    {
        // Here, we get the propeller(s), rudder(s), and stern plane(s) into separate dynamic arrays
        for(i=0; i<JointParts.length; i++)
        {
            // Get propeller(s)
            if(InStr(Caps(string(JointParts[i].PartName)), "PROPELLER") != -1)
            {
                Propellers.Insert(Propellers.length, 1);        // Make space in the dynamic array to add a propeller
                Propellers[Propellers.length-1].PartNumber = i; // Store the index number of the part in the dynamic array
            }

            // Get rudder(s)
            if(InStr(Caps(string(JointParts[i].PartName)), "RUDDER") != -1)
            {
                Rudders.Insert(Rudders.length, 1);        // Make space in the dynamic array to add a rudder
                Rudders[Rudders.length-1].PartNumber = i; // Store the index number of the part in the dynamic array
            }

            // Get stern plane(s)
            if(InStr(Caps(string(JointParts[i].PartName)), "STERNPLANE") != -1)
            {
                SternPlanes.Insert(SternPlanes.length, 1);        // Make space in the dynamic array to add a stern plane
                SternPlanes[SternPlanes.length-1].PartNumber = i; // Store the index number of the part in the dynamic array
            }
        }

        // Initialize the controller's properties
        USARRemoteBot(Controller).Normalized = false;
        USARRemoteBot(Controller).Propeller = 0.0;
        USARRemoteBot(Controller).Rudder = 0.0;
        USARRemoteBot(Controller).SternPLane = 0.0;
        initialized = true;
        
        // Section used for debugging purposes to see if the correct joints have been saved in the dynamic arrays
        if(bDebug)
        {
            for(i=0; i<Propellers.Length; i++) Log("Propeller #" $ i+1 $ ": " $ Propellers[i].PartNumber);
            for(i=0; i<Rudders.Length; i++) Log("Rudder #" $ i+1 $ ": " $ Rudders[i].PartNumber);
            for(i=0; i<SternPlanes.Length; i++) Log("Stern Plane #" $ i+1 $ ": " $ SternPlanes[i].PartNumber);
        }
    }

    currentTime = Level.TimeSeconds;

    // If a DRIVE command was issued
    if(USARRemoteBot(Controller).bNewThrottle)
    {
        // If the underwater robot is underwater (not at the surface)
        if(!atSurface)
        {
            isCommandNormalized = USARRemoteBot(Controller).Normalized;  // Get the normalized bool from the controller
            ControllerSpeed = USARRemoteBot(Controller).Propeller;       // Get the propeller's spin speed value from the controller
            ControllerRudder = USARRemoteBot(Controller).Rudder;         // Get the rudder angle from the controller
            ControllerSternPlane = USARRemoteBot(Controller).SternPlane; // Get the stern plane angle from the controller

            // Here, we deal with the propeller(s)
            for(i=0; i<Propellers.Length; i++)
            {
                maxSpeed = Propeller(Parts[Propellers[i].PartNumber]).maxSpinSpeed; // Get the maximum spin speed for this propeller

                // If a normalized drive command was received (e.g. the propeller spin speed is between -100 and 100)
                if (isCommandNormalized)
                {
                    if(ControllerSpeed < -100)     InputSpeed = -maxSpeed;                         // If the normalized value is less than -100, we use the negative of the propeller's maximum spin speed
                else if(ControllerSpeed > 100) InputSpeed = maxSpeed;                          // If the normalized value is more than 100, we use the propeller's maximum spin speed
                else                           InputSpeed = (ControllerSpeed/100) * maxSpeed;  // If the normalized value is between -100 and 100, we use a percentage of the maximum spin speed
                }
                // If a non-normalized drive command was received (e.g. the propeller spin speed is an absolute value)
                else
                {
                    if(ControllerSpeed < -maxSpeed)     InputSpeed = -maxSpeed;       // If the absolute value is less than -(maximum speed), we use the negative of the propeller's maximum spin speed
                else if(ControllerSpeed > maxSpeed) InputSpeed = maxSpeed;        // If the absolute value is more than the maximum speed, we use the propeller's maximum spin speed
                else                                InputSpeed = ControllerSpeed; // Otherwise, we use the controller's value for the propeller's spin speed
                }

                // Physically spin the propeller (spin the part)
                setSpinSpeed(Propellers[i].PartNumber, Converter.SpinSpeedToUU(InputSpeed));

                // Find the velocity generated by this propeller
                currentVel = (InputSpeed/6.283185307179586476925286766559) * (Propeller(Parts[Propellers[i].PartNumber]).Pitch);

                // Add the velocity generated by this propeller to the velocity generated by the other propellers
                if(i==0) totalVel = currentVel;
                else     totalVel += currentVel;

                // Find the force generated by this propeller
                Propellers[i].Force = getPropellerForce();
            }

            // Here, we deal with the rudder(s)
            for(i=0; i<Rudders.Length; i++)
            {
                maxAngle = ControlSurface(Parts[Rudders[i].PartNumber]).maxAngle; // Get the maximum rudder angle for this rudder

                // If a normalized drive command was received (e.g. the rudder angle is between -100 and 100)
                if (isCommandNormalized)
                {
                    if(ControllerRudder < -100)     InputRudder = -maxAngle;                         // If the normalized value is less than -100, we use the negative of the maximum rudder angle
                else if(ControllerRudder > 100) InputRudder = maxAngle;                          // If the normalized value is more than 100, we use the maximum rudder angle
                else                            InputRudder = (ControllerRudder/100) * maxAngle; // If the normalized value is between -100 and 100, we use a percentage of the maximum rudder angle
                }
                // If a non-normalized drive command was received (e.g. the rudder angle is an absolute value)
                else
                {
                    if(ControllerRudder < -maxAngle)     InputRudder = -maxAngle;        // If the absolute value is less than -(maximum angle), we use the negative of the rudder's maximum angle
                else if(ControllerRudder > maxAngle) InputRudder = maxAngle;         // If the absolute value is more than the maximum angle, we use the rudder's maximum angle
                else                                 InputRudder = ControllerRudder; // Otherwise, we use the controller's value for the rudder angle
                }

                // Physically rotate the rudder (rotate the part)
                setAngle(Rudders[i].PartNumber, Converter.AngleToUU(InputRudder));
                
                // Find the side force generated by the rudder
                Rudders[i].Force = getSideForce(ControlSurface(Parts[Rudders[i].PartNumber]).Area, InputRudder, InputSpeed);
            }

            // Here, we deal with the stern plane(s)
            for(i=0; i<SternPlanes.Length; i++)
            {
                maxAngle = ControlSurface(Parts[SternPlanes[i].PartNumber]).maxAngle; // Get the maximum stern plane angle for this stern plane

                // If a normalized drive command was received (e.g. the stern plane angle is between -100 and 100)
                if(isCommandNormalized)
                {
                    if(ControllerSternPlane < -100)     InputSternPlane = -maxAngle;                             // If the normalized value is less than -100, we use the negative of the maximum stern plane angle
                else if(ControllerSternPlane > 100) InputSternPlane = maxAngle;                              // If the normalized value is more than 100, we use the maximum stern plane angle
                else                                InputSternPlane = (ControllerSternPlane/100) * maxAngle; // If the normalized value is between -100 and 100, we use a percentage of the maximum stern plane angle
                }
                // If a non-normalized drive command was received (e.g. the stern plane angle is an absolute value)
                else
                {
                    if(ControllerSternPlane < -maxAngle)     InputSternPlane = -maxAngle;            // If the absolute value is less than -(maximum angle), we use the negative of the stern plane's maximum angle
                else if(ControllerSternPlane > maxAngle) InputSternPlane = maxAngle;             // If the absolute value is more than the maximum angle, we use the stern plane's maximum angle
                else                                     InputSternPlane = ControllerSternPlane; // Otherwise, we use the controller's value for the stern plane angle
                }

            // Physically rotate the stern plane (rotate the part)
            setAngle(SternPlanes[i].PartNumber, Converter.AngleToUU(InputSternPlane));

            // Find the side force generated by the stern plane
                SternPlanes[i].Force = getSideForce(ControlSurface(Parts[SternPlanes[i].PartNumber]).Area, InputSternPlane, InputSpeed);
            }
        }

        oldTime = currentTime;

        KWake(); // Make sure that the Karma Engine is "awake"
    }
}


//*********************************************************************************************************************
// getPropellerForce Function
// --------------------------
//     Function that calculates and returns the force generated by the underwater robot's propeller.
//*********************************************************************************************************************
function float getPropellerForce()
{
    return (1.1 * currentVel * ChassisMass) / (currentTime - oldTime);
}


//*********************************************************************************************************************
// getSideForce Function
// --------------------------
//     Function that calculates and returns the side force generated by the underwater robot's control surfaces.
//   The control surfaces can either be rudders or stern planes. The equation used is the drag equation.
//*********************************************************************************************************************
function float getSideForce(float SurfaceArea, float Angle, float Speed)
{
    local float Force;

    // Calculate the side force generated by the part
    if(Sign(Angle) == Sign(Speed))
    {
        Force = 0.5 * waterDensity * Square(totalVel) * getDragConstant(Angle) * SurfaceArea;
    }
    else
    {
        Force = -0.5 * waterDensity * Square(totalVel) * getDragConstant(Angle) * SurfaceArea;
    }

    return Force;
}


//*********************************************************************************************************************
// Sign Function
// ------------------------
//     Helper function (for the getSideForce function) that returns the sign of the number passed as a parameter.
//     If the variable passed as a parameter is 0, the function returns 0.
//     If the variable passed as a parameter is positive, the function returns 1.
//     If the variable passed as a parameter is negative, the function returns -1.
//*********************************************************************************************************************
function int Sign(float theValue)
{
    if(theValue == 0) return 0;
    return (theValue / Abs(theValue));
}


//*********************************************************************************************************************
// getDragConstant Function
// ------------------------
//     Helper function (for the getSideForce function) that calculates and returns the drag coefficient given
//   the angle of a part (e.g given the angle of a rudder or of a stern plane).
//*********************************************************************************************************************
function float getDragConstant(float angle)
{
    local float X_2,X_3;

    angle = Converter.AngleToDeg(Abs(angle));

    X_2 = angle * angle;
    X_3 = X_2 * angle;

    return ((0.00003 * X_3) - (0.0002 * X_2) + (0.0028 * angle));
}


//*********************************************************************************************************************
// KApplyForce Event
// -----------------
//     Event called at the same frequency of the Tick function, as long as the karma engine is "awake".
//     The event calculates the appropriate forces and torques, based on the underwater robot's rotation, which
//   are then automatically applied by the Karma Engine.
//*********************************************************************************************************************
event KApplyForce(out vector Force, out vector Torque)
{
    local int i;
    local float PropellerForce, RudderTorque, SternPlaneTorque;

    // Calculate the total force generated by all the propellers
    PropellerForce = 0;
    for(i=0; i<Propellers.Length; i++)
    {
        PropellerForce += Propellers[i].Force;
    }

    // Calculate the total torque generated by all the rudders (Torque = Force * Length_from_center_of_mass)
    RudderTorque = 0;
    for(i=0; i<Rudders.Length; i++)
    {
        RudderTorque += (Rudders[i].Force * Converter.LengthFromUU(Sqrt(Square(Location.X - Parts[Rudders[i].PartNumber].Location.X) +
                                                                        Square(Location.Y - Parts[Rudders[i].PartNumber].Location.Y) +
                                                                        Square(Location.Z - Parts[Rudders[i].PartNumber].Location.Z))));
    }

    // Calculate the total torque generated by all the stern planes (Torque = Force * Length_from_center_of_mass)
    SternPlaneTorque = 0;
    for(i=0; i<SternPlanes.Length; i++)
    {
        SternPlaneTorque += (SternPlanes[i].Force * Converter.LengthFromUU(Sqrt(Square(Location.X - Parts[SternPlanes[i].PartNumber].Location.X) +
                                                                                Square(Location.Y - Parts[SternPlanes[i].PartNumber].Location.Y) +
                                                                                Square(Location.Z - Parts[SternPlanes[i].PartNumber].Location.Z))));
    }

    // Set the Force and Torque to be applied by the Karma Engine, based on the vehicle's rotation
    Force = (PropellerForce * vect(1,0,0)) >> Rotation;
    Torque = ((Vect(0,1,0) * SternPlaneTorque) + (Vect(0,0,1) * RudderTorque)) >> Rotation;
}


//*********************************************************************************************************************
// PhysicsVolumeChange Event
// -------------------------
//     Event called whenever the submarine enters a new physics volume.
//     The event sets the atSurface boolean variable if the underwater robot is not in a water volume.
//*********************************************************************************************************************
simulated event PhysicsVolumeChange( PhysicsVolume NewVolume )
{
    Super.PhysicsVolumeChange(NewVolume);

    atSurface = !NewVolume.bWaterVolume;
}


//*********************************************************************************************************************
// DEFAULT PROPERTIES
// DO NOT change these properties since they are used to initialize programming variables
//*********************************************************************************************************************
defaultproperties
{
    atSurface = false
    initialized = false
}