Cost function 2016 a2

    
    private static final int ALGO_COST_TIMER_TRANS0 = 70;
    private static final int ALGO_COST_TIMER_TRANS1 = 70;
    private static final int ALGO_COST_TIMER_STATE = 300;
    private static final int ALGO_COST_ANNOUCETIME = ALGO_COST_TIMER_TRANS0+ALGO_COST_TIMER_TRANS1;
    private int algoCost_timer = 0;
    private int algoCost_cycleSection = 0;
    public void algoCost(boolean useAutos, boolean usePacketLoss){
	updateLaneTime();
	
	// Timer
	if(algoCost_timer > 0){
	    algoCost_timer--; 
	    return;
	}
	
	// What part of the cycle are we in?
	if( algoCost_cycleSection == 0){
	    algoCost_cycleSection++;
	    algoCost_timer = ALGO_COST_TIMER_TRANS0;
	    // Get our next state
	    nextSystemState = algoCostGetNextSystemState(useAutos,usePacketLoss);
	    // Transition part one
	    setTransitionState0(nextSystemState);
	    // Annouce this to the autonomous cars
	    if(useAutos)
		announceState(nextSystemState,ALGO_COST_ANNOUCETIME,usePacketLoss);
	}
	else if( algoCost_cycleSection == 1){
	    algoCost_cycleSection++;
	    algoCost_timer = ALGO_COST_TIMER_TRANS1;
	    // Transition part two
	    setTransitionState1(nextSystemState);
	}
	else{
	    algoCost_cycleSection = 0;
	    algoCost_timer = ALGO_COST_TIMER_STATE;
	    // Set state
	    setSystemState(nextSystemState);
	}
    }
    
    private int algoCostGetNextSystemState(boolean useAutos, boolean usePacketLoss){
	// For all lanes, calculate cost
	calcLaneCosts(usePacketLoss);
	// For all system states, calculate cost reduction
	int arr[]; float cost;
	for(int i = 0; i < systemStateList.size(); i++){
	    cost = 0;
	    arr = systemStateList.get(i);
	    // Get cost
	    if( useAutos )
		cost += getSystemStateLanesCost(arr);
	    cost += getSystemStateTimeCost(arr);
	    // Place cost in list
	    systemStateCostList.set(i, cost);
	}
	// Get highest cost reduction state
	int highestIndex = 0;
	float highestCost = systemStateCostList.get(0), compcost;
	for(int i = 1; i < systemStateCostList.size(); i++){
	    compcost = systemStateCostList.get(i);
	    if( compcost > highestCost ){
		highestCost = compcost;
		highestIndex = i;
	    }
	}
	// Return this state
	return highestIndex;
    }
    
    private void announceState(int state, int time, boolean usePacketLoss){
	int lights[] = systemStateList.get(state);
	// Tell each auto this
	Car car;
	boolean correctLight;
	for(int i = 0; i < main.cars.size();i++){
	    // Vars
	    car = main.cars.get(i);
	    // is it a auto?
	    if(!car.isAuto)
		continue;
	    // Check if it is indeed nearing a relevant light
	    correctLight = false;
	    for(int j = 0; j < lights.length; j++){
		if( car.light == lights[j] ){
		    correctLight = true;
		    break;
		}
	    }
	    if(!correctLight)
		continue;
	    // Did we lose the packet?
	    if(usePacketLoss && isPacketLost())
		continue;
	    // Lets tell it what to do
	    car.tellNextGreenWave(time);
	}
    }