LMRS.java

package org.opentrafficsim.road.gtu.lane.tactical.lmrs;

import java.util.ArrayList;

import nl.tudelft.simulation.language.d3.DirectedPoint;

import org.djunits.unit.AccelerationUnit;
import org.djunits.unit.SpeedUnit;
import org.djunits.value.vdouble.scalar.Acceleration;
import org.djunits.value.vdouble.scalar.Speed;
import org.djunits.value.vdouble.scalar.Time.Abs;
import org.opentrafficsim.core.gtu.GTU;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.drivercharacteristics.ParameterException;
import org.opentrafficsim.core.gtu.drivercharacteristics.ParameterTypeDouble;
import org.opentrafficsim.core.gtu.drivercharacteristics.ParameterTypes;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.tactical.AbstractLaneBasedTacticalPlanner;
import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
import org.opentrafficsim.road.gtu.lane.tactical.following.HeadwayGTU;

/**
 * Implementation of the LMRS (Lane change Model with Relaxation and Synchronization).
 * See Schakel, W.J., Knoop, V.L., and Van Arem, B. (2012), 
 * <a href="http://victorknoop.eu/research/papers/TRB2012_LMRS_reviewed.pdf">LMRS: Integrated Lane Change Model with 
 * Relaxation and Synchronization</a>, Transportation Research Records: Journal of the Transportation Research Board, 
 * No. 2316, pp. 47-57. Note in the official versions of TRB and TRR some errors appeared due to the typesetting of the 
 * papers (not in the preprint provided here). A list of errata for the official versions is found 
 * <a href="http://victorknoop.eu/research/papers/Erratum_LMRS.pdf">here</a>.
 * @author Wouter Schakel
 */
public class LMRS extends AbstractLaneBasedTacticalPlanner {

	private static final long serialVersionUID = 20160803L;
	
	/** Free lane change desire threshold. */
	public static final ParameterTypeDouble DFREE = new ParameterTypeDouble("dFree", 
			"Free lane change desire threshold.", 0.365) {
		public void check(double value) throws ParameterException {
			ParameterException.failIf(value<=0, "Parameter of type dFree may not have a negative or zero value.");
			ParameterException.failIf(value>1, "Parameter of type dFree may not have a value greater than 1.");
		}		
	};
	
	/** Synchronized lane change desire threshold. */
	public static final ParameterTypeDouble DSYNC = new ParameterTypeDouble("dSync", 
			"Synchronized lane change desire threshold.", 0.577) {
		public void check(double value) throws ParameterException {
			ParameterException.failIf(value<=0, "Parameter of type dSync may not have a negative or zero value.");
			ParameterException.failIf(value>1, "Parameter of type dSync may not have a value greater than 1.");
		}		
	};
	
	/** Cooperative lane change desire threshold. */
	public static final ParameterTypeDouble DCOOP = new ParameterTypeDouble("dCoop", 
			"Cooperative lane change desire threshold.", 0.788) {
		public void check(double value) throws ParameterException {
			ParameterException.failIf(value<=0, "Parameter of type dCoop may not have a negative or zero value.");
			ParameterException.failIf(value>1, "Parameter of type dCoop may not have a value greater than 1.");
		}		
	};
	
	/** Minimum acceleration for current plan. */
	protected Acceleration minimumAcceleration;
	
	/** List of mandatory lane change incentives. */
	protected ArrayList<MandatoryIncentive> mandatoryIncentives;
	
	/** List of voluntary lane change incentives. */
	protected ArrayList<VoluntaryIncentive> voluntaryIncentives;
	
	/**
	 * Adds a mandatory incentive.
	 * @param incentive Incentive to add.
	 */
	public void addMandatoryIncentive(MandatoryIncentive incentive) {
		this.mandatoryIncentives.add(incentive);
	}
	
	/**
	 * Adds a voluntary incentive.
	 * @param incentive Incentive to add.
	 */
	public void addVoluntaryIncentive(VoluntaryIncentive incentive) {
		this.voluntaryIncentives.add(incentive);
	}
	
	/**
	 * Sets the default lane change incentives. These are the mandatory route incentive, and the voluntary speed and 
	 * keep incentives. Any existing incentives are removed.
	 */
	public void setDefaultIncentives() {
		this.mandatoryIncentives.clear();
		this.voluntaryIncentives.clear();
		this.mandatoryIncentives.add(new IncentiveRoute());
		this.voluntaryIncentives.add(new IncentiveSpeedWithCourtesy());
		this.voluntaryIncentives.add(new IncentiveKeep());
	}
	
	/**
	 * Disables lane changes by clearing all incentives and setting a dummy incentive as mandatory incentive.
	 */
	public void disableLaneChanges() {
		this.mandatoryIncentives.clear();
		this.voluntaryIncentives.clear();
		this.mandatoryIncentives.add(new IncentiveDummy());
	}
	
	@Override
	public OperationalPlan generateOperationalPlan(GTU gtu, Abs startTime, DirectedPoint locationAtStartTime) 
			throws OperationalPlanException, GTUException, NetworkException {
		
		// TODO: Remove this when other todo's are done, it is used as a placeholder where some acceleration needs to be
		// determined.
		Acceleration dummy = new Acceleration(0, AccelerationUnit.SI);
		
		// Check existence of mandatory incentive 
		if (mandatoryIncentives.isEmpty()) {
			throw new OperationalPlanException("At the least the LMRS requires 1 mandatory lane change incentive.");
		}

		// Obtain objects to get info
		LaneBasedGTU gtuLane = (LaneBasedGTU) gtu;
		LanePerception perception = (LanePerception) gtu.getPerception();
		CarFollowingModel dfm = (CarFollowingModel) gtuLane.getStrategicalPlanner().getDrivingCharacteristics().getGTUFollowingModel();
				
		
		// TODO: Throw ParameterException
		Acceleration b;
		double dFree;
		double dSync;
		double dCoop;
		try {
			b = gtuLane.getBehavioralCharacteristics().getAccelerationParameter(ParameterTypes.B);
			dFree = gtuLane.getBehavioralCharacteristics().getParameter(DFREE);
			dSync = gtuLane.getBehavioralCharacteristics().getParameter(DSYNC);
			dCoop = gtuLane.getBehavioralCharacteristics().getParameter(DCOOP);
		} catch (ParameterException pe) {
			throw new RuntimeException(pe);
		}
		
		// Reset stuff that is used in creating a plan
		minimumAcceleration = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
		
		// Determine tactical plan
		if (Math.random()>.5) { // TODO: if changing lane (rand to disable annoying warnings)
			
			/*
			 * During a lane change, both leaders are followed.
			 */
			lowerAcceleration(dummy);
			lowerAcceleration(dummy);
			
			/*
    		 * Operational plan.
    		 */
			// TODO: Build the operational plan using minimum acceleration
			//LaneOperationalPlanBuilder
			return null;
			
		}
		
		/*
		 * Relaxation
		 */
		// TODO: relaxation
			
		/*
		 * Determine desire
		 * Mandatory is deduced as the maximum of a set of mandatory incentives, while voluntary desires are added.
		 * Depending on the level of mandatory lane change desire, voluntary desire may be included partially. If 
		 * both are positive or negative, voluntary desire is fully included. Otherwise, voluntary desire is less
		 * considered within the range dSync < |mandatory| < dCoop. The absolute value is used as large negative
		 * mandatory desire may also dominate voluntary desire.
		 */
		// Mandatory desire
		double dLeftMandatory = Double.NEGATIVE_INFINITY;
		double dRightMandatory = Double.NEGATIVE_INFINITY;
		for (MandatoryIncentive incentive : this.mandatoryIncentives) {
			Desire d = incentive.determineDesire(gtuLane, perception);
			dLeftMandatory = d.getLeft()>dLeftMandatory ? d.getLeft() : dLeftMandatory;
			dRightMandatory = d.getRight()>dRightMandatory ? d.getRight() : dRightMandatory;
		}
		Desire mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
		// Voluntary desire
		double dLeftVoluntary = 0;
		double dRightVoluntary = 0;
		for (VoluntaryIncentive incentive : this.voluntaryIncentives) {
			Desire d = incentive.determineDesire(gtuLane, perception, mandatoryDesire);
			dLeftVoluntary += d.getLeft();
			dRightVoluntary += d.getRight();
		}
		// Total desire
		double thetaLeft = 0;
		if (dLeftMandatory<=dSync || dLeftMandatory*dLeftVoluntary>=0) {
			// low mandatory desire, or same sign
			thetaLeft = 1;
		} else if (dSync<dLeftMandatory && dLeftMandatory<dCoop && dLeftMandatory*dLeftVoluntary<0) {
			// linear from 1 at dSync to 0 at dCoop
			thetaLeft = (dCoop-Math.abs(dLeftMandatory)) / (dCoop-dSync);
		}
		double thetaRight = 0;
		if (dRightMandatory<=dSync || dRightMandatory*dRightVoluntary>=0) {
			// low mandatory desire, or same sign
			thetaRight = 1;
		} else if (dSync<dRightMandatory && dRightMandatory<dCoop && dRightMandatory*dRightVoluntary<0) {
			// linear from 1 at dSync to 0 at dCoop
			thetaRight = (dCoop-Math.abs(dRightMandatory)) / (dCoop-dSync);
		}
		Desire totalDesire = new Desire(dLeftMandatory + thetaLeft*dLeftVoluntary, 
				dRightMandatory + thetaRight*dRightVoluntary);
		
		
		/*
		 * Gap acceptance
		 * The adjacent gap is accepted if acceleration is safe for the potential follower and for this driver.
		 */
		// TODO: get neighboring vehicles, use their (perceived) car-following model with altered headway
		Acceleration aFollow = dummy;
		Acceleration aSelf = dummy;
		boolean leftAllowed = true; // TODO: get from perception / infrastructure, with relaxation
		boolean acceptLeft = aSelf.getSI()>=-b.si*totalDesire.getLeft() && 
                aFollow.getSI()>=-b.si*totalDesire.getLeft() && leftAllowed;
        aFollow = dummy;
		aSelf = dummy;
		boolean rightAllowed = true; // TODO: get from perception / infrastructure, with relaxation
		boolean acceptRight = aSelf.getSI()>=-b.si*totalDesire.getRight() && 
				aFollow.getSI()>=-b.si*totalDesire.getRight() && rightAllowed;
		
        /*
         * Lane change decision
         * A lane change is initiated for the largest desire if this is above the threshold and the gap is accepted.
         * Otherwise, the indicator may be switched on.
         */
		// TODO: switch on indicators (and off)?
        boolean changeLeft = false;
        boolean changeRight = false;
        if (totalDesire.leftIsLargerOrEqual() && totalDesire.getLeft()>=dFree && acceptLeft) {
        	// change left
        	changeLeft = true;
        } else if (!totalDesire.leftIsLargerOrEqual() && totalDesire.getRight()>=dFree && acceptRight) {
        	// change right
        	changeRight = true;
        } else if (totalDesire.leftIsLargerOrEqual() && totalDesire.getLeft()>=dCoop) {
        	// switch on left indicator
        	
        } else if (!totalDesire.leftIsLargerOrEqual() && totalDesire.getRight()>=dCoop) {
        	// switch on right indicator
        	
        }
        
        /*
         * Acceleration
         * Acceleration is determined by the leader, and possibly adjacent vehicles for synchronization and 
         * cooperation.
         */
        // follow leader
        lowerAcceleration(dummy);
        // synchronize
        // TODO: get neighboring vehicles, use car-following model with altered headway
        if (totalDesire.leftIsLargerOrEqual() && totalDesire.getLeft()>=dSync) {
        	// sync left
        	lowerAcceleration(safe(dummy, b));
        } else if (!totalDesire.leftIsLargerOrEqual() && totalDesire.getRight()>=dSync) {
        	// sync right
        	lowerAcceleration(safe(dummy, b));
        }
        // cooperate
        // TODO: get neighboring vehicles, their indicators, use car-following model with altered headway
        if (Math.random()>0) {
        	// cooperate left
        	lowerAcceleration(safe(dummy, b));
        }
        if (Math.random()>0) {
        	// cooperate right
        	lowerAcceleration(safe(dummy, b));
        }
        
        /*
		 * Operational plan.
		 */
		// TODO: Build the operational plan using minimum acceleration and including a possible lane change using 
        // changeLeft/changeRight
		//LaneOperationalPlanBuilder
        return null;
		
	}
	
	protected Acceleration calculateAcceleration(LaneBasedGTU follower, HeadwayGTU leader, double d) {
		// TODO: adjust desired headway based on desire
		// set T
		Acceleration a = calculateAcceleration(follower, leader);
		// reset T
		return a;
	}
	
	protected Acceleration calculateAcceleration(LaneBasedGTU follower, HeadwayGTU leader) {
		// TODO: speed limit
		// TODO: follower != self
		Speed speedLimit = new Speed(0, SpeedUnit.SI);
		return follower.getStrategicalPlanner().getDrivingCharacteristics().getGTUFollowingModel().computeAcceleration(follower.getVelocity(), follower.getMaximumVelocity(), leader.getGtuSpeed(), leader.getDistance(), speedLimit);
	}

	/**
	 * Remembers the lowest acceleration per tactical plan.
	 * @param a Acceleration to remember if lower than any previous acceleration.
	 */
	protected void lowerAcceleration(Acceleration a) {
		if (a.getSI()<this.minimumAcceleration.si) {
			this.minimumAcceleration = a;
		}
	}
	
	/**
	 * Limits the supplied acceleration to safe values, i.e. above or equal to -b.
	 * @param a Acceleration to limit.
	 * @param b Deceleration to limit to.
	 * @return Limited acceleration.
	 */
	protected Acceleration safe(Acceleration a, Acceleration b) {
		if (a.si>=-b.si) {
			return a;
		}
		return new Acceleration(-b.si, AccelerationUnit.SI);
	}

}