// Zawartosc pliku main.c
#include "sumolib.h"

// deklaracje funkcji
void decide();
void forward();
void forward_atack();
void forward_left();
void forward_right();
void backward();
void backward_atack();
void backward_left();
void backward_right();
void stop();
void rotate_left();
void rotate_right();
void countdown();
void timer1();

// zmienne globalne
volatile int dist_treshold = 0;
volatile int normal_power = 0;
volatile int dist1_detected = 0;
volatile int atack_power = 0;
volatile int dist2_detected = 0;
volatile int dist3_detected = 0;
volatile int started = 0;

int main() {
  init();
  // Wartości stałe
  dist_treshold = 300;
  normal_power = 40;
  atack_power = 100;
  // Planowanie działania timera
  timerSchedule(timer1, 1);
  // Oczekiwanie na naciśnięcie przycisku i odliczanie
  countdown();
  // Pętla główna programu
  for (;;) {
    // Decyzja o podejmowanej akcji
    decide();
    // Opóźnienie
    waitMs(20);
  }
}

void decide() {
  // Funkcja podejmująca decyzję o akcji - algorytm właściwy
  // Zmienne pomocnicze
  dist1_detected = distValue(DIST1) > dist_treshold;
  dist2_detected = distValue(DIST2) > dist_treshold;
  dist3_detected = distValue(DIST3) > dist_treshold;
  // Najwyższy priorytet mają czujniki podłoża, sprawdzanie czy którykolwiek z nich widzi linię

    // Żaden czujnik podłoża nie wykrył linii
    // Sprawdzanie, czy któryś z czujników odległości widzi przeciwnika
    if (dist1_detected || dist2_detected || dist3_detected) {
      // Którykolwiek czujnik odległości widzi przeciwnika, sprawdzanie dalej
      if (dist1_detected && dist2_detected) {
        // Przeciwnik jest na wprost
        forward_atack();
      }
    } else {
      // Żaden czujnik nie wykrył przeciwnika ani linii
      // Sprawdzanie czy robot wystartował
      if (!started) {
        // W zależności od stanu przełącznika wybór pierwszego ruchu
        if (switchIsOn(SWITCH2)) {
          forward_left();
        } else {
          forward_right();
        }
        // Zaznaczanie, że robot wystartował
        started = 1;
      }
    }

}

void forward() {
  // Do przodu
  motorSetPower(MOTOR1, normal_power);
  motorSetPower(MOTOR2, normal_power);
  motorForward(MOTOR1);
  motorForward(MOTOR2);
  ledOn(LED9);
  ledOff(LED10);
  ledOn(LED11);
}

void forward_atack() {
  // Do przodu - atak
  motorSetPower(MOTOR1, atack_power);
  motorSetPower(MOTOR2, atack_power);
  motorForward(MOTOR1);
  motorForward(MOTOR2);
  ledOn(LED9);
  ledOff(LED10);
  ledOn(LED11);
}

void forward_left() {
  // Do przodu na lewo
  motorSetPower(MOTOR1, normal_power / 3);
  motorSetPower(MOTOR2, normal_power);
  motorForward(MOTOR1);
  motorForward(MOTOR2);
  ledOn(LED9);
  ledOff(LED10);
  ledOff(LED11);
}

void forward_right() {
  // Do przodu na prawo
  motorSetPower(MOTOR1, normal_power);
  motorSetPower(MOTOR2, normal_power / 3);
  motorForward(MOTOR1);
  motorForward(MOTOR2);
  ledOff(LED9);
  ledOff(LED10);
  ledOn(LED11);
}

void backward() {
  // Do tyłu
  motorSetPower(MOTOR1, normal_power);
  motorSetPower(MOTOR2, normal_power);
  motorBackward(MOTOR1);
  motorBackward(MOTOR2);
  ledOff(LED9);
  ledOn(LED10);
  ledOff(LED11);
}

void backward_atack() {
  // Atak do tyłu
  motorSetPower(MOTOR1, atack_power);
  motorSetPower(MOTOR2, atack_power);
  motorBackward(MOTOR1);
  motorBackward(MOTOR2);
  ledOff(LED9);
  ledOn(LED10);
  ledOff(LED11);
}

void backward_left() {
  // Do tyłu na lewo
  motorSetPower(MOTOR1, normal_power / 3);
  motorSetPower(MOTOR2, normal_power);
  motorBackward(MOTOR1);
  motorBackward(MOTOR2);
  ledOn(LED9);
  ledOff(LED10);
  ledOff(LED11);
}

void backward_right() {
  // Do tyłu na prawo
  motorSetPower(MOTOR1, normal_power);
  motorSetPower(MOTOR2, normal_power / 3);
  motorBackward(MOTOR1);
  motorBackward(MOTOR2);
  ledOff(LED9);
  ledOff(LED10);
  ledOn(LED11);
}

void stop() {
  // Stop
  motorStop(MOTOR1);
  motorStop(MOTOR2);
  ledOff(LED9);
  ledOff(LED10);
  ledOff(LED11);
}

void rotate_left() {
  // Obrót w lewo
  motorSetPower(MOTOR1, normal_power);
  motorSetPower(MOTOR2, normal_power);
  motorBackward(MOTOR1);
  motorForward(MOTOR2);
  ledOn(LED9);
  ledOff(LED10);
  ledOff(LED11);
}

void rotate_right() {
  // Obrót w prawo
  motorSetPower(MOTOR1, normal_power);
  motorSetPower(MOTOR2, normal_power);
  motorForward(MOTOR1);
  motorBackward(MOTOR2);
  ledOff(LED9);
  ledOff(LED10);
  ledOn(LED11);
}

void countdown() {
  // Oczekiwanie na naciśnięcie któregoś przycisku
  while (!buttonPressed(BUTTON1) && !buttonPressed(BUTTON2)) {
  }
  // 5...
  ledOn(LED9);
  ledOff(LED10);
  ledOn(LED11);
  wait(1);
  // 4...
  ledOff(LED11);
  wait(1);
  // 3...
  ledOff(LED9);
  ledOn(LED10);
  ledOn(LED11);
  wait(1);
  // 2...
  ledOff(LED11);
  wait(1);
  // 1...
  ledOff(LED10);
  ledOn(LED11);
  wait(1);
  // Start
  ledOn(LED9);
  ledOn(LED10);
}

void timer1() {
  // Miganie diodą co 500 ms
  ledSet(LED8, timeInMilis() / 500 % 2);
  // Wyświetlenie n diodach stanów czujników
  ledSet(LED1, grdDetected(GRD1));
  ledSet(LED2, grdDetected(GRD2));
  ledSet(LED3, grdDetected(GRD3));
  ledSet(LED4, grdDetected(GRD4));
  ledSet(LED5, distValue(DIST1) > dist_treshold);
  ledSet(LED6, distValue(DIST2) > dist_treshold);
  ledSet(LED7, distValue(DIST3) > dist_treshold);
}