const uint8_t Ap = 4;
const uint8_t An = 0;
const uint8_t Bn = 15;
const uint8_t Bp = 2;
const uint8_t NRedLed = 32;
const uint8_t RedLed = 26;
const uint8_t GreenLed =33;
const uint8_t BlueLed = 25;
const uint8_t StartCwButton = 23;
const uint8_t StartCcwButton = 21;
const uint8_t StopCcwButton = 19;
const uint8_t StopCwButton = 22;
const uint8_t EmStopButton = 5;
const uint8_t Endebryter = 17;

bool Standby = false;
bool Cw = false;
bool Ccw =false;
bool Stop = false;
bool endebryter = false;


void setup() 
{
  Serial.begin(115200);
  pinMode(Ap, OUTPUT);
  pinMode(An, OUTPUT);
  pinMode(Bp, OUTPUT);
  pinMode(Bn, OUTPUT);
  pinMode(NRedLed, OUTPUT);
  pinMode(RedLed, OUTPUT);
  pinMode(GreenLed, OUTPUT);
  pinMode(BlueLed, OUTPUT);
  pinMode(StartCwButton, INPUT_PULLUP);
  pinMode(StartCcwButton, INPUT_PULLUP);
  pinMode(StopCwButton, INPUT_PULLUP);
  pinMode(StopCcwButton, INPUT_PULLUP);
  pinMode(EmStopButton, INPUT_PULLUP);
  pinMode(Endebryter, INPUT_PULLUP);
}

void StandbyMode () 
{
  digitalWrite(RedLed, HIGH);
  digitalWrite(BlueLed, HIGH);
  digitalWrite(GreenLed, HIGH);
  digitalWrite(NRedLed, LOW);
}

void MotorCwMode () 
{
  digitalWrite(GreenLed, HIGH);
  digitalWrite(BlueLed, LOW);
  digitalWrite(RedLed, LOW);
  Standby = false;
}

void MotorCcwMode () 
{
  digitalWrite(BlueLed, HIGH);
  digitalWrite(GreenLed, LOW);
  digitalWrite(RedLed, LOW);
  Standby = false;
}

void StopMode () 
{
  digitalWrite(RedLed, HIGH);
  digitalWrite(NRedLed, HIGH);
  digitalWrite(GreenLed, LOW);
  digitalWrite(BlueLed, LOW);
  Stop = false;
  Cw = false;
  Ccw = false;
  Standby = false;
}

void EndebyrterMode () 
{
  Stop = false;
  Cw = false;
  Ccw = false;
  Standby = false;
  endebryter = false;
}



void CcwMotor () 
{
  digitalWrite(Bn,LOW) ;  digitalWrite(Ap,HIGH);
  delay(5);
  digitalWrite(Ap,LOW) ;  digitalWrite(Bp,HIGH);
  delay(5);
  digitalWrite(Bp,LOW) ;  digitalWrite(An,HIGH);
  delay(5);
  digitalWrite(An,LOW) ;  digitalWrite(Bn,HIGH);
}


void CwMotor () 
{
  digitalWrite(An, HIGH); digitalWrite(Bn, LOW);
  delay(5);
  digitalWrite(Bp, HIGH); digitalWrite(An, LOW);
  delay(5);
  digitalWrite(Ap, HIGH); digitalWrite(Bp, LOW);
  delay(5);
  digitalWrite(Bn, HIGH); digitalWrite(Ap, LOW);
}
void les_innganger () 
{
  if (digitalRead(StartCwButton) == LOW &&  (Standby)) {Cw = true;}
  if (digitalRead(StartCwButton)== LOW && (Ccw) & (!Standby)) 
  {
    Ccw = false;
    digitalWrite(BlueLed, LOW);
    delay(500);
    Cw = true;
  }
  if (digitalRead(StartCcwButton) == LOW &&  (Standby)) {Ccw = true;}
  if (digitalRead(StartCcwButton) == LOW && (Cw) && (!Standby)) 
  {
    Cw = false;
    digitalWrite(GreenLed, LOW);
    delay(500);
    Ccw = true;
  }
  if (digitalRead(StopCwButton) == LOW) {Stop = true;}
  if (digitalRead(StopCcwButton) == LOW) {Stop = true;}
  if (digitalRead(EmStopButton) == LOW) {Stop = true;}
  if (digitalRead(Endebryter) == LOW) {endebryter = true;}
  if (digitalRead(Stop) == false && (Cw) == false && (Ccw) == false) 
  {
    Standby = true;
  }
}

void loop () 
{
  les_innganger ();
  if (Standby) 
  {
    StandbyMode ();
  }
  if (Cw)
  {
    MotorCwMode ();
    CwMotor ();
  }
  if (Ccw) 
  {
    MotorCcwMode ();
    CcwMotor ();
  }
  if (Stop) 
  {
    StopMode ();
  }
  if (endebryter) 
  {
    EndebyrterMode ();
  }
}