/*------------------------------------------------------------------------------
----------------------------Project B.R.U.C.E. ---------------------------------
------------------------------- Versie 0.0 -------------------------------------
------------------------------------------------------------------------------*/

#include <iostream>
#include <mbed.h>

//-------------------------Lijst met defenities--------------------------------
#define Aan 1
#define Uit 0
#define Rechtdoor 1
#define Achteruit 2
#define Stil 0
#define Klaar 1

using namespace std;

//--------------------------------OUTPUTS---------------------------------------
//Motoren
DigitalOut Motor_Directi_L_achter(PC_10);
DigitalOut Motor_Directi_L_voor(PC_12);
PwmOut Motor_L_snelheid(PB_7);
DigitalOut Motor_Directi_R_achter(PA_15);
DigitalOut Motor_Directi_R_voor(PA_13);
PwmOut Motor_R_snelheid(PB_7);
PwmOut MotorPomp(PC_8);

//LED input
PwmOut LED_R(PB_15);
PwmOut LED_G(PB_14);
PwmOut LED_B(PB_13);

//--------------------------------INPUTS----------------------------------------
//Gyroscoop
AnalogIn Gyro_Xass();
AnalogIn Gyro_Yass();

//Sensoren Rand
DigitalIn Rand_Voor_Rechts(PA_12);
DigitalIn Rand_Achter_Rechts(PA_11);
DigitalIn Rand_Voor_Links(PB_12);
DigitalIn Rand_Achter_Links(PA_9);

//Sensoren onderkant
DigitalIn Onder_Voor_Rechts(PA_5);
DigitalIn Onder_Achter_Rechts(PA_6);
DigitalIn Onder_Voor_Links(PA_7);
DigitalIn Onder_Achter_Links(PB_2);

//motor encoders
DigitalIn Encoder_Rechts();
DigitalIn Encoder_Links();

//---------------------------------Globals--------------------------------------
int Stand_Van_Drukknop;
int bumper = 1;
int vloer = 1;
int bumper_links = 1;
Timer Debounce;
Timer Drukknop_Tijd_Waarde;

//--------------------------------Interupts-------------------------------------
void Start_Timer(){
    //timmer starter
    Drukknop_Tijd_Waarde.start();
}

void Stand_Motor_Keuzen(){
    // Waardes
    if((Drukknop_Tijd_Waarde.read() <= 1)&&( Drukknop_Tijd_Waarde.read() >= 2)){
        Stand_Van_Drukknop = 2;
    }
    else if((Drukknop_Tijd_Waarde.read() <= 1)&&( Drukknop_Tijd_Waarde.read() >= 2)){
        Stand_Van_Drukknop = 1;
    }
    else if((Drukknop_Tijd_Waarde.read() <= 1)&&( Drukknop_Tijd_Waarde.read() >= 2)){
        Stand_Van_Drukknop = 0;
    }

    //reset timer
    Drukknop_Tijd_Waarde.reset();
}

//----------------------Functies--------------------------
//motor drivers
void motor_drive(int links, int rechts){
    //motor links
    if(links == 1){
        cout<<"motor links aan";
    }
    else if(links == 0){
        cout<< "motor links uit";
    }

    //motor rechts
    if(rechts == 1){
        cout<<"motor rechts aan";
    }
    else if (rechts == 0){
        cout<< "motor rechts uit";
    }

    cout<< '\n';

}

void motor_pomp(int A){
    cout<<"Pomp aan";
    cout<< '\n';
}

void motor_richting(int links, int rechts){
    //motor links
    switch(links){
        case(0):
            cout << "motor links uit";
            break;
        case(1):
            cout << "motor links vooruit";
            break;
        case(2):
            cout << "motor links achteruit";
            break;
    }

    cout << '\t';

    //motor rechts
    switch(rechts){
        case(0):
            cout << "motor rechts uit";
            break;
        case(1):
            cout << "motor rechts vooruit";
            break;
        case(2):
            cout << "motor rechts achteruit";
            break;
    }

    cout<< '\n';
}

int Gyro_Uitlezen(){
    return(1);
}

void Homing_Naar_Rechts_Bovenin(){
    //inisizatie waardes
    int Gyro_waarde = 0;

    //start homing procdeuren
    //hommen op Y ass
    //kijken of de robot recht op de Yass staat
    do{
        motor_drive(Aan,Aan);
        motor_richting(Achteruit,Rechtdoor);
        Gyro_waarde = Gyro_Uitlezen();
    }while(Gyro_waarde != 1);

    //robot recht naar het eind van de Yass laten rijden
    do{
        motor_richting(Rechtdoor,Rechtdoor);
    }while(bumper < 1);

    //hommen op X ass
    //kijken of de robot recht op de X ass staat
    do{
        motor_richting(Achteruit,Rechtdoor);
        Gyro_waarde = Gyro_Uitlezen();
    }while(Gyro_waarde < 1);

    //robot rechdoor naar het eind van de X ass latern rijden
    do{
        motor_richting(Rechtdoor,Rechtdoor);
    }while(bumper < 1);

    //eind homing proceuren
    motor_richting(Stil,Stil);
}

int Startup(){
    //kijken of aan signaal is gegeven
    if(Stand_Van_Drukknop == Aan){//robot krijgt aan signaal
        //kijken of robot op de vloer staat
        while(vloer != 1){};
        motor_pomp(Aan); //pomp aan zetten
        Homing_Naar_Rechts_Bovenin(); // beginnen met homming van robot
        return(Klaar);//return klaar
    }
    else{//robot kriijgt geen aan singnaal
        return(0); //return geen aan signaal
    }
}

int Patroon_Rijden(){
    //basis waardes
    int Memory = 1; //1 is een nulpunt waarden
    int Gyro_afstel_waarden = 0; //0 is een nulpunt waarden
    int Motor_standA;
    int Motor_standB;
    //timer;

    //Rij patroon
    while(1){
        if (Memory == 1){ //standen van links naar rechts
            Motor_standA= Rechtdoor;
            Motor_standB= Achteruit;
            Memory = 2;
        }
        else if (Memory == 2){ //standen van rechts naar rechts
            Motor_standA= Achteruit;
            Motor_standB= Rechtdoor;
            Memory = 1;
        }

        //robot gaat rechtdoor
        motor_richting(Motor_standB, Motor_standB);
        motor_drive(Aan,Aan);

        //wacht tot rand gedeterteerdt word
        while(bumper != 1){}

        //detectie of de robot de onderkant van opervlakte raakt
        if (bumper_links == 1){
                return(1);
            }

        //draai proceduren
        //robot laten draaien
        motor_drive(Uit,Uit);
        motor_richting(Motor_standA, Motor_standB);
        motor_drive(Aan,Aan);

        //kijken of robot onder een hoek staat
        do{
            Gyro_afstel_waarden = Gyro_Uitlezen();
        }while(Gyro_afstel_waarden < 1);

        //onder een periode vooruit rijden voor baan veranderrn
        /*timer.start()
        while(timer = 1){}
        timer.stop;
        timer.rest;*/

        //robot laten draaien
        motor_richting(Stil,Stil);
        motor_richting(Motor_standB, Motor_standA);

        //kijken of robot recht staat
        do{
            Gyro_afstel_waarden = Gyro_Uitlezen();
        }while(Gyro_afstel_waarden < 1);
     }
}

void Afsluit_Proceduren(){
    //kijken of afsluit signaal is gegeven
    if(Stand_Van_Drukknop != 0){//ja afsluit signaal is gegeven
        Stand_Van_Drukknop = 0; // reset afsluit signaal
        motor_richting(Stil,Stil);  // stop motoren
        motor_drive(Uit,Uit);       //stoppen motoren

        /*timer.start() //count down for afsluiten zuiger
        while(timer = 1){}
        timer.stop;
        timer.rest;*/

        //pomp gaat uit
        motor_pomp(Uit);
    }
}

int main(){
    //inizatie waarde
    int status; //status van de robot
    Stand_Van_Drukknop = 1; //waarden van de drukknop

    //Interupt
    InterruptIn Signaal_Knop(PB_6);
    Signaal_Knop.rise(&Start_Timer);
    Signaal_Knop.fall(&Stand_Motor_Keuzen);

    //uitvoeren van het programma
    while(1){
        //beginnen met start up
        int memory = Startup();

        //programma
        if(memory == 1){
            status = Patroon_Rijden();
            cout << status;
        }
        Afsluit_Proceduren();

    }
}