package dp.subcomponents;

component Drivercontroller {
    ports
        in Q^{4} anglesIn,
	    in Affordance affordanceIn,
	    in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
	    out Q(0:1)^{3} commandsOut;

	implementation Math {
        Q roadWidth = 8;

	    // Acceleration
	    Q slowDown = 100;

	    if (affordanceIn.distMM < 15)
            Q vMax = 20;
            Q vC = 2.772;
            Q vD = 0.693;
            slowDown = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD));

            if (slowDown < 0)
                slowDown = 0;
            end
        end

        Q centerLine;
        Q coeSteer;
        Q preML;
        Q preMR;
        if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
            coeSteer = 1.5;
            centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
            preML = affordanceIn.toMarkingML;
            preMR = affordanceIn.toMarkingMR;
            if (affordanceIn.toMarkingM < 1)
                  coeSteer = 0.4;
            end
        else
            if (-preML > preMR)
               centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
            else
               centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
            end
            coeSteer = 0.3;
        end

        // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
	    commandsOut(2) = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;

	    Q desiredSpeed = 20;
	    if (desiredSpeed > slowDown)
            desiredSpeed = slowDown;
	    end

        // acceleration and brake
        if (desiredSpeed >= speedIn)
            Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
            if (accelCmd > 1)
                accelCmd = 1;
            else
                commandsOut(1) = accelCmd;
            end
            commandsOut(3) = 0;
        else
            Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
            if (brakeCmd > 1)
                commandsOut(3) = 1;
            else
                commandsOut(3) = brakeCmd;
            end

            commandsOut(1) = 0;
        end
    }
}