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


void Sensor::update() {
  m_board->analog(m_port);
  m_board->analog(m_port);
  m_wert = m_board->analog(m_port);
}



void Sensors::update() {
  m_lsLinks->update();
  m_lsMitte->update();
  m_lsRechts->update();
  
  m_dsL->update();
  m_dsVL->update();
  m_dsV->update();
  m_dsVR->update();
  m_dsR->update();
}

bool Sensors::kalibrieren() {
  int links = m_lsLinks->getWert();
  int mitte = m_lsMitte->getWert();
  int rechts = m_lsRechts->getWert();
  
  int weiss = (rechts + links) / 2;
  int diff = mitte - weiss;
  
  
  if(links > mitte || rechts > mitte) return false;
  
  if(diff < KALI_MINDIFF) return false;
  if(abs(rechts - links) > KALI_WEISSMAXDIFF) return false;
  
  if(!m_kalibriert) {
    m_gw_sg = mitte - diff/8;
    m_gw_gw = weiss + diff/8;
    m_gw_ws = weiss - diff/8;
    m_gw_sw = weiss + diff/2;
    
    m_kalibriert = true;
  }
  else {
    m_gw_sg = m_gw_sg/2 + mitte/2 - diff/16;
    m_gw_gw = m_gw_gw/2 + weiss/2 + diff/16;
    m_gw_ws = m_gw_ws/2 + weiss/2 - diff/16;
    m_gw_sw = m_gw_sw/2 + weiss/2 + diff/4;
  }
  
  return true;
}

void Sensors::auswerten() {
  // Linie
  m_liniePos = int(SW_SCHWARZ(m_lsRechts)) - int(SW_SCHWARZ(m_lsLinks)); //Linie links oder rechts -> -1 oder 1, beides oder keines -> 0
  m_liniePos *= (1+int(SW_WEISS(m_lsMitte))); //Kurvenstärke bestimmen
  
  if((!m_liniePos) && SW_WEISS(m_lsMitte)) //alles weiß?
    m_liniePos = LINIE_WEISS;
  
  if(SW_SCHWARZ(m_lsLinks) && SW_SCHWARZ(m_lsRechts)) // beide äußeren schwarz? -> fehler
    m_liniePos = LINIE_FEHLER;
  
  
  // Opfer
  if(m_sLinks) m_sLinks--;
  if(m_sMitte) m_sMitte--;
  if(m_sRechts) m_sRechts--;
  
  if(m_gLinks) m_gLinks--;
  if(m_gMitte) m_gMitte--;
  if(m_gRechts) m_gRechts--;
  
  if(C_SILBER(m_lsLinks)) m_sLinks = OPFER_TIMEOUT;
  if(C_SILBER(m_lsMitte)) m_sMitte = OPFER_TIMEOUT;
  if(C_SILBER(m_lsRechts)) m_sRechts = OPFER_TIMEOUT;
  
  if(C_GRUEN(m_lsLinks)) m_gLinks = OPFER_TIMEOUT;
  if(C_GRUEN(m_lsMitte)) m_gMitte = OPFER_TIMEOUT;
  if(C_GRUEN(m_lsRechts)) m_gRechts = OPFER_TIMEOUT;
}