const int narR=7;//digitalni vstupy

const int narM=4;

const int narL=2;

const int prep=10;



const int IRSR=A5; //analogove vstupy

const int IRSL=A3;

const int IRSM=A4;

const int caraR=A1;

const int caraL=A2;



const int LED=13;//vystupy

const int vetrak=8;

const int RMF=5;

const int RMR=3;

const int LMF=9;

const int LMR=6;



const int dobaOtocky=1000;//jak dlouho se ma robot otacet(mS)



void setup() {

  pinMode(LED, OUTPUT);

  pinMode(narR, INPUT);

  pinMode(narM, INPUT);

  pinMode(vetrak, OUTPUT);

  pinMode(narL, INPUT);

  pinMode(prep, INPUT);

  pinMode(RMF, OUTPUT);

  pinMode(LMF, OUTPUT);

  pinMode(RMR, OUTPUT);

  pinMode(LMR, OUTPUT);

  pinMode(12, OUTPUT);

 // Serial.begin(9600);

 }



int stavNaraz() //vyhodnocuje stav narazoveho senzoru, uzivatel zvoli program stiskem narazoveho senzoru, 0->nedoslo k narazu, 1->naraz vlevo, 2->naraz uprostred, 3->naraz vpravo

{int R, L, M;

L=digitalRead(narL);

M=digitalRead(narM);

R=digitalRead(narR);

if(L==1&&R==1&&M==1){return 0;}//nedoslo k narazu

if(M<L&&M<R){return 2;}//naraz uprostred

   if(L<R){return 1;}//naraz vlevo

   else{return 3;}//naraz vpravo



}



 int stavSvicka() //0->svicka mimo dosah;  1->svicka vlevo;  2->svicka uprostred;  3->svicka vpravo; 4->jsem u svicky

 { int R, L, M;

   R=analogRead(IRSR);

   L=analogRead(IRSL);

   M=analogRead(IRSM);

   if(L>980&&R>980&&M>980){return 0;}//neni svicka

   if(L<33 or R<33 or M<33){return 4;}

    if(M<L&&M<R){return 2;}//svicka uprostred

   if(L<R){return 1;}//svicka vlevo

   else{return 3;}//svicka vpravo

 }



int svicka=0, naraz=0, LmotorF=0, LmotorR=0, RmotorF=0, RmotorR=0, stuj=0, program=0, start=0, mereniZac=0;

bool stavLED=0;

unsigned long casLED, cas, stopky;



void loop() 

{ cas=millis();

  while(start==0&&program==0)//podle narazoveho senzoru bude volen program,  L(1)->sledovani cary, M(2)-> narazy, R(3)->detekce svicky, 0->nezvoleno

        {program=stavNaraz();

        cas=millis();

        if(cas-casLED>100){casLED=cas; stavLED=!stavLED; digitalWrite(LED, stavLED);}//blikani LEDkou, cekani na zvoleni programu

        }

  if(start==0){start=1;}//zabrani opakovani volby programu pri dalsim pruchodu cyklem

        

  switch(program)

        {case 0: {

          break;}

         case 1: {

            RmotorF=1;LmotorF=1;RmotorR=0;LmotorR=0;//vychozi stav-oba motory jedou dopredu, pokud podminky neco nezmeni

            if(analogRead(caraL)>70){RmotorF=0;}//kdyz vyjede vlevo z cary, vypne pravy motor

            if(analogRead(caraR)>110){LmotorF=0;}//kdyz vyjede vpravo z cary, vypne levy motor

            if(analogRead(caraL)>70 && analogRead(caraR)>110){RmotorF=1;RmotorR=0;LmotorF=0;LmotorR=1;}//kdyz vyjede obema senzory, zacne se tocit na miste

            break;}

         case 2: {

            switch(naraz){

              case 0: {RmotorF=1;RmotorR=0;LmotorF=1;LmotorR=0; naraz=stavNaraz(); break;}//nedoslo k narazu, jede

              case 1: {RmotorF=0;RmotorR=1;LmotorF=0;LmotorR=1;//naraz vlevo, pravy motor R

                        if(mereniZac==0){mereniZac=1; stopky=cas;} 

                        if(cas-stopky>dobaOtocky){stopky=cas; mereniZac=0; naraz=5;}//az po otoceni se znova pta na naraz. Do te doby bere naraznik jako aktvni

                        break;}

              case 2: {RmotorF=0;RmotorR=1;LmotorF=0;LmotorR=1;//naraz uprostred, couva, pak se otoci 

                        if(mereniZac==0){mereniZac=1; stopky=cas;}

                        if(cas-stopky>dobaOtocky){stopky=cas; mereniZac=0; naraz=4;}

                        break;}

              case 3: {RmotorF=0;RmotorR=1;LmotorF=0;LmotorR=1;//naraz vpravo, levy motor R 

                        if(mereniZac==0){mereniZac=1; stopky=cas;}

                        if(cas-stopky>dobaOtocky){stopky=cas; mereniZac=0; naraz=4;}

                        break;}

              case 4: {RmotorF=1;RmotorR=0;LmotorF=0;LmotorR=1; //prikaz o otoceni vlevo

                        if(mereniZac==0){mereniZac=1; stopky=cas;}

                        if(cas-stopky>dobaOtocky){stopky=cas; mereniZac=0; naraz=stavNaraz();}        

                        break;}

              case 5: {RmotorF=0;RmotorR=1;LmotorF=1;LmotorR=0;//prikaz pro otoceni vpravo

                        if(mereniZac==0){mereniZac=1; stopky=cas;}

                        if(cas-stopky>dobaOtocky){stopky=cas; mereniZac=0; naraz=stavNaraz();}

                        break;}

              default: {break;}}

            break;}

         case 3: {                         //detekce svicky

            if(stuj==0){svicka=stavSvicka();}

            else{svicka=4;}

            switch (svicka){//podle polohy svicky nastavi oba motory

              case 0: {RmotorF=0;RmotorR=0;LmotorF=0;LmotorR=0; break;}

              case 1: {RmotorF=1;RmotorR=0;LmotorF=0;LmotorR=1;break;}

              case 2: {RmotorF=1;RmotorR=0;LmotorF=1;LmotorR=0;break;}

              case 3: {RmotorF=0;RmotorR=1;LmotorF=1;LmotorR=0;break;}

              case 4: {RmotorF=0;RmotorR=0;LmotorF=0;LmotorR=0; if(digitalRead(prep)!=1)digitalWrite(vetrak, 1);stuj=1; break;}

              default: break;

                 }

             break;}

         default: {break;}

          

        }

            

 if(analogRead(caraR)<54 or analogRead(caraL)<40){stuj=1;}//zastavi na kraji stolu

  if(digitalRead(prep)!=1&&stuj!=1){//sepne motory pouze kdyz neni vypnuty prepinac

  digitalWrite(RMF, RmotorF);

  digitalWrite(LMF, LmotorF);

  digitalWrite(RMR, RmotorR);

  digitalWrite(LMR, LmotorR);

  digitalWrite(12, LOW);}

  else{//zastavi vsechny motory, kdyz je prepinac prepnut

  digitalWrite(RMF, LOW);

  digitalWrite(LMF, LOW);

  digitalWrite(RMR, LOW);

  digitalWrite(LMR, LOW);

  digitalWrite(12, HIGH);}

}



//hodnoty IR senzoru cary:

//stul        L 184-185   R 262-263

//bily papir  L 209-210   R 324-326

//cerna cara  L  53- 55   R  99-103

//kraj stolu  L    < 36   R    < 54

//