USARBot.GroundVehicle

class GroundVehicle extends KRobot config(USARBot);

// Wheel Properties
var enum PowerTypes{Not_Powered, Left_Powered, Right_Powered}List1;
var enum SteerTypes{Not_Steered, Front_Steered, Rear_Steered}List2;
struct WheelData
{
    var int Number;           //Wheel Number.  Note: this number needs to match the order in which the joints were declared in USARBot.ini, starting from 0.
    var PowerTypes PowerType; //Is the wheel not powered? Is the wheel powered by the left throttle? Is the wheel powered by the right throttle?

    //Note: the next two variables are not needed for skid-steer vehicles
    var SteerTypes SteerType; //Is the wheel steered? If so, is it steering using the front steering angle or the back steering angle?
    var float MaxSteerAngle;  //Maximum steering angle for the wheel.
};
var array<WheelData> Wheels;  //Array to store the wheel data of all the wheels

function ProcessCarInput()
{
    Super.ProcessCarInput();

    // Return robot GEO information
    if(USARRemoteBot(Controller).GeoType == "Robot")
        USARRemoteBot(Controller).myConnection.SendLine(getRobotGeo());

    // Return robot CONF Information
    if(USARRemoteBot(Controller).ConfType == "Robot")
        USARRemoteBot(Controller).myConnection.SendLine(getRobotConf());
}

function timer()
{
    local string outstring, FrontSteerAngle, RearSteerAngle;
    local vector loc,vel;
    local float time;
    local int i;

    Super.timer();

    time = Level.TimeSeconds;

    // Front Steer and Rear Steer Angles are 0.00 by default
    FrontSteerAngle = "0.0000";
    RearSteerAngle = "0.0000";

    // Get the current front steer angle
    for(i=0; i<Wheels.Length; i++)
    {
       if(Wheels[i].SteerType == Front_Steered)
       {
           FrontSteerAngle = Converter.Str_AngleFromUU(getSteerAngleOfKCarWheelJoint(KCarWheelJoint(Joints[Wheels[i].Number])));
           break;
       }
    }

    // Get the current rear steer angle
    for(i=0; i<Wheels.Length; i++)
    {
       if(Wheels[i].SteerType == Rear_Steered)
       {
           RearSteerAngle = Converter.Str_AngleFromUU(getSteerAngleOfKCarWheelJoint(KCarWheelJoint(Joints[Wheels[i].Number])));
           break;
       }
    }

    // Robot State Message
    if (bGroundTruth) {
    loc = Location;
    vel = (loc-lastLocation)/(time-lastTime);

    outstring = "STA {Type GroundVehicle} {Time "$time$"}"$
                " {Location "$Converter.Str_LengthVectorFromUU(loc)$"}"$
                " {Orientation "$Converter.Str_RotatorFromUU(Rotation)$"}"$
                " {Velocity "$Converter.Str_VelocityVectorFromUU(vel)$"}"$
                " {FrontSteer "$FrontSteerAngle$"}"$
                " {RearSteer "$RearSteerAngle$"}"$
                //" {CameraFov "$Converter.Str_AngleFromDeg(CameraZoom)$"}"$
                " {LightToggle "$bHeadlightOn$"}"$
                " {LightIntensity "$HeadlightItensity$"}"$
                " {Battery "$(batteryLife-myLife)$"}"$
                " {View "$ViewNum$"}";
        lastTime = time;
        lastLocation = loc;
    } else {
        outstring = "STA {Type GroundVehicle} {Time "$time$"}"$
                " {Location 0,0,0}"$
                " {Orientation 0,0,0}"$
                " {Velocity 0,0,0}"$
                " {FrontSteer "$FrontSteerAngle$"}"$
                " {RearSteer "$RearSteerAngle$"}"$
                //" {CameraFov "$Converter.Str_AngleFromDeg(CameraZoom)$"}"$
                " {LightToggle "$bHeadlightOn$"}"$
                " {LightIntensity "$HeadlightItensity$"}"$
                " {Battery "$(batteryLife-myLife)$"}"$
                " {View "$ViewNum$"}";
    }
    USARRemoteBot(Controller).myConnection.SendLine(outstring);
}

function string getRobotGeo()
{
    local int i,j;
    local float currentMax1;
    local string tmpStr;

    // This is all the information that we need to send out
    local string geoType;
    local string geoName;
    local string geoDimensions;
    local string geoCOG;
    local string geoWheelRadius;
    local string geoWheelSeparation;
    local string geoWheelBase;

    geoType = "GroundVehicle";
    Divide(string(self.class), ".", tmpStr, geoName);
    geoDimensions = converter.FloatString(Dimensions.X) $ "," $ converter.FloatString(Dimensions.Y) $ "," $ converter.FloatString(Dimensions.Z);
    geoCOG = converter.Str_LengthFromUU(KarmaParamsRBFull(KParams).KCOMOffset.X * 50) $ "," $ converter.Str_LengthFromUU(KarmaParamsRBFull(KParams).KCOMOffset.Y * 50) $ "," $ converter.Str_LengthFromUU(KarmaParamsRBFull(KParams).KCOMOffset.Z * 50);

    geoWheelRadius = converter.FloatString(WheelRadius);

    for(i=0; i<JointParts.Length; i++)
        if(JointParts[i].JointClass==class'KCarWheelJoint')
            for(j=0; j<JointParts.Length; j++)
                if(JointParts[j].JointClass==class'KCarWheelJoint')
                    if(getRelativePosition(Parts[i].Location, Parts[j].Location, Rotation).Y > currentMax1)
                        currentMax1 = getRelativePosition(Parts[i].Location, Parts[j].Location, Rotation).Y;

    geoWheelSeparation = converter.Str_LengthFromUU(currentMax1);
    
    currentMax1 = 0;
    
    for(i=0; i<JointParts.Length; i++)
        if(JointParts[i].JointClass==class'KCarWheelJoint')
            for(j=0; j<JointParts.Length; j++)
                if(JointParts[j].JointClass==class'KCarWheelJoint')
                    if(getRelativePosition(Parts[i].Location, Parts[j].Location, Rotation).X > currentMax1)
                        currentMax1 = getRelativePosition(Parts[i].Location, Parts[j].Location, Rotation).X;

    geoWheelBase = converter.Str_LengthFromUU(currentMax1);

    return "GEO {Type " $ geoType $ "} {Name " $ geoName $ "} {Dimensions " $ geoDimensions $ "} {COG " $ geoCOG $ "} " $
                "{WheelRadius " $ geoWheelRadius $ "} " $ "{WheelSeparation " $ geoWheelSeparation $ "} {WheelBase " $ geoWheelBase $ "}";
}

function string getRobotConf()
{
    local int i;
    local float currentMax1, currentMax2;
    local string tmpStr;

    // This is all the information that we need to send out
    local string confType;
    local string confName;
    local string confMass;
    local string confMaxSpeed;
    local string confMaxTorque;
    local string confSteeringType;
    local string confMaxFrontSteer;
    local string confMaxRearSteer;

    confType = "GroundVehicle";
    Divide(string(self.class), ".", tmpStr, confName);
    confMass = converter.FloatString(Weight);
    confMaxSpeed = converter.FloatString(maxSpinSpeed);
    confMaxTorque = converter.FloatString(MaxTorque);

    if(ClassIsChildOf(self.Class,class'AckermanSteeredRobot'))  confSteeringType = "AckermanSteered";
    else if(ClassIsChildOf(self.Class,class'SkidSteeredRobot')) confSteeringType = "SkidSteered";

    currentMax1 = 1000000;
    currentMax2 = 1000000;
    for(i=0; i<Wheels.Length; i++)
    {
       if(Wheels[i].SteerType == Front_Steered)
       {
          if(Wheels[i].MaxSteerAngle < currentMax1) currentMax1 = Wheels[i].MaxSteerAngle;
       }
       else if(Wheels[i].SteerType == Rear_Steered)
       {
          if(Wheels[i].MaxSteerAngle < currentMax2) currentMax2 = Wheels[i].MaxSteerAngle;
       }
    }

    if(currentMax1 == 1000000) currentMax1 = 0;
    if(currentMax2 == 1000000) currentMax2 = 0;


    confMaxFrontSteer = converter.FloatString(currentMax1);
    confMaxRearSteer = converter.FloatString(currentMax2);


    return "CONF {Type " $ confType $ "} {Name " $ confName $ "} {SteeringType " $ confSteeringType $ "} {Mass " $ confMass $ "} {MaxSpeed " $ confMaxSpeed $ "} " $
                "{MaxTorque " $ confMaxTorque $ "} {MaxFrontSteer " $ confMaxFrontSteer $ "} {MaxRearSteer " $ confMaxRearSteer $ "}";
}

// almost same as getJointAngle() in KRobot.uc
// the difference is that this func returns a steering angle of KCarWheelJoint
// while getJointAngle() returns a spinning angle of KCarWheelJoint.
simulated function int getSteerAngleOfKCarWheelJoint(KCarWheelJoint WheelJ)
{
    local Quat curQ;
    local Vector axis11, axis12, axis21, axis22;
    local Quat relQ2;
    local Vector newAxis12;
    local float difCos2, difSign2;
    local int curAng2;

    curQ = WheelJ.KConstraintActor1.KGetRBQuaternion();
    axis11 = QuatRotateVector(curQ,WheelJ.KPriAxis1);
    axis12 = QuatRotateVector(curQ,WheelJ.KSecAxis1);

    curQ = WheelJ.KConstraintActor2.KGetRBQuaternion();
    axis21 = QuatRotateVector(curQ,WheelJ.KPriAxis2);
    axis22 = QuatRotateVector(curQ,WheelJ.KSecAxis2);

    relQ2 = QuatFindBetween(axis11,axis21);
    newAxis12 = QuatRotateVector(relQ2,axis12);

    difCos2 = newAxis12 Dot axis22;
    if (difCos2>1.0) difCos2 = 1.0;
    if (difCos2<-1.0) difCos2 = -1.0;

    difSign2 = (axis22 Cross newAxis12) Dot axis21;
    if (difSign2<0) difSign2=-1.0;
    else difSign2=1.0;

    curAng2 = difSign2 * ACos(difCos2)*32768/PI;

    return curAng2;
}