#include "Global.h"
#include "Navigation.h"
#include "Sensors.h"
#include "qfixSoccerBoard.h"



void OpferSignal(SoccerBoard *board) {
  board->ledOn(0);
  board->beep(OPFER_BEEP);
  board->sleep(OPFER_WAIT);
  board->ledOff(0);
  board->beepOff();
}


int main() {
  int status = STATUS_OK;
  int linie = 0, last_linie = 0;
  SoccerBoard board;
  Sensors sensors(&board);
  Navigation nav(&board);
  
  do {
    sensors.update();
  } while(!sensors.kalibrieren());
  
  board.ledOn(0);
  board.waitForButton(0);
  board.ledOff(0);
  board.sleep(4000);
  
  nav.start();
  
  while(true) {
    sensors.update();    
    sensors.auswerten();    
    
    last_linie = linie;
    linie = sensors.getLinie();
    
    switch(status) {
      case STATUS_OK:
        if(linie == 0) sensors.kalibrieren();
        
        if(sensors.getOpfer() && board.timer()) {
          nav.stop();
          OpferSignal(&board);
          nav.start();
          board.setTimer(OPFER_TIMER);
          break;
        }
        
        if(sensors.hindernisV()) {
          nav.setSpeed(SPEED_RUECK);
          nav.setRichtung(0);
	  board.sleep(HINDERNIS_SLEEP);
	  
	  nav.setSpeed(DEFAULT_SPEED);
	  
	  if(sensors.getDsVL() > sensors.getDsVR()) {
          // Hindernis eher links -> wir fahren rechts rum
            nav.setRichtung(2);
            status = STATUS_HINDERNIS_VOR_L; // status variable setzen
          }
          else {
          // Hindernis eher rechts -> wir fahren links rum
            nav.setRichtung(-2);
            status = STATUS_HINDERNIS_VOR_R; // status variable setzen
          }
          
          break;
        }
        
        switch(linie) {
          case LINIE_FEHLER:
            break;
          case LINIE_WEISS:
            /*if(abs(last_linie) == 2) break;
            
            nav.setRichtung(0);
            status = STATUS_WEISS;*/
            
            break;
          default:
            nav.setRichtung(linie);
        }
        
        break;
      case STATUS_WEISS:
        if(sensors.getOpfer() && board.timer()) {
          nav.stop();
          OpferSignal(&board);
          nav.start();
          board.setTimer(OPFER_TIMER);
          continue;
        }
        
        if(linie != LINIE_WEISS) status = STATUS_OK;
        break;
      case STATUS_HINDERNIS_VOR_L:
        if(sensors.hindernisL() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_L;
        break;
      case STATUS_HINDERNIS_VOR_R:
        if(sensors.hindernisR() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_R;
        break;
      case STATUS_HINDERNIS_L:
        if(sensors.hindernisVL()) {
          nav.setSpeed(SPEED_RUECK);
          nav.setRichtung(0);
	  board.sleep(HINDERNIS_SLEEP);
	  
	  nav.setSpeed(DEFAULT_SPEED);
	  nav.setRichtung(2);
	  status = STATUS_HINDERNIS_VOR_L;
        }
        else if(linie == LINIE_WEISS) {
          if(sensors.getDsL() > HINDERNIS_SOLLWERT + HINDERNIS_ABW)
            nav.setRichtung(2);
          else if(sensors.getDsL() < HINDERNIS_SOLLWERT - HINDERNIS_ABW)
            nav.setRichtung(-2);
          else
            nav.setRichtung(0);
        }
        else {
          status = STATUS_HINDERNIS_WAR_L;
          nav.setRichtung(0);	
        }
        break;
      case STATUS_HINDERNIS_R:
        if(sensors.hindernisVR()) {
          nav.setSpeed(SPEED_RUECK);
          nav.setRichtung(0);
	  board.sleep(HINDERNIS_SLEEP);
	  
	  nav.setSpeed(DEFAULT_SPEED);
	  nav.setRichtung(-2);
	  status = STATUS_HINDERNIS_VOR_R;
        }
        else if(linie == LINIE_WEISS) {
          if(sensors.getDsR() > HINDERNIS_SOLLWERT + HINDERNIS_ABW)
            nav.setRichtung(-2);
          else if(sensors.getDsR() < HINDERNIS_SOLLWERT - HINDERNIS_ABW)
            nav.setRichtung(2);
          else
            nav.setRichtung(0);
        }
        else {
          status = STATUS_HINDERNIS_WAR_R;
          nav.setRichtung(0);	
        }
        break;
      case STATUS_HINDERNIS_WAR_L:
        if(linie != LINIE_WEISS) break;
        
        board.sleep(40);
        status = STATUS_OK;
        nav.setRichtung(2);
        
        break;
      case STATUS_HINDERNIS_WAR_R:
        if(linie != LINIE_WEISS) break;
        
        board.sleep(40);
	status = STATUS_OK;
	nav.setRichtung(-2);
    }
  }
  
  return 0;
}