/********************************************************************************
 * Description:
 * This file contains all the functions needed for the light and scotch sensor 
 * detection logic.
 ********************************************************************************
 * Changes:
 * Max. Heise, 07.05.2002, 15H50 - created by cut and paste from cruise2.c
 * MAHE, 13.05.2002, 12H40 - now compiling as project of separated source files
 * MAHE, 13.05.2002, 14H30 - Added nore functions to be more abstract
 * MAHE, 14.05.2002, 15H00 - Of by one bug in _doSensorValuesShiftRight
 * MAHE, 15.05.2002, 17H00 - Found bugs in isSensorActive in calculation of middle
 *  and in _getSensorValuesAverage where 1/5 is always 0 for integer types
 ********************************************************************************/
#include "sensor.h"
#include <string.h> /* For memset */
/********************************************************************************
 * zero out sensor structure
 ********************************************************************************/
Sensor * setSensorToZero(Sensor *const this) {
  /********************
   * zero out sensor, 
   * is there anything like bzero
   * in the libc of this µP ?
   * => memset
   ********************/
  memset(this, 0x00, sizeof(Sensor));
  return this;
}
/********************************************************************************
 * accessor and mutatur for sensor's value at index
 ********************************************************************************/
SensorValue _getSensorValue(const Sensor *const this, const SensorIndex index) {
  if(index>=0 && index < MAXVALUES) {
    return this->sensorValues[index];
  } else {
    errorno=ERR_SENSORNO;
    return 0;
  }
}
Sensor * _setSensorValue(Sensor *const this, const SensorIndex index, const SensorValue newValue) {
  if(index>=0 && index < MAXVALUES) {
    this->sensorValues[index]=newValue;
  } else {
    errorno=ERR_SENSORNO;
  }
  return this;
}
/********************************************************************************
 * accessor and mutator for sensor's highest value
 ********************************************************************************/
Sensor * setSensorHighestValue(Sensor *const this, const SensorValue newValue) {
  this->highestValue=newValue;
  return this;
}
SensorValue getSensorHighestValue(const Sensor *const this) {
  return this->highestValue;
}
/********************************************************************************
 * accessor and mutator for sensor's lowest value
 ********************************************************************************/
Sensor * setSensorLowestValue(Sensor *const this, const SensorValue newValue) {
  this->lowestValue=newValue;
  return this;
}
SensorValue getSensorLowestValue(const Sensor *const this) {
  return this->lowestValue;
}
/********************************************************************************
 * Shift values in sensorValues
 * function not needed any more
 ********************************************************************************/
/*
Sensor * _doSensorValuesShiftRight(Sensor *const this) {
  int i;
  for(i=MAXVALUES-1;i>0;i--) {
    _setSensorValue(this, i, _getSensorValue(this, i-1));
  }
  return this;
}
*/
/********************************************************************************
 * compute average of sensor values
 ********************************************************************************/
SensorValue _getSensorValuesAverage(const Sensor *const this) {
  SensorValue retval=0;
  int i;
  for(i=0;i<MAXVALUES;i++) {
    retval+=_getSensorValue(this, i);
  }
  retval=retval/MAXVALUES;
  return retval;
}
/********************************************************************************
 * push value to sensor value array at index 0
 * funktion nod needed any more
 ********************************************************************************/
/*
Sensor * pushSensorValue(Sensor *const this, const SensorValue newValue) {
_doSensorValuesShiftRight(this);
_setSensorValue(this, 0, newValue);
return this;
}
*/
/********************************************************************************
 * Insert newvalue into arrat of last MAXVALUES sensor values
 ********************************************************************************/
Sensor *insertSensorValue(Sensor *const this, SensorValue newValue) {
  _setSensorValue(this, (this->insertIndex)++, newValue);
  if((this->insertIndex)>=MAXVALUES) {
    this->insertIndex=0;
  }
  return this;
}
/********************************************************************************
 * Is sensor active ?
 * This is calculated by taking the average of the SensorValues array
 * If this average is higher than (highestValue-lowestValue)/2 sensor is considered
 *  to be active.
 ********************************************************************************/
unsigned char isSensorActive(const Sensor *const this) {
  SensorValue middle; 
  SensorValue average; 
  unsigned char retval;
  SensorValue high, low;
  high=getSensorHighestValue(this);
  low=getSensorLowestValue(this);
  middle=((high-low)/2)+low;
  average=_getSensorValuesAverage(this); 
  /******************************
   * Note that we have inverted 
   *  sense. So the robot is on the scotch
   *  if average is below middle
   ******************************/
  (average < middle) ? retval=1 : retval=0;
  return retval;
}