#ifndef _ROBOCUP_SENSORS_H_
#define _ROBOCUP_SENSORS_H_

#include "qfixSoccerBoard.h"

#define SENSOR_PORT_LINIE_LINKS		0
#define SENSOR_PORT_LINIE_MITTE		1
#define SENSOR_PORT_LINIE_RECHTS	2

#define SENSOR_PORT_D_L		3
#define SENSOR_PORT_D_VL	4
#define SENSOR_PORT_D_V		5
#define SENSOR_PORT_D_VR	6
#define SENSOR_PORT_D_R		7

#define LINIE_WEISS 70
#define LINIE_FEHLER 71

#define SW_SCHWARZ(sensor) (sensor->getWert() > m_gw_sw)
#define SW_WEISS(sensor) (sensor->getWert() <= m_gw_sw)

#define C_SCHWARZ(sensor) (sensor->getWert() > m_gw_sg)
#define C_GRUEN(sensor) (sensor->getWert() > m_gw_gw && sensor->getWert() <= m_gw_sg)
#define C_WEISS(sensor) (sensor->getWert() > m_gw_ws && sensor->getWert() <= m_gw_gw)
#define C_SILBER(sensor) (sensor->getWert() <= m_gw_ws)


class Sensor {
 private:
  SoccerBoard *m_board;
  int m_port;
  int m_wert;
  
 public:
  Sensor(SoccerBoard *board, int port) {
    m_board = board;
    m_port = port;
  }
  
  void update();

  int getWert() {
    return m_wert;
  }
};


class Sensors {
 private:
  SoccerBoard *m_board;
  
  bool m_kalibriert;

  Sensor *m_lsLinks, *m_lsMitte, *m_lsRechts;
  int m_sLinks, m_sMitte, m_sRechts;
  int m_gLinks, m_gMitte, m_gRechts;
  int m_liniePos;
  
  int m_gw_sg;          // Grenzwert schwarz/grün
  int m_gw_gw;          // Grenzwert grün/weiß
  int m_gw_ws;          // Grenzwert weiß/silber
  int m_gw_sw;          // Grenzwert schwarz/weiß
  
  Sensor *m_dsL, *m_dsVL, *m_dsV, *m_dsVR, *m_dsR;
 public:
  Sensors(SoccerBoard *board) {
    m_board = board;
    
    m_kalibriert = false;
  
    m_lsLinks = new Sensor(board, SENSOR_PORT_LINIE_LINKS);
    m_lsMitte = new Sensor(board, SENSOR_PORT_LINIE_MITTE);
    m_lsRechts = new Sensor(board, SENSOR_PORT_LINIE_RECHTS);
    
    m_dsL = new Sensor(board, SENSOR_PORT_D_L);
    m_dsVL = new Sensor(board, SENSOR_PORT_D_VL);
    m_dsV = new Sensor(board, SENSOR_PORT_D_V);
    m_dsVR = new Sensor(board, SENSOR_PORT_D_VR);
    m_dsR = new Sensor(board, SENSOR_PORT_D_R);
    
    m_sLinks = m_sMitte = m_sRechts = 0;
    m_gLinks = m_gMitte = m_gRechts = 0;

    m_liniePos = 0;
  }

  ~Sensors() {
    delete m_lsLinks;
    delete m_lsMitte;
    delete m_lsRechts;
    
    delete m_dsL;
    delete m_dsVL;
    delete m_dsV;
    delete m_dsVR;
    delete m_dsR;
  }
  
  int getLinie() {return m_liniePos;}
  bool getOpfer() {
    return /*(m_sLinks && m_sMitte && m_sRechts) || */(m_gLinks && m_gMitte && m_gRechts);
  }  
  
  bool hindernisL() {
    return (m_dsL->getWert() > HINDERNIS_ERKENNUNGSWERT_SEITL);
  }
  
  bool hindernisVL() {
    return (m_dsVL->getWert() > HINDERNIS_ERKENNUNGSWERT);
  }
  
  bool hindernisV() {
    return (m_dsV->getWert() > HINDERNIS_ERKENNUNGSWERT);
  }
  
  bool hindernisVR() {
    return (m_dsVR->getWert() > HINDERNIS_ERKENNUNGSWERT);
  }
  
  bool hindernisR() {
    return m_dsR->getWert();
  }
  
  int getDsL() {
    return m_dsL->getWert();
  }
  
  int getDsVL() {
    return m_dsVL->getWert();
  }
  
  int getDsV() {
    return m_dsV->getWert();
  }
  
  int getDsVR() {
    return m_dsVR->getWert();
  }
  
  int getDsR() {
    return m_dsR->getWert();
  }
  
  void update();
  void auswerten();
  bool kalibrieren();
};

#endif