File content as of revision 3:7cfbf73d6809:

#include "mbed.h"
#include <string>
#include <stdarg.h>
#include "TextLCD.h"
#include "MCP23017.h"
#include "math.h"
#include <list>    

const bool test = false;
int iter;
std::list<int> passedMarkers;
std::list<int>::iterator it;
int speedsetting; 

//BOX CONNECTIONS
DigitalIn button1 (p8);
DigitalIn button2 (p9);
DigitalIn button3 (p10);
DigitalIn button4 (p30);
AnalogIn potentio (p15); // configures pin15 for analog input. Creates object Ain. It is the potentiometer.
TextLCD lcd(p22, p21, p23, p24, p25, p26); // configures the LCD pins. Creates object lcd
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut boxled1 (p19);
DigitalOut boxled2 (p20);
DigitalOut buzzer (p29);
Serial pc(USBTX, USBRX); // Initialise the serial connection for terminal output
InterruptIn int0(p13);
InterruptIn int1(p14);
I2C i2c(p28, p27);
MCP23017 *mcp;  //hall detectors
DigitalOut data(p16); // configures pin16 for digital output. Creates object train. This signal should go to the booster
DigitalOut enable(p17); //configures pin 17 for digital output. creates object enable, this enables the train track or disables it in case of emergency

//CONSTANTS
const unsigned int DCCaddress_train1 = 0x01;
const unsigned int DCCaddress_train2 = 0x02;
const unsigned int DCCaddress_decoder = 0x06;
const unsigned int DCCinst_stop = 0x70;         //stop 01 1 1 0000
const unsigned int DCCinst_estop = 0x71;        //01 1 1 0001
const unsigned int DCCswitchneutral = 0b10000000;
const unsigned int DCCswitch1 = 0b10000001;
const unsigned int DCCswitch2 = 0b10000010;
const unsigned int DCCswitch3 = 0b10000100;
const unsigned int DCCswitch4 = 0b10001000;
const double stationWaitTime = 5.;
const double switchsafety = 4.; //contant time limit between uses of same switch
const int switchwait = 1500; //time inbetween different switch commands

//interrupt flags
bool int0flag;
bool int1flag;

//global variables for train1
bool dir1;
bool light1;
int speed1;
int prev1;
int curr1;
int next1;
Timer station1;
bool boarding1;
bool command1;

bool collision;

//global variables for train2
bool dir2;
bool light2;
int speed2;
int prev2;
int curr2;
int next2;
Timer station2;
bool boarding2;
bool command2;

//global variables for switches
std::time_t switch1action;
std::time_t switch2action;
std::time_t switch3action;
std::time_t switch4action;
bool switch1;
bool switch2;
bool switch3;
bool switch4;

//FUNCTION PROTOTYPES
void init_mcp(void);
void on_int0_change(void);
void on_int1_change(void);
std::list<int> circuit0markers(int sensor_data);
std::list<int> circuit1markers(int sensor_data);

void displayTerminalStatus(void);
void displayLCDstatus(void);

void init(void);
void initSwitches(void);
void startupWait(void);
void emergencyStop(void);
void handleInt0(void);
void handleInt1(void);
void trainCommand(void);
void updateTrainStatus(int marker);
int predictNextMarker(int prev, int curr);
void iterAction(void);

void changeSwitch1(bool);
void changeSwitch2(bool);
void changeSwitch3(bool);
void changeSwitch4(bool);
void updateTrainPredictions(void);

unsigned int trainInstruction(bool direction, bool light, int speed);
void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count);

void driveTrain(int train);
void reverseTrain(int train);
void stopTrain(int train);

void atStation2(int train);
void atStation12(int train);
void atSwitch3(int train);
void atSwitch4(int train);
void headCollision(void);
void sideCollision(void);
void xCollision(void);
bool isXCollision(void);
void tailCollision(int backTrain);
void atStation(int train);
//void loopDeltaCrossing(int train);

int main() {
    buzzer = 0;
    wait_ms(1000); //safety wait when starting (power surge)
    enable = false; //track off while doing startup
    while(1){
        init(); // initialize all the things to starting values
        startupWait(); //wait for user to press button 3 to begin
        enable = true;
        initSwitches(); //put switches in starting position
        wait_ms(2000); //power surge safety
        if(int0flag || int1flag){
            pc.printf("WARNING: Initialization power surge! Sensor ignored\n\r"); 
            if(int0flag){
                int trash = mcp->_read(INTCAPA);
                int0flag = false;
            }
            if(int1flag){
                int trash = mcp->_read(INTCAPB);
                int1flag = false;
            }  
        }
        
        if(!test){
            iter = 0; //iter count   
            while(enable){
                if(!button4){ 
                    emergencyStop();
                    break;
                }          
                
                if(int0flag){
                    handleInt0(); //store passed circuit 0 markers in the list passed markers
                }
            
                if(int1flag){
                    handleInt1(); //same for circuit 1
                }
                
                if(!passedMarkers.empty()){ 
                    for (it=passedMarkers.begin(); it!=passedMarkers.end(); ++it){
                        if(*it != next1 && *it != next2 && *it != curr1 && *it != curr2){  //TODO check if safe
                            pc.printf("ERROR: Unexpected marker %d. Expected train1: %d or train2: %d. Sensor failure or unknown object on track. Aborting.\n\r", *it, next1, next2);
                            enable = false;
                            break;   
                        }        
                        updateTrainStatus(*it);
                    }
                    passedMarkers.clear();  
                }
                
                trainCommand(); //decides for a command to send to the track
                
                iterAction(); //every X iterations display some stuff
                iter++;
            }
        } else {
            //test scripts   
        }
    }
}

//MAIN LOOP ROUTINES
void init(){  
    command1 = false;
    command2 = false;
    collision = false;
      
    //initialize clock time
    set_time(1);
    switch1action = time(NULL);
    switch2action = time(NULL);
    switch3action = time(NULL);
    switch4action = time(NULL);
    
    led1 = 0;
    led2 = 0;
    led3 = 0;
    buzzer = 0;
    boxled1 = 0;
    boxled2 = 0;
    
    speedsetting = 7;
    
    //pc.baud(921600); //faster printing
    pc.baud(19200);
    
    //initialize mcp   //(initialize interrupt flags)
    init_mcp();
  
    //initialize train globals
    dir1 = true;
    light1 = true;
    speed1 = speedsetting;
    boarding1 = false;
    //train 1 starting position
    prev1 = 0;
    curr1 = 1;
    next1 = predictNextMarker(prev1, curr1); //2

    //global variables for train2
    dir2 = true;
    light2 = true;
    speed2 = speedsetting;
    boarding2 = false;
    //train 2 starting position
    prev2 = 4;
    curr2 = 6;
    next2 = predictNextMarker(prev2, curr2); //7

    //initialize presumed switch positions
    switch1 = false;
    switch2 = false;
    switch3 = false;
    switch4 = false;
}

void startupWait(){
    lcd.cls();
    lcd.printf("Press START when ready");
    pc.printf("Press START when ready. Starting position: train 1 between marker 1 and 2, train 2 between marker 6 and 7.\n\r");  
    while(button3){
        //do nothing   
    }
    pc.printf("Starting...\n\r");  
}

void initSwitches(){
    wait_ms(switchwait);
    changeSwitch1(switch1);
    wait_ms(switchwait);
    changeSwitch2(switch2);
    wait_ms(switchwait);
    changeSwitch3(switch3);
    wait_ms(switchwait);
    changeSwitch4(switch4);
    wait_ms(500);
}

void emergencyStop(){
    enable = false;
    pc.printf("Emergency stop! Press button 3 when ready to restart\n\r");
    
    lcd.cls();
    lcd.printf("Press START when ready to restart");
    while(button3){
        //do nothing   
    }
}

void handleInt0(){
    wait_us(2000);
    int sensor_data = mcp->_read(INTCAPA);
    int0flag = false;
    std::list<int> temp = circuit0markers(sensor_data);
    passedMarkers.splice(passedMarkers.end(), temp);
}

void handleInt1(){
    wait_us(2000);
    int sensor_data = mcp->_read(INTCAPB);
    int1flag = false;  
    std::list<int> temp = circuit1markers(sensor_data);
    passedMarkers.splice(passedMarkers.end(), temp); 
}

void updateTrainStatus(int marker){
    if(marker == 21){
        return; //marker 21 is unused    
    }
    
    if(marker == next1){
        prev1 = curr1;
        curr1 = next1;
        next1 = predictNextMarker(prev1, curr1);
        command1 = false;
        collision = false;
        pc.printf("STATUS: Train 1 reached marker %d from marker %d. Predicted marker %d\n\r", curr1, prev1, next1);
    } else { //marker == next2
        prev2 = curr2;
        curr2 = next2;
        next2 = predictNextMarker(prev2, curr2);
        command2 = false;
        collision = false;
        pc.printf("STATUS: Train 2 reached marker %d from marker %d. Predicted marker %d\n\r", curr2, prev2, next2);
    }   
}

int predictNextMarker(int prev, int curr){
    switch(prev){
        case 0: switch(curr){
                    case 1: return 2;
                    case 13: return 12;
                }
        case 1: switch(curr){
                    case 2: return switch2?3:4;
                    case 0: return 13;
                }
        case 2: switch(curr){
                    case 3: return 9;
                    case 4: return 6;
                    case 1: return 0;
                }
        case 3: switch(curr){
                    case 9: return 8;
                    case 2: return 1;
                }
        case 4: switch(curr){
                    case 6: return 7;
                    case 2: return 1;
                }
        case 5: switch(curr){
                    case 6: return 7;
                    case 11: return 12;
                }
        case 6: switch(curr){
                    case 4: return 2;
                    case 5: return 11;
                    case 7: return 8;
                }
        case 7: switch(curr){
                    case 6: return switch3?5:4;
                    case 8: return switch4?9:10;
                }
        case 8: switch(curr){
                    case 7: return 6;
                    case 9: return 3;
                    case 10: return 12;
                }
        case 9: switch(curr){
                    case 3: return 2;
                    case 8: return 7;
                }
        case 10: switch(curr){
                    case 8: return 7;
                    case 12: return 13;
                }
        case 11: switch(curr){
                    case 5: return 6;
                    case 12: return 13;
                }
        case 12: switch(curr){
                    case 10: return 8;
                    case 11: return 5;
                    case 13: return 0;
                }
        case 13: switch(curr){
                    case 0: return 1;
                    case 12: return switch1?11:10;
                }       
    }
    return 666; //TODO default
}  

void iterAction(){
    displayLCDstatus();
    if(iter % 100 == 0){
        displayTerminalStatus();
    }
}

//TRAIN COMMAND
void trainCommand(){
    float Vin = potentio.read() * 3.3;
    speedsetting = floor(Vin/0.22);  //0.22 = 3.3/15  
    
    if(boarding1 || boarding2){
        if(station1.read() > stationWaitTime){ //timer works? TODO
             boarding1 = false;
             led1 = 0;
             station1.stop();          
        } 
                    
        if(station2.read() > stationWaitTime){
             boarding2 = false;
             led2 = 0;
             station2.stop();          
        }
    }
    speed1 = boarding1?0:speedsetting;
    speed2 = boarding2?0:speedsetting;
    
    if(next1 == next2 && !collision){
        headCollision();
        collision = true;
    } else if ((next1 == curr2 || next2 == curr1) && !collision){
        sideCollision();
        collision = true;
    } else if (isXCollision() && !collision){
        xCollision();
        collision = true;
    } else if (next1 == prev2 && !command1){
        tailCollision(1);
        command1 = true;
    } else if (next2 == prev1 && !command1){
        tailCollision(2);
        command1 = true;
    } else if (curr1 == 2 && !command1){
        atStation2(1);
        command1 = true;
    } else if (curr1 == 6 && !command1){
        atSwitch3(1);
        command1 = true;
    } else if (curr1 == 8 && !command1){
        atSwitch4(1);
        command1 = true;
    } else if (curr1 == 12 && !command1){
        atStation12(1);
        command1 = true;
    } else if (curr2 == 2 && !command2){
        atStation2(1);
        command2 = true;
    } else if (curr2 == 6 && !command2){
        atSwitch3(2);
        command2 = true;
    } else if (curr2 == 8 && !command2){
        atSwitch4(2);
        command2 = true;
    } else if (curr2 == 12 && !command2){
        atStation12(2);
        command2 = true;
    }
    
    driveTrain(2);
    driveTrain(1);
    
    /*
    else if (!command1 && (curr1 == 9 && next1 == 3) || (curr1 == 5 && next1 == 11)){
        loopDeltaCrossing(1);
        command1 = true;
    } else if (!command2 && (curr2 == 9 && next2 == 3) || (curr2 == 5 && next2 == 11)){
        loopDeltaCrossing(2);
        command2 = true;
    } 
    */
}

bool isXCollision(){
    return (curr1 == 3 && next1 == 9) && (next2 == 5 || next2 == 11)
        || (curr1 == 5 && next1 == 11) && (next2 == 3 || next2 == 9)
        || (curr1 == 9 && next1 == 3) && (next2 == 5 || next2 == 11)
        || (curr1 == 11 && next1 == 5) && (next2 == 3 || next2 == 9)
        || (curr2 == 3 && next2 == 9) && (next1 == 5 || next1 == 11)
        || (curr2 == 5 && next2 == 11) && (next1 == 3 || next1 == 9)
        || (curr2 == 9 && next2 == 3) && (next1 == 5 || next1 == 11)
        || (curr2 == 11 && next2 == 5) && (next1 == 3 || next1 == 9);
}

void stopTrain(int train){
    if(train == 1){
        DCC_send_command(DCCaddress_train1, DCCinst_estop, 10);
        pc.printf("STATUS: Train 1 stopped\n\r");
    } else { //train == 2
        DCC_send_command(DCCaddress_train2, DCCinst_estop, 10);
        pc.printf("STATUS: Train 2 stopped\n\r");
    }
}

void reverseTrain(int train){ //always stop before reversing
    if(train == 1){
        int stupid;
        dir1 = !dir1;
        stupid = next1;
        next1 = curr1;
        curr1 = stupid;
        prev1 = predictNextMarker(curr1, next1);
        pc.printf("STATUS: Train 1 reversed\n\r");
    } else { //train == 2
        int swap;
        dir2 = !dir2;
        swap = next2;
        next2 = curr2;
        curr2 = swap;
        prev2 = predictNextMarker(curr2, next2);
        pc.printf("STATUS: Train 2 reversed\n\r");
    }
}

void driveTrain(int train){
    if(train == 1){
        DCC_send_command(DCCaddress_train1, trainInstruction(dir1, light1, speed1), 25);
    } else { //train == 2
        DCC_send_command(DCCaddress_train2, trainInstruction(dir2, light2, speed2), 25);  
    }
}

void atStation2(int train){
    if(train == 1){
        if(next1 != 22){
            pc.printf("STATUS: train 1 reached switch 2\n\r");
            std::time_t current = time(NULL);
            if(difftime(current, switch2action) < switchsafety){
                pc.printf("STATUS: Switch 2 on overheat cooldown!\n\r"); 
                return;
            }
            stopTrain(1);
            if(curr2 == 6 || next2 == 6){
                switch2 = true;
            } else if(curr2 == 9 || next2 == 9){
                switch2 = false;
            } else { //other train far away
                switch2 = rand()%2;
            }
            changeSwitch2(switch2);
            switch2action = time(NULL);
        }
        atStation(1);
    } else { //train == 2
        if(next2 != 22){
            pc.printf("STATUS: train 2 reached switch 2\n\r");
            std::time_t current = time(NULL);
            if(difftime(current, switch2action) < switchsafety){
                pc.printf("STATUS: Switch 2 on overheat cooldown!\n\r"); 
                return;
            }
            stopTrain(2);
            if(curr1 == 6 || next1 == 6){
                //switch2 = true;
            } else if(curr1 == 9 || next1 == 9){
                //switch2 = false;
            } else { //other train far away
                switch2 = rand()%2;
            }
            changeSwitch2(switch2);
            switch2action = time(NULL); 
        }
        atStation(2);
    }
}

void atStation12(int train){
    if(train == 1){
        if(next1 != 22){
            pc.printf("STATUS: train 1 reached switch 1\n\r");
            std::time_t current = time(NULL);
            if(difftime(current, switch1action) < switchsafety){
                pc.printf("STATUS: Switch 1 on overheat cooldown!\n\r"); 
                return;
            }
            stopTrain(1);
            if(curr2 == 8 || next2 == 8){
                //switch1 = true;
            } else if(curr2 == 5 || next2 == 5){
                //switch1 = false;
            } else { //other train far away
                switch1 = rand()%2;
            }
            changeSwitch1(switch1);
            switch1action = time(NULL);
        }
        atStation(1);
    } else { //train == 2
        if(next2 != 22){
            pc.printf("STATUS: train 2 reached switch 1\n\r");
            std::time_t current = time(NULL);
            if(difftime(current, switch1action) < switchsafety){
                pc.printf("STATUS: Switch 1 on overheat cooldown!\n\r"); 
                return;
            }
            stopTrain(2);
            if(curr1 == 8 || next1 == 8){
                //switch1 = true;
            } else if(curr1 == 5 || next1 == 5){
                //switch1 = false;
            } else { //other train far away
                switch1 = rand()%2;
            }
            changeSwitch1(switch1);
            switch1action = time(NULL); 
        }
        atStation(2);
    }
}

void atStation(int train){
    if(train == 1){
        pc.printf("STATUS: Train 1 reached station!\n\r");
        boxled1 = 1;
        speed1 = 0;
        station1.start();
        boarding1 = true;
    } else { //train == 2
        pc.printf("STATUS: Train 2 reached station!\n\r");
        boxled2 = 1;
        speed2 = 0;
        station2.start();
        boarding2 = true;
    }
}

void atSwitch3(int train){
    if(train == 1){
        if(next1 == 7){ return; }
        pc.printf("STATUS: train 1 reached switch 3\n\r");
        std::time_t current = time(NULL);
        if(difftime(current, switch3action) < switchsafety){
                pc.printf("STATUS: Switch 3 on overheat cooldown!\n\r"); 
                return;
        }
        stopTrain(1);
        if(curr2 == 2 || next2 == 2){
            //switch3 = true; TODO complicated
        } else if(curr2 == 11 || next2 == 11){
            //switch3 = false;
        } else { //other train far away
            switch3 = rand()%2;
        }
        changeSwitch3(switch3);
        switch3action = time(NULL);
    } else { //train == 2
        if(next2 == 7){ return; }
        pc.printf("STATUS: train 2 reached switch 3\n\r");
        std::time_t current = time(NULL);
        if(difftime(current, switch3action) < switchsafety){
            pc.printf("STATUS: Switch 3 on overheat cooldown!\n\r"); 
            return;
        }
        stopTrain(2);
        if(curr1 == 2 || next1 == 2){
            //switch3 = true;
        } else if(curr1 == 11 || next1 == 11){
            //switch3 = false;
        } else { //other train far away
            switch3 = rand()%2;
        }
        changeSwitch3(switch3);
        switch3action = time(NULL);
    }
}

void atSwitch4(int train){
    if(train == 1){
        if(next1 == 7){ return; }
        pc.printf("STATUS: train 1 reached switch 4\n\r");
        std::time_t current = time(NULL);
        if(difftime(current, switch4action) < switchsafety){
            pc.printf("STATUS: Switch 4 on overheat cooldown!\n\r"); 
            return;
        }
        stopTrain(1);
        if(curr2 == 2 || next2 == 2){
            //switch4 = true;
        } else if(curr2 == 11 || next2 == 11){
            //switch4 = false;
        } else { //other train far away
            switch4 = rand()%2;
        }
        changeSwitch4(switch4);
        switch4action = time(NULL);
    } else { //train == 2
        if(next2 == 7){ return; }
        pc.printf("STATUS: train 2 reached switch 4\n\r");
        std::time_t current = time(NULL);
        if(difftime(current, switch4action) < switchsafety){
            pc.printf("STATUS: Switch 4 on overheat cooldown!\n\r"); 
            return;
        }
        stopTrain(2);
        if(curr1 == 2 || next1 == 2){
            //switch4 = true;
        } else if(curr1 == 11 || next1 == 11){
            //switch4 = false;
        } else { //other train far away
            switch4 = rand()%2;
        }
        changeSwitch4(switch4);
        switch4action = time(NULL);   
    }
}

void headCollision(void){
    buzzer = 1;
    pc.printf("WARNING: inbound head collision\n\r");
    stopTrain(1);
    stopTrain(2);
    reverseTrain(1);
    reverseTrain(2);
    buzzer = 0;
}

void sideCollision(void){
    buzzer = 1;
    pc.printf("WARNING: possible side collision\n\r");
    stopTrain(1);
    stopTrain(2);
    reverseTrain(1);
    reverseTrain(2);
    buzzer = 0;
}

void xCollision(void){
    buzzer = 1;
    pc.printf("WARNING: possible cross collision\n\r");
    stopTrain(1);
    stopTrain(2);
    reverseTrain(1);
    reverseTrain(2);
    buzzer = 0;
}

void tailCollision(int backTrain){
    buzzer = 1;
    pc.printf("WARNING: possible tail collision\n\r");
    stopTrain(backTrain);
    reverseTrain(backTrain);
    buzzer = 0;
}
/*
void loopDeltaCrossing(int train){
    buzzer = 1;
    pc.printf("WARNING: attempting to exit the loop\n\r");
    if(train == 1){
        if(curr2 != 13 && curr2 != 0 && curr2 != 1 && curr2 != 2){
            stopTrain(1);
            reverseTrain(1);   
        }
        //if train 2 in loop
            //stop, reverse
    } else { //train == 2    
        //if train 1 in loop
            //stop, reverse
        if(curr1 != 13 && curr1 != 0 && curr1 != 1 && curr1 != 2){
            stopTrain(1);
            reverseTrain(1);   
        }
    }
    buzzer = 0;
}
*/




//TRACK COMMANDS
//code from the tutorial, sends a message to the track through the train pin
void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
    switch(address){
        case DCCaddress_train1: led1 = 1; break;
        case DCCaddress_train2: led2 = 1; break;
        case DCCaddress_decoder: led3 = 1; break;    
    }
    
    unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
    unsigned __int64 temp_command = 0x0000000000000000;
    unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
    unsigned int error = 0x00; //error byte
    //calculate error detection byte with xor
    error = address ^ inst;
    //combine packet bits in basic DCC format
    command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01;
    //printf("\n\r %llx \n\r",command);
    int i=0;
    //repeat DCC command lots of times
    while(i < repeat_count) {
        temp_command = command;
    //loops through packet bits encoding and sending out digital pulses for a DCC command
        for (int j=0; j<64; j++) {
            if((temp_command&0x8000000000000000)==0) { //test packet bit
                //send data for a "0" bit
                data=0;
                wait_us(100);
                data=1;
                wait_us(100);
                //printf("0011");
            } else {
                //send data for a "1"bit
                data=0;
                wait_us(58);
                data=1;
                wait_us(58);
                //printf("01");
            }
            // next bit in packet
            temp_command = temp_command<<1;
        }
        i++;
    }
    
    led1 = 0;
    led2 = 0;
    led3 = 0;
}

void changeSwitch1(bool inwards){
    if(inwards){
        DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
        pc.printf("STATUS: Switch 1 changed inward\n\r");
    } else {
        DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
        pc.printf("STATUS: Switch 1 changed outward\n\r");        
    }
    updateTrainPredictions();
}

void changeSwitch2(bool inwards){
    if(inwards){
        DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
        pc.printf("STATUS: Switch 2 changed inward\n\r");
    } else {
        DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
        pc.printf("STATUS: Switch 2 changed outward\n\r");        
    }
    updateTrainPredictions();    
}

void changeSwitch3(bool inwards){
    if(inwards){
        DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);  //switch 3 has reverse behavior
        pc.printf("STATUS: Switch 3 changed inward\n\r");
    } else {
        DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
        pc.printf("STATUS: Switch 3 changed outward\n\r");
    }
    updateTrainPredictions();    
}

void changeSwitch4(bool inwards){
    if(inwards){
        DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
        pc.printf("STATUS: Switch 4 changed inward\n\r");
    } else {
        DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
        pc.printf("STATUS: Switch 4 changed outward\n\r");        
    }
    updateTrainPredictions();    
}

void updateTrainPredictions(){  
    next1 = predictNextMarker(prev1, curr1);
    next2 = predictNextMarker(prev2, curr2);   
}

unsigned int trainInstruction(bool direction, bool light, int speed){
    unsigned int result = 0x40; //0100 0000
    if(direction){
        result += 0x20;  //0010 0000
    } 
    if(light){
        result += 0x10;  //0001 0000
    }
    if(speed>0 && speed<15){
        result += speed + 1;   
    }
    return result;   
}

//INTERPRET INTERRUPT SIGNALS
//INTERRUPTS
//code from the tutorial, initializes MCP and sets it up for interrupts
void init_mcp() {
    // Initialisation of MCP registers, documentation on registers is available at
    //Niels/Abel/Robert/Natalia
    mcp = new MCP23017(i2c, 0x40);
    mcp->_write(IODIRA, (unsigned char )0xff);
    mcp->_write(IODIRB, (unsigned char )0xff);
    mcp->_write(IPOLA, (unsigned char )0x00);
    mcp->_write(IPOLB, (unsigned char )0x00);
    mcp->_write(DEFVALA, (unsigned char )0xff);
    mcp->_write(DEFVALB, (unsigned char )0xff);
    mcp->_write(INTCONA, (unsigned char )0xff);
    mcp->_write(INTCONB, (unsigned char )0xff);
    mcp->_write(IOCONA, (unsigned char )0x2);
    mcp->_write(IOCONB, (unsigned char )0x2);
    mcp->_write(GPPUA, (unsigned char )0xff);
    mcp->_write(GPPUB, (unsigned char )0xff);

    // Clear current interrupts
    mcp->_read(GPIOA);
    mcp->_read(GPIOB);
    // Register callbacks
    int0.fall(&on_int0_change);
    int1.fall(&on_int1_change);
    // Enable interrupts on the MCP
    mcp->_write(GPINTENA, (unsigned char )0xff);
    mcp->_write(GPINTENB, (unsigned char )0xff);
    
    int0flag = false;
    int1flag = false;
}

void on_int0_change() {
    int0flag = true;
}

void on_int1_change() {
    int1flag = true;
}

std::list<int> circuit0markers(int sensor_data){
    std::list<int> result;
    sensor_data = ~sensor_data;
    if((sensor_data & 0b00000001) == 0b00000001){
        result.push_back(0);
    }
    if((sensor_data & 0b00000010) == 0b00000010){
        result.push_back(1);
    }
    if((sensor_data & 0b00000100) == 0b00000100){
        result.push_back(2);
    }
    if((sensor_data & 0b00001000) == 0b00001000){
        result.push_back(3);
    }
    if((sensor_data & 0b00010000) == 0b00010000){
        result.push_back(4);
    }
    if((sensor_data & 0b00100000) == 0b00100000){
        result.push_back(5);
    }
    if((sensor_data & 0b01000000) == 0b01000000){
        result.push_back(6);
    }
    if((sensor_data & 0b10000000) == 0b10000000){
        result.push_back(7);
    }
    return result;
}    
    
std::list<int> circuit1markers(int sensor_data){
    std::list<int> result;
    sensor_data = ~sensor_data;
    if((sensor_data & 0b00000001) == 0b00000001){
        result.push_back(8);
    }
    if((sensor_data & 0b00000010) == 0b00000010){
        result.push_back(9);
    }
    if((sensor_data & 0b00000100) == 0b00000100){
        result.push_back(10);
    }
    if((sensor_data & 0b00001000) == 0b00001000){
        result.push_back(11);
    }
    if((sensor_data & 0b00010000) == 0b00010000){
        result.push_back(12);
    }
    if((sensor_data & 0b00100000) == 0b00100000){
        result.push_back(13);
    }
    if((sensor_data & 0b01000000) == 0b01000000){
        //result.push_back(21); do nothing; sensor 21 badly positioned
    }
    if((sensor_data & 0b10000000) == 0b10000000){
        //result.push_back(22); do nothing; too close
    }
    return result;
}

//LOG OUTPUT
void displayLCDstatus(){
    lcd.cls();
    lcd.printf("Train 1: %d \nTrain 2: %d", speed1, speed2);   
}

void displayTerminalStatus(){
    string dir1Text = dir1?"forward":"reverse";
    string dir2Text = dir2?"forward":"reverse";
    string light1Text = light1?"on, ":"off,";
    string light2Text = light2?"on, ":"off,";
    pc.printf("SYSTEM: Iteration %d\n\r", iter);
    pc.printf("Train 1: dir %s, light %s spd %d\n\rTrain 2: dir %s, light %s spd %d\n\r", dir1Text.c_str(), light1Text.c_str(), speed1, dir2Text.c_str(), light2Text.c_str(), speed2);   
}