package dp.subcomponents;

component Drivercontroller {
    ports
	    in Q(0:1)^{1,1,13} affordanceIn,
	    in Q speedIn,
	    out Q(0:1)^{3,1,1} commandsOut;

	implementation Math {
	    Q Constant = 1;
	    Q angle = affordanceIn(0,0,0);
	    Q roadWidth = affordanceIn(0,0,1) + affordanceIn(0,0,4);
	    Q laneWidth = affordanceIn(0,0,2) + affordanceIn(0,0,3);
	    Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));

	    // Steering
	    commandsOut(2,1,1) = 0;//Constant * (angle-distToLaneCenter/roadWidth);  // steering

	    // Acceleration
	    Q vMax = 25;
	    Q vC = 1;
	    Q vD = 0;
	    Q desiredSpeed = 15;//vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));

	    Q accelCmd = 0.1 * (desiredSpeed - speedIn + 1);
	    if accelCmd > 1
            accelCmd = 1.0;
        end

        Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
        if brakeCmd > 1
            brakeCmd = 1;
        end

        // acceleration and brake
	    if speedIn < desiredSpeed
	        commandsOut(1,1,1) = accelCmd;
	        commandsOut(3,1,1) = 0;
	    else
            commandsOut(1,1,1) = 0;
            commandsOut(3,1,1) = brakeCmd;
        end
    }
}