Road road = Road(SPEED_LIMIT, TRAFFIC_DENSITY, LANE_SPEEDS)
Vehicle ego = Vehicle(lane_num, s, this->lane_speeds[lane_num], 0);
Vehicle vehicle = Vehicle(l,s,lane_speed,0);

    while(it != this->vehicles.end())
    {
        int v_id = it->first;
        vector<Vehicle> preds = it->second.generate_predictions();
        predictions[v_id] = preds;
        it++;
    }
	it = this->vehicles.begin();
	while(it != this->vehicles.end())
    {
    	int v_id = it->first;
        if(v_id == ego_key)
        {   
        	vector<Vehicle> trajectory = it->second.choose_next_state(predictions);
        	it->second.realize_next_state(trajectory);
        }
        else {
            it->second.increment(1);
        }
        it++;
    }

vector<string> Vehicle::successor_states() {

	vector<string> states;
	states.push_back("KL");
	string state = this->state;
	if (state.compare("KL") == 0) {
		states.push_back("PLCL");
		states.push_back("PLCR");
	}
	else if (state.compare("PLCL") == 0) {
		if (lane != lanes_available - 1) {
			states.push_back("PLCL");
			states.push_back("LCL");
		}
	}
	else if (state.compare("PLCR") == 0) {
		if (lane != 0) {
			states.push_back("PLCR");
			states.push_back("LCR");
		}
	}
	//If state is "LCL" or "LCR", then just return "KL"
	return states;
}

vector<Vehicle> Vehicle::generate_trajectory(string state, map<int, vector<Vehicle>> predictions) {

	vector<Vehicle> trajectory;
	if (state.compare("CS") == 0) {
		trajectory = constant_speed_trajectory();
	}
	else if (state.compare("KL") == 0) {
		trajectory = keep_lane_trajectory(predictions);
	}
	else if (state.compare("LCL") == 0 || state.compare("LCR") == 0) {
		trajectory = lane_change_trajectory(state, predictions);
	}
	else if (state.compare("PLCL") == 0 || state.compare("PLCR") == 0) {
		trajectory = prep_lane_change_trajectory(state, predictions);
	}
	return trajectory;
}

vector<Vehicle> Vehicle::prep_lane_change_trajectory(string state, map<int, vector<Vehicle>> predictions) {

	float new_s;
	float new_v;
	float new_a;
	Vehicle vehicle_behind;
	int new_lane = this->lane + lane_direction[state];
	vector<Vehicle> trajectory = { Vehicle(this->lane, this->s, this->v, this->a, this->state) };
	vector<float> curr_lane_new_kinematics = get_kinematics(predictions, this->lane);

	if (get_vehicle_behind(predictions, this->lane, vehicle_behind)) {
		//Keep speed of current lane so as not to collide with car behind.
		new_s = curr_lane_new_kinematics[0];
		new_v = curr_lane_new_kinematics[1];
		new_a = curr_lane_new_kinematics[2];
	}
	else {
		vector<float> best_kinematics;
		vector<float> next_lane_new_kinematics = get_kinematics(predictions, new_lane);
		//Choose kinematics with lowest velocity.
		if (next_lane_new_kinematics[1] < curr_lane_new_kinematics[1]) {
			best_kinematics = next_lane_new_kinematics;
		}
		else {
			best_kinematics = curr_lane_new_kinematics;
		}
		new_s = best_kinematics[0];
		new_v = best_kinematics[1];
		new_a = best_kinematics[2];
	}
	trajectory.push_back(Vehicle(this->lane, new_s, new_v, new_a, state));
	return trajectory;
}

vector<Vehicle> Vehicle::lane_change_trajectory(string state, map<int, vector<Vehicle>> predictions) {

	int new_lane = this->lane + lane_direction[state];
	vector<Vehicle> trajectory;
	Vehicle next_lane_vehicle;
	//Check if a lane change is possible (check if another vehicle occupies that spot).
	for (map<int, vector<Vehicle>>::iterator it = predictions.begin(); it != predictions.end(); ++it) {
		next_lane_vehicle = it->second[0];
		if (next_lane_vehicle.s == this->s && next_lane_vehicle.lane == new_lane) {
			//If lane change is not possible, return empty trajectory.
			return trajectory;
		}
	}
	trajectory.push_back(Vehicle(this->lane, this->s, this->v, this->a, this->state));
	vector<float> kinematics = get_kinematics(predictions, new_lane);
	trajectory.push_back(Vehicle(new_lane, kinematics[0], kinematics[1], kinematics[2], state));
	return trajectory;
}

vector<Vehicle> Vehicle::choose_next_state(map<int, vector<Vehicle>> predictions) {
	/*
	INPUT: A predictions map. This is a map of vehicle id keys with predicted
	vehicle trajectories as values. Trajectories are a vector of Vehicle objects representing
	the vehicle at the current timestep and one timestep in the future.
	OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state.
	*/
	vector<string> states = successor_states();
	float cost;
	vector<float> costs;
	vector<string> final_states;
	vector<vector<Vehicle>> final_trajectories;

	for (vector<string>::iterator it = states.begin(); it != states.end(); ++it) {
		vector<Vehicle> trajectory = generate_trajectory(*it, predictions);
		if (trajectory.size() != 0) {
			cost = calculate_cost(*this, predictions, trajectory);
			costs.push_back(cost);
			final_trajectories.push_back(trajectory);
		}
	}
	vector<float>::iterator best_cost = min_element(begin(costs), end(costs));
	int best_idx = distance(begin(costs), best_cost);
	return final_trajectories[best_idx];
}

float calculate_cost(const Vehicle & vehicle, const map<int, vector<Vehicle>> & predictions, const vector<Vehicle> & trajectory) {
    /*
    Sum weighted cost functions to get total cost for trajectory.
    */
    map<string, float> trajectory_data = get_helper_data(vehicle, trajectory, predictions);
    float cost = 0.0;

    //Add additional cost functions here.
    vector< function<float(const Vehicle & , const vector<Vehicle> &, const map<int, vector<Vehicle>> &, map<string, float> &)>> cf_list = {goal_distance_cost, inefficiency_cost};
    vector<float> weight_list = {REACH_GOAL, EFFICIENCY};
    
    for (int i = 0; i < cf_list.size(); i++) {
        float new_cost = weight_list[i]*cf_list[i](vehicle, trajectory, predictions, trajectory_data);
        cost += new_cost;
    }
    return cost;
}

cost = 1 - 2*exp(-(abs(2.0*vehicle.goal_lane - data["intended_lane"] - data["final_lane"]) / distance));

float cost = (2.0*vehicle.target_speed - proposed_speed_intended - proposed_speed_final)/vehicle.target_speed;

float new_cost = weight_list[i]*cf_list[i](vehicle, trajectory, predictions, trajectory_data);