/**
* The constructor for TL controllers
*
* @param The model being used.
*/
public SL1TLC(Infrastructure infra) throws InfraException {
super(infra);
Node[] nodes =
infra.getAllNodes(); // Moet Edge zijn eigenlijk, alleen testSimModel knalt er dan op
int num_nodes = nodes.length;
count = new Vector();
int numSigns = infra.getAllInboundLanes().size();
q_table = new float[numSigns + 1][][][];
int num_specialnodes = infra.getNumSpecialNodes();
for (int i = 0; i < nodes.length; i++) {
Node n = nodes[i];
Drivelane[] dls = n.getInboundLanes();
for (int j = 0; j < dls.length; j++) {
Drivelane d = dls[j];
Sign s = d.getSign();
int id = s.getId();
int num_pos_on_dl = d.getCompleteLength();
q_table[id] = new float[num_pos_on_dl][][];
for (int k = 0; k < num_pos_on_dl; k++) {
q_table[id][k] = new float[num_specialnodes][];
for (int l = 0; l < q_table[id][k].length; l++) {
q_table[id][k][l] = new float[2];
q_table[id][k][l][0] = 0.0f;
q_table[id][k][l][1] = 0.0f;
}
}
}
}
System.out.println("Startet med Alpha = " + alpha);
random_number = new Random();
}
/**
* Calculates how every traffic light should be switched Per node, per sign the waiting roadusers
* are passed and per each roaduser the gain is calculated.
*
* @param The TLDecision is a tuple consisting of a traffic light and a reward (Q) value, for it
* to be green
* @see gld.algo.tlc.TLDecision
*/
public TLDecision[][] decideTLs() {
int num_dec;
int num_tld = tld.length;
// Determine wheter it should be random or not
boolean do_this_random = false;
if (random_number.nextFloat() < random_chance) do_this_random = true;
for (int i = 0; i < num_tld; i++) {
num_dec = tld[i].length;
for (int j = 0; j < num_dec; j++) {
Sign currenttl = tld[i][j].getTL();
float gain = 0;
Drivelane currentlane = currenttl.getLane();
int waitingsize = currentlane.getNumRoadusersWaiting();
ListIterator queue = currentlane.getQueue().listIterator();
if (!do_this_random) {
for (; waitingsize > 0; waitingsize--) {
Roaduser ru = (Roaduser) queue.next();
int pos = ru.getPosition();
Node destination = ru.getDestNode();
gain +=
q_table[currenttl.getId()][pos][destination.getId()][1]
- q_table[currenttl.getId()][pos][destination.getId()][0]; // red - green
}
float q = gain;
} else gain = random_number.nextFloat();
tld[i][j].setGain(gain);
}
}
return tld;
}
protected void recalcQ(
Sign tl,
int pos,
Node destination,
boolean light,
Sign tl_new,
int pos_new,
boolean light_new,
PosMov[] posMovs) {
/* Recalculate the Q values, only one PEntry has changed, meaning also only 1 QEntry has to change
*/
int R;
float oldQvalue = 0;
float Qmark = 0;
float newQvalue = 0;
R = rewardFunction(tl_new, pos_new, posMovs);
try {
oldQvalue = q_table[tl.getId()][pos][destination.getId()][light ? green_index : red_index];
Qmark =
q_table[tl_new.getId()][pos_new][destination.getId()][
light_new ? green_index : red_index]; // Q( [ tl' , p' ] , L')
} catch (Exception e) {
System.out.println("ERROR");
System.out.println("tl: " + tl.getId());
System.out.println("pos:" + pos);
System.out.println("des:" + destination.getId());
}
newQvalue = oldQvalue + alpha * (R + gamma * Qmark - oldQvalue);
q_table[tl.getId()][pos][destination.getId()][light ? green_index : red_index] = newQvalue;
}
protected void recalcQa(Sign tl, int pos, Node destination, boolean light, Sign tl_new, int pos_new, PosMov[] posMovs)
{
float newQvalue=0;
int size = tl.getLane().getCompleteLength()-1;
int R;
int tlId = tl.getId();
int desId = destination.getId();
float Va;
for(; size>=0; size--) {
PEntry P = new PEntry(tl, pos, destination, light, tl, size);
int p_index = p_table[tlId][pos][desId].indexOf(P);
if(p_index>=0) {
try {
P = (PEntry) p_table[tlId][pos][desId].elementAt(p_index);
Va = va_table[tlId][size][desId];
R = rewardFunction(tl_new, pos_new, posMovs);
newQvalue += P.getValue() *(((float)R) + gamma * Va);
}
catch (Exception e) {
System.out.println("Error in recalc Q'");
}
}
}
try {
qa_table[tl.getId()][pos][destination.getId()][light?green_index:red_index] = newQvalue;
}
catch (Exception e) {
System.out.println("ERROR, Zwaluw is not found");
}
}
protected void recalcQ(Sign tl, int pos, Node destination, boolean light, Sign tl_new, int pos_new, PosMov[] posMovs, int Ktl)
{
/* The calculation of the Q values in TC-3 */
float newQvalue = qa_table[tl.getId()][pos][destination.getId()][light?green_index:red_index];
float V=0;
// Waarom splitst TC2 wel op rood/groen, en TC3 niet??
CountEntry currentsituation = new CountEntry (tl, pos, destination, light, tl_new, pos_new, Ktl);
Enumeration e = pKtl_table[tl.getId()][pos][destination.getId()].elements();
while(e.hasMoreElements()) {
PKtlEntry P = (PKtlEntry) e.nextElement();
if(P.sameSourceKtl(currentsituation) != -1.0f) {
try {
V = v_table[P.tl_new.getId()][P.pos_new][destination.getId()];
}
catch (Exception excep) {
System.out.println("ERROR in q");
}
// Moet er hier geen reward functie??
newQvalue += P.getValue() *gamma * V;
}
}
q_table[tl.getId()][pos][destination.getId()][light?green_index:red_index] = newQvalue; //sign, pos, des, color (red=0, green=1)
}
protected void recalcV(Sign tl, int pos, Node destination, boolean light, int Ktl)
{
/* The calculation of the V values in TC-3 */
float newVvalue;
float tempSumGreen=0, tempSumRed=0;
float V;
int[] amount = count(tl, pos, destination);
int tlId = tl.getId();
int desId = destination.getId();
float total = (float) amount[green_index] + (float) amount[red_index];
newVvalue = va_table[tl.getId()][pos][destination.getId()];
CountEntry currentsituation_green = new CountEntry (tl, pos, destination, green, tl, pos, Ktl);
CountEntry currentsituation_red = new CountEntry (tl, pos, destination, red, tl, pos, Ktl);
Enumeration e = pKtl_table[tlId][pos][desId].elements();
while(e.hasMoreElements()) {
//Green part
PKtlEntry P = (PKtlEntry) e.nextElement();
if(P.sameSourceKtl(currentsituation_green) != -1) {
try {
V = v_table[P.tl_new.getId()][P.pos_new][destination.getId()];
tempSumGreen += P.getValue() *gamma * V;
}
catch (Exception excep) {
System.out.println(excep+"");
excep.printStackTrace();
}
}
//Red Part
if(P.sameSourceKtl(currentsituation_red) != -1) {
try {
V = v_table[P.tl_new.getId()][P.pos_new][destination.getId()];
tempSumRed += P.getValue() *gamma * V;
}
catch (Exception excep) {
System.out.println("ERROR in recalc V2");
System.out.println(excep+"");
excep.printStackTrace();
}
}
}
newVvalue += ((float)amount[green_index]/ (float)total) * tempSumGreen + ((float)amount[red_index]/ (float)total) * tempSumRed;
try {
v_table[tl.getId()][pos][destination.getId()] = newVvalue;
}
catch (Exception excep) {
System.out.println("Error in v");
}
}
public void addRoad(Road r, int pos) throws InfraException {
if (r == null) throw new InfraException("Parameter r is null");
if (pos > 3 || pos < 0) throw new InfraException("Position out of range");
if (allRoads[pos] != null)
throw new InfraException("Road already connected to position " + pos);
allRoads[pos] = r;
Node other = r.getOtherNode(this);
if (other == null || !other.isAlphaRoad(r))
alphaRoads = (Road[]) Arrayutils.addElement(alphaRoads, r);
updateLanes();
calculateWidth();
}
protected int[] count(Sign tl, int pos, Node destination)
{
int tlId = tl.getId();
int desId = destination.getId();
int[] counters;
counters = new int[2];
//See the green_index definitions above !!!!
counters[green_index] = 0;
counters[red_index] = 0;
//Calcs the number of entries in the table matching the given characteristics, and returns the count
int psize = p_table[tlId][pos][desId].size()-1;
for(; psize>=0; psize--)
{
PEntry candidate = (PEntry) p_table[tlId][pos][desId].elementAt(psize);
if(candidate.tl.getId() == tlId && candidate.pos == pos && candidate.destination.getId() == desId) {
if(candidate.light == green) {
counters[green_index]++;
}
else {
counters[red_index]++;
}
}
}
return counters;
}
public float getColearnValue(Sign sign_new, Sign sign, Node destination, int pos)
{
int Ktl = sign.getLane().getNumRoadusersWaiting();
int tlId = sign.getId();
int desId = destination.getId();
// Calculate the colearning value
float newCovalue=0;
int size = sign.getLane().getCompleteLength()-1;
for(; size>=0; size--) {
float V;
PKtlEntry P = new PKtlEntry(sign, 0, destination, green, sign_new, size, Ktl);
int p_index = pKtl_table[tlId][pos][desId].indexOf(P);
if(p_index>=0) {
try {
P = (PKtlEntry) pKtl_table[tlId][pos][desId].elementAt(p_index);
V = v_table[tlId][size][desId];
newCovalue += P.getValue() * V;
}
catch (Exception e) {
System.out.println("Error");
}
}
}
return newCovalue;
}
public void saveChilds(XMLSaver saver)
throws XMLTreeException, IOException, XMLCannotSaveException {
super.saveChilds(saver);
XMLArray.saveArray(alphaRoads, this, saver, "alpha-roads");
XMLArray.saveArray(getRoadIdArray(), this, saver, "roads");
XMLArray.saveArray(getSignConfigIdArray(), this, saver, "sign-configs");
XMLArray.saveArray(getSignIdArray(), this, saver, "signs");
}
protected void recalcVa(Sign tl, int pos, Node destination)
{
float newWvalue;
float qa_red = qa_table[tl.getId()][pos][destination.getId()][red_index];
float qa_green = qa_table[tl.getId()][pos][destination.getId()][green_index];
int[] amount = count(tl, pos, destination);
float total = (float) amount[green_index] + (float) amount[red_index];
newWvalue = ((float)amount[green_index]/(float)total)*qa_green + ((float)amount[red_index]/(float)total)*qa_red;
try {
va_table[tl.getId()][pos][destination.getId()] = newWvalue;
}
catch (Exception e) {
System.out.println("Error in recalc W");
}
}
public void load(XMLElement myElement, XMLLoader loader)
throws XMLTreeException, IOException, XMLInvalidInputException {
super.load(myElement, loader);
width = myElement.getAttribute("width").getIntValue();
alphaRoads = (Road[]) XMLArray.loadArray(this, loader);
loadData.roads = (int[]) XMLArray.loadArray(this, loader);
loadData.signconfigs = (int[][]) XMLArray.loadArray(this, loader);
loadData.signs = (int[]) XMLArray.loadArray(this, loader);
}
public void reset() {
super.reset();
for (int i = 0; i < alphaRoads.length; i++) {
alphaRoads[i].reset();
}
for (int i = 0; i < signs.length; i++) {
signs[i].reset();
}
}
public float getVValue(Sign sign, Node des, int pos)
{
try {
return v_table[sign.getId()][pos][des.getId()];
}
catch (Exception e) {
System.out.print("Error in v_table");
return 0;
}
}
public XMLElement saveSelf () throws XMLCannotSaveException
{ XMLElement result=new XMLElement("pval");
result.addAttribute(new XMLAttribute("tl-id",tl.getId()));
result.addAttribute(new XMLAttribute("pos",pos));
result.addAttribute(new XMLAttribute("destination",destination.getId()));
result.addAttribute(new XMLAttribute("light",light));
result.addAttribute(new XMLAttribute("newtl-id",tl_new.getId()));
result.addAttribute(new XMLAttribute("new-pos",pos_new));
result.addAttribute(new XMLAttribute("value",value));
return result;
}
public XMLElement saveSelf () throws XMLCannotSaveException
{ XMLElement result=new XMLElement("count");
result.addAttribute(new XMLAttribute("tl-id",tl.getId()));
result.addAttribute(new XMLAttribute("pos",pos));
result.addAttribute(new XMLAttribute("destination",destination.getId()));
result.addAttribute(new XMLAttribute("light",light));
result.addAttribute(new XMLAttribute("newtl-id",tl_new.getId()));
result.addAttribute(new XMLAttribute("new-pos",pos_new));
result.addAttribute(new XMLAttribute("ktl",Ktl));
result.addAttribute(new XMLAttribute("value",value));
if ( ! infrastructure.laneDictionary.containsKey
(new Integer (tl.getId())))
{
System.out.println
("WARNING : Unknown Trafficlight ID "+tl.getId()+
" in TC3$CountEntry. Loading will go wrong");
}
return result;
}
public void loadSecondStage(Dictionary dictionaries)
throws XMLInvalidInputException, XMLTreeException {
super.loadSecondStage(dictionaries);
// Load roads
Dictionary roadDictionary = (Dictionary) (dictionaries.get("road"));
allRoads = new Road[loadData.roads.length];
for (int t = 0; t < loadData.roads.length; t++) {
allRoads[t] = (Road) (roadDictionary.get(new Integer(loadData.roads[t])));
if (allRoads[t] == null && loadData.roads[t] != -1)
System.out.println("Warning : " + getName() + " could not find road " + loadData.roads[t]);
}
// Load normal signs
Dictionary laneDictionary = (Dictionary) (dictionaries.get("lane"));
signs = new Sign[loadData.signs.length];
for (int t = 0; t < loadData.signs.length; t++)
signs[t] = getSign(laneDictionary, loadData.signs[t]);
// Load Signconfigurations
signconfigs = new Sign[loadData.signconfigs.length][2];
for (int t = 0; t < signconfigs.length; t++) {
signconfigs[t] = new Sign[loadData.signconfigs[t].length];
for (int u = 0; u < signconfigs[t].length; u++) {
signconfigs[t][u] = getSign(laneDictionary, loadData.signconfigs[t][u]);
}
}
// Tell *all* roads to load themselves
// It's possible that this Node has a BetaLane that has not been SecondStageLoaded
// And so we cant do an UpdateLanes() as that one needs secondStageData to proceed.
// Hence, we need to 2ndStage all Roads.
Enumeration e = new ArrayEnumeration(allRoads);
Road tmpRoad;
while (e.hasMoreElements()) {
tmpRoad = (Road) e.nextElement();
if (tmpRoad != null) tmpRoad.loadSecondStage(dictionaries);
}
try { // System.out.println("Trying to updateLanes()");
updateLanes();
} catch (InfraException x) {
throw new XMLInvalidInputException("Cannot initialize lanes of node " + nodeId);
}
}
public void setInfrastructure( Infrastructure infra )
{ super.setInfrastructure(infra);
Node[] nodes = infra.getAllNodes();
num_nodes = nodes.length;
try{
int numSigns = infra.getAllInboundLanes().size();
q_table = new float [numSigns][][][];
qa_table = new float [numSigns][][][];
v_table = new float [numSigns][][];
va_table = new float [numSigns][][];
count = new Vector[numSigns][][];
p_table = new Vector[numSigns][][];
pKtl_table = new Vector[numSigns][][];
int num_specialnodes = infra.getNumSpecialNodes();
for (int i=0; i<num_nodes; i++) {
Node n = nodes[i];
Drivelane[] dls = dls = n.getInboundLanes();
int num_dls = num_dls = dls.length;
// huh?
Drivelane [] lanes = new Drivelane[numSigns];
infra.getAllInboundLanes().copyInto(lanes);
for (int j=0; j<num_dls; j++) {
Drivelane d = dls[j];
Sign s = d.getSign();
int id = d.getId();
int num_pos_on_dl = d.getCompleteLength();
q_table[id] = new float [num_pos_on_dl][][];
qa_table[id] = new float [num_pos_on_dl][][];
v_table[id] = new float [num_pos_on_dl][];
va_table[id] = new float [num_pos_on_dl][];
count[id] = new Vector[num_pos_on_dl][];
p_table[id] = new Vector[num_pos_on_dl][];
pKtl_table[id] = new Vector[num_pos_on_dl][];
for (int k=0; k<num_pos_on_dl; k++) {
q_table[id][k] = new float[num_specialnodes][2];
qa_table[id][k] = new float[num_specialnodes][2];
v_table[id][k] = new float[num_specialnodes];
va_table[id][k] = new float[num_specialnodes];
count[id][k] = new Vector[num_specialnodes];
p_table[id][k] = new Vector[num_specialnodes];
pKtl_table[id][k] = new Vector[num_specialnodes];
for (int l=0; l<num_specialnodes;l++) {
q_table[id][k][l][0] = 0.0f;
q_table[id][k][l][1] = 0.0f;
qa_table[id][k][l][0] = 0.0f;
qa_table[id][k][l][1] = 0.0f;
v_table[id][k][l] = 0.0f;
va_table[id][k][l] = 0.0f;
count[id][k][l] = new Vector();
p_table[id][k][l] = new Vector();
pKtl_table[id][k][l] = new Vector();
}
}
}
}
random_number = new Random();
}
catch(Exception e) { System.out.println("Error."); }
}
protected void recalcP(Sign tl, int pos, Node destination, boolean light, Sign tl_new, int pos_new, int Ktl)
{
int tlId = tl.getId();
int desId = destination.getId();
//Update the count table
CountEntry currentsituation = new CountEntry(tl, pos, destination, light, tl_new, pos_new, Ktl);
int count_index = count[tlId][pos][desId].indexOf(currentsituation);
if (count_index>=0) {
currentsituation = (CountEntry) count[tlId][pos][desId].elementAt(count_index);
currentsituation.incrementValue();
}
else {
count[tlId][pos][desId].add(currentsituation);
}
//Update the p_table
PEntry currentchance = new PEntry(tl, pos, destination, light, tl_new, pos_new);
int dest=0, source=0;
Enumeration enum = count[tlId][pos][desId].elements();
while(enum.hasMoreElements()) {
CountEntry current = (CountEntry) enum.nextElement();
dest += current.sameSourceDifferentKtl(currentsituation);
source += current.sameSource(currentsituation);
}
if(source == 0) currentchance.setValue(0);
else currentchance.setValue((float)dest/(float)source);
int p_index = p_table[tlId][pos][desId].indexOf(currentchance);
if(p_index>=0) p_table[tlId][pos][desId].setElementAt(currentchance, p_index);
else {
p_table[tlId][pos][desId].add(currentchance);
p_index = p_table[tlId][pos][desId].indexOf(currentchance);
}
// Change the rest of the p_table, Also check the other chances for updates
int size = p_table[tlId][pos][desId].size()-1;
for(; size>=0; size--) {
PEntry P = (PEntry) p_table[tlId][pos][desId].elementAt(size);
float pvalue = P.sameSource(currentsituation);
if(pvalue > -1.0f) {
if(size != p_index)
P.setValue(pvalue * (float)(source-1) / (float)source);
}
}
//update the p'_table ......
PKtlEntry currentchance2 = new PKtlEntry(tl, pos, destination, light, tl_new, pos_new, Ktl);
source=0;
enum = count[tlId][pos][desId].elements();
while(enum.hasMoreElements()) {
source += ((CountEntry) enum.nextElement()).sameSourceWithKtl(currentsituation);
}
dest = currentsituation.getValue();
if(source == 0) currentchance2.setValue(0);
else currentchance2.setValue((float)dest/(float)source);
p_index = pKtl_table[tlId][pos][desId].indexOf(currentchance2);
if(p_index>=0) pKtl_table[tlId][pos][desId].setElementAt(currentchance2, p_index);
else {
pKtl_table[tlId][pos][desId].add(currentchance2);
p_index = pKtl_table[tlId][pos][desId].indexOf(currentchance2);
}
// Change the rest of the pKtl_table, Also check the other chances for updates
size = pKtl_table[tlId][pos][desId].size()-1;
for(; size>=0; size--) {
PKtlEntry P = (PKtlEntry) pKtl_table[tlId][pos][desId].elementAt(size);
float pvalue = P.sameSource(currentsituation);
if(pvalue > -1) {
if(size != p_index) {
P.setValue(pvalue * (float)(source-1) / (float)source);
}
}
}
if(currentchance.getValue() >1 ||currentchance2.getValue() >1 || currentchance.getValue() <0 ||currentchance2.getValue() <0 ) System.out.println("Serious error !!!!!!!!!1");
}