#include <SPI.h>
#include <Controllino.h>

int fotocelica = CONTROLLINO_A1;
int magnets = CONTROLLINO_A2;
int zaprtos = CONTROLLINO_A3;
int odprtos = CONTROLLINO_A4;
int rdeca = CONTROLLINO_D4;
int zelena = CONTROLLINO_D5;
int M0 = CONTROLLINO_D0;
int M1 = CONTROLLINO_D1;
int M2 = CONTROLLINO_D2;
int M3 = CONTROLLINO_D3;
int x = 0;
int a = 1;

int fotocelica_read;
int magnets_read;
int zaprtos_read;
int odprtos_read;

int state = 0;

void setup() {
  pinMode(fotocelica, INPUT);
  pinMode(magnets, INPUT);
  pinMode(zaprtos, INPUT);
  pinMode(odprtos, INPUT);
  pinMode(rdeca, OUTPUT);
  pinMode(zelena, OUTPUT);
  pinMode(M0, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
}

void loop() {
    FC = fotocelica_read;
    MG = magnets_read;
    ZS = zaprtos_read;
    OS = odprtos_read;

    switch(state){
        case 0:{
            if (FC == true) && (MG == false) && (ZS == true) && (OS == false){              //zaprto
                state = 1;
                digitalWrite(rdeca, HIGH);
                digitalWrite(zelena, LOW);

                digitalWrite(M0, LOW);
                digitalWrite(M1, LOW);
                digitalWrite(M2, LOW);
                digitalWrite(M3, LOW);
            }break;
        }
        case 1:{
            if (FC == true) && (MG == true) && (ZS == true) && (OS == false){              //odpira
                state = 2;
                digitalWrite(rdeca, HIGH);
                digitalWrite(zelena, LOW);

                digitalWrite(M0, LOW);
                digitalWrite(M1, HIGH);
                digitalWrite(M2, HIGH);
                digitalWrite(M3, LOW);
            }break;
        }
        case 2:{
            if (FC == true) && (MG == true) && (ZS == false) && (OS == true){               //odrto luč zelena true
                state = 3;
                digitalWrite(rdeca, LOW);
                digitalWrite(zelena, HIGH);

                digitalWrite(M0, LOW);
                digitalWrite(M1, LOW);
                digitalWrite(M2, LOW);
                digitalWrite(M3, LOW);
            }break;
        }
        case 3:{
            if (FC == false) && (MG == false) && (ZS == false) && (OS == true){             //avto je pri light senzorju
                state = 4;
                digitalWrite(rdeca, LOW);
                digitalWrite(zelena, HIGH);

                digitalWrite(M0, LOW);
                digitalWrite(M1, LOW);
                digitalWrite(M2, LOW);
                digitalWrite(M3, LOW);
            }break;
        }
        case 4:{
            if (FC == true) && (MG == false) && (ZS == false) && (OS == true){               // avto je sel mimo --- rampa se lahko zapre
                state = 0;
                digitalWrite(rdeca, HIGH);
                digitalWrite(zelena, LOW);

                digitalWrite(M0, HIGH);
                digitalWrite(M1, LOW);
                digitalWrite(M2, LOW);
                digitalWrite(M3, HIGH);
            }break;
        }
    }
}