/**
 * \file
 * \brief Reflectance sensor driver implementation.
 * \author Erich Styger, erich.styger@hslu.ch
 * \license SPDX-License-Identifier: BSD-3-Clause
 * This module implements the driver for the bot front sensor.
 */
 
#include "platform.h"
#if PL_CONFIG_USE_LINE_SENSOR
#include "Reflectance.h"
#include "McuRTOS.h"
#include "McuGPIO.h"
#include "McuUtility.h"
#include "McuWait.h"
#if PL_CONFIG_USE_BUZZER
  #include "Buzzer.h"
#endif
#include "Application.h"
#if PL_CONFIG_HAS_NVM_CONFIG
  #include "NVM_Config.h"
#endif
#include "Shell.h"
#include "Timer.h"

typedef enum {
  REF_STATE_INIT,
  REF_STATE_NOT_CALIBRATED,
  REF_STATE_START_CALIBRATION,
  REF_STATE_CALIBRATING,
  REF_STATE_STOP_CALIBRATION,
  REF_STATE_SAVE_CALIBRATION,
  REF_STATE_READY
} RefStateType;

static volatile RefStateType refState = REF_STATE_INIT;

static struct {
  bool isEnabled; /* if sensor is enabled or not. If not enabled, it does not do any measurements */
  bool savePower; /* if true, the IR LEDs are turned off between measurements */
  McuGPIO_Handle_t irOn; /* HIGH active, pin to turn on/off power to the IR LEDs */
  McuGPIO_Handle_t sensor[REF_NOF_SENSORS]; /* pins for the IR sensor */
} REF_Sensor;

#define REF_USE_WHITE_LINE  0  /* if set to 1, then the robot is using a white (on black) line, otherwise a black (on white) line */

#define REF_START_STOP_CALIB      1 /* start/stop calibration commands */
#if REF_START_STOP_CALIB
  static SemaphoreHandle_t REF_StartStopSem = NULL;
#endif

#define REF_TIMEOUT_TICKS   (((TMR_GetRefelectanceTimerFrequencyHz()/1000)*REF_SENSOR_TIMEOUT_US)/1000) /* REF_SENSOR_TIMEOUT_US translated into timeout ticks */

typedef uint16_t SensorTimeType;
#define MAX_SENSOR_VALUE  ((SensorTimeType)-1)

/* type of NVM Configuration data (in FLASH) */
typedef struct SensorCalibT_ {
  SensorTimeType minVal[REF_NOF_SENSORS];
  SensorTimeType maxVal[REF_NOF_SENSORS];
} SensorCalibT;

static SensorCalibT *SensorCalibMinMaxPtr; /* pointer to calibrated data in FLASH, NULL if not calibrated */
static SensorCalibT *SensorCalibMinMaxTmpPtr=NULL; /* temporary calibration data */

static SensorTimeType SensorRaw[REF_NOF_SENSORS]; /* raw sensor values */
static SensorTimeType SensorCalibrated[REF_NOF_SENSORS]; /* 0 means white/min value, 1000 means black/max value */
static bool REF_LedOn = TRUE;
static int16_t refCenterLineVal=0; /* 0 means no line, >0 means line is below sensor 0, 1000 below sensor 1 and so on */
static REF_LineKind refLineKind = REF_LINE_NONE;
static uint16_t refLineWidth = 0;

void REF_GetSensorValues(uint16_t *values, int nofValues) {
  int i;

  for(i=0;i<nofValues && i<REF_NOF_SENSORS;i++) {
    values[i] = SensorCalibrated[i];
  }
}

/*!
 * \brief Measures the time until the sensor discharges
 * \param raw Array to store the raw values.
 * \return ERR_OVERFLOW if there is a timeout, ERR_OK otherwise
 */
static bool REF_MeasureRaw(SensorTimeType raw[REF_NOF_SENSORS], uint32_t timeoutCntVal) {
  uint8_t cnt; /* number of sensor */
  uint8_t i;
  uint32_t timerVal;

  if (!REF_Sensor.isEnabled) {
    return ERR_DISABLED;
  }
  if (REF_Sensor.savePower) {
    McuGPIO_SetHigh(REF_Sensor.irOn); /* turn IR LED's on */
    McuWait_Waitus(200);
  }
  for(i=0;i<REF_NOF_SENSORS;i++) {
    McuGPIO_SetAsOutput(REF_Sensor.sensor[i], true); /* turn I/O line as output */
    McuGPIO_SetHigh(REF_Sensor.sensor[i]);
    raw[i] = MAX_SENSOR_VALUE;
  }
  McuWait_Waitus(50); /* give at least 10 us to charge the capacitor */
  taskENTER_CRITICAL();
  for(i=0;i<REF_NOF_SENSORS;i++) {
    McuGPIO_SetAsInput(REF_Sensor.sensor[i]); /* turn I/O line as input */
  }
  TMR_ResetReflectanceTimerCounter();
  TMR_StartReflectanceTimer(); /* start counting */
  do {
    cnt = 0;
    timerVal = TMR_GetReflectanceTimerCounter();
    if (timerVal>timeoutCntVal) {
      break; /* get out of while loop */
    }
    for(i=0;i<REF_NOF_SENSORS;i++) {
      if (raw[i]==MAX_SENSOR_VALUE) { /* not measured yet? */
        if (McuGPIO_IsLow(REF_Sensor.sensor[i])) {
          raw[i] = timerVal;
        }
      } else { /* have value */
        cnt++;
      }
    }
  } while(cnt!=REF_NOF_SENSORS);
  taskEXIT_CRITICAL();
  TMR_StopReflectanceTimer();
  if (REF_Sensor.savePower) {
    McuGPIO_SetLow(REF_Sensor.irOn);; /* turn IR LED's off */
  }
  return ERR_OK;
}

static void REF_CalibrateMinMax(SensorTimeType min[REF_NOF_SENSORS], SensorTimeType max[REF_NOF_SENSORS], SensorTimeType raw[REF_NOF_SENSORS]) {
  int i;

  if (REF_MeasureRaw(raw, REF_TIMEOUT_TICKS)==ERR_OK) { /* if timeout, do not count values */
    for(i=0;i<REF_NOF_SENSORS;i++) {
      if (raw[i] < min[i]) {
        min[i] = raw[i];
      }
      if (raw[i]> max[i]) {
        max[i] = raw[i];
      }
    }
  }
}

static void ReadCalibrated(SensorTimeType calib[REF_NOF_SENSORS], SensorTimeType raw[REF_NOF_SENSORS]) {
  int i;
  int32_t x, denominator;
  uint32_t max;
  
  max = 0; /* determine maximum timeout value */
  for(i=0;i<REF_NOF_SENSORS;i++) {
    if (SensorCalibMinMaxPtr->maxVal[i]>max) {
      max = SensorCalibMinMaxPtr->maxVal[i];
    }
  }
  if (max > REF_TIMEOUT_TICKS) { /* limit to timeout value */
    max = REF_TIMEOUT_TICKS;
  }
  (void)REF_MeasureRaw(raw, max);
  if (SensorCalibMinMaxPtr==NULL) { /* no calibration data? */
    return;
  }
  for(i=0;i<REF_NOF_SENSORS;i++) {
    x = 0;
    denominator = SensorCalibMinMaxPtr->maxVal[i]-SensorCalibMinMaxPtr->minVal[i];
    if (denominator!=0) {
      x = (((int32_t)raw[i]-SensorCalibMinMaxPtr->minVal[i])*1000)/denominator;
    }
    if (x<0) {
      x = 0;
    } else if (x>1000) {
      x = 1000;
    }
    calib[i] = x;
  }
}

/*
 * Operates the same as read calibrated, but also returns an
 * estimated position of the robot with respect to a line. The
 * estimate is made using a weighted average of the sensor indices
 * multiplied by 1000, so that a return value of 1000 indicates that
 * the line is directly below sensor 0, a return value of 2000
 * indicates that the line is directly below sensor 1, 2000
 * indicates that it's below sensor 2000, etc. Intermediate
 * values indicate that the line is between two sensors. The
 * formula is:
 *
 * 1000*value0 + 2000*value1 + 3000*value2 + ...
 * --------------------------------------------
 * value0 + value1 + value2 + ...
 *
 * By default, this function assumes a dark line (high values)
 * surrounded by white (low values). If your line is light on
 * black, set the optional second argument white_line to true. In
 * this case, each sensor value will be replaced by (1000-value)
 * before the averaging.
 */
#define RETURN_LAST_VALUE  0
static int ReadLine(SensorTimeType calib[REF_NOF_SENSORS], SensorTimeType raw[REF_NOF_SENSORS], bool white_line) {
  int i;
  unsigned long avg; /* this is for the weighted total, which is long */
  /* before division */
  unsigned int sum; /* this is for the denominator which is <= 64000 */
  unsigned int mul; /* multiplication factor, 0, 1000, 2000, 3000 ... */
  int value;
#if RETURN_LAST_VALUE  
  bool on_line = FALSE;
  static int last_value=0; /* assume initially that the line is left. */
#endif
  
  (void)raw; /* unused */
  avg = 0;
  sum = 0;
  mul = 1000;
#if REF_SENSOR1_IS_LEFT
  for(i=0;i<REF_NOF_SENSORS;i++) {
#else
  for(i=REF_NOF_SENSORS-1;i>=0;i--) {
#endif
    value = calib[i];
    if(white_line) {
      value = 1000-value;
    }
    /* only average in values that are above a noise threshold */
    if(value > REF_MIN_NOISE_VAL) {
      avg += ((long)value)*mul;
      sum += value;
    }
    mul += 1000;
  }
#if RETURN_LAST_VALUE
  /* keep track of whether we see the line at all */
  if(value > REF_MIN_LINE_VAL) {
    on_line = TRUE;
  }
  if(!on_line) { /* If it last read to the left of center, return 0. */
    if(last_value < endIdx*1000/2) {
      return 0;
    } else { /* If it last read to the right of center, return the max. */
      return endIdx*1000;
    }
  }
  last_value = avg/sum;
  return last_value;
#else
  if (sum>0) {
    return avg/sum;
  } else {
    return avg;
  }
#endif
}

unsigned char *REF_LineKindStr(REF_LineKind line) {
  switch(line) {
  case REF_LINE_NONE:
    return (unsigned char *)"NONE";
  case REF_LINE_STRAIGHT:
    return (unsigned char *)"STRAIGHT";
  case REF_LINE_LEFT:
    return (unsigned char *)"LEFT";
  case REF_LINE_RIGHT:
    return (unsigned char *)"RIGHT";
  case REF_LINE_FULL:
    return (unsigned char *)"FULL";
  case REF_LINE_AIR:
    return (unsigned char *)"AIR";
  default:
    return (unsigned char *)"unknown";
  } /* switch */
}

REF_LineKind REF_GetLineKind(REF_LineKindMode mode) {
  if (mode==REF_LINE_KIND_MODE_ALL || mode==REF_LINE_KIND_MODE_MAZE || mode==REF_LINE_KIND_MODE_SUMO) {
    return refLineKind;
  } else if (mode==REF_LINE_KIND_MODE_LINE_FOLLOW) {
    switch(refLineKind) {
      case REF_LINE_NONE:
        return REF_LINE_NONE;

      case REF_LINE_LEFT:
      case REF_LINE_RIGHT:
      case REF_LINE_STRAIGHT:
        return REF_LINE_STRAIGHT;

      case REF_LINE_AIR:
      case REF_LINE_FULL:
        return REF_LINE_NONE;

      default:
        return REF_LINE_NONE;
    }
  }
  return refLineKind;
}

static bool SensorsSaturated(void) {
#if 0
  int i, cnt;
  
  /* check if robot is in the air or does see something */
  cnt = 0;
  for(i=0;i<REF_NOF_SENSORS;i++) {
    if (SensorRaw[i]==MAX_SENSOR_VALUE) { /* sensor not seeing anything? */
      cnt++;
    }
  }
  return (cnt==REF_NOF_SENSORS); /* all sensors see raw max value: not on the ground? */
#else
  return FALSE;
#endif
}

static REF_LineKind ReadLineKind(SensorTimeType val[REF_NOF_SENSORS]) {
  uint32_t sum, sumLeft, sumRight, outerLeft, outerRight, nofLines;
  int i;
  
  /* check if robot is in the air or does see something */
  if (SensorsSaturated()) {
    return REF_LINE_AIR;
  }
  for(i=0;i<REF_NOF_SENSORS;i++) {
    if (val[i]<REF_MIN_LINE_VAL) { /* smaller value? White seen! */
      break;
    }
  }
  if (i==REF_NOF_SENSORS) { /* all sensors see 'black' */
    return REF_LINE_FULL;
  }
  /* check the line type */
  sum = 0; sumLeft = 0; sumRight = 0; nofLines = 0;
  for(i=0;i<REF_NOF_SENSORS;i++) {
    if (val[i]>REF_MIN_LINE_VAL) { /* count only line values */
      nofLines++;
      sum += val[i];
      if (i<REF_NOF_SENSORS/2) {
#if REF_SENSOR1_IS_LEFT
        sumLeft += val[i];
#else
        sumRight += val[i];
#endif
      } else {
#if REF_SENSOR1_IS_LEFT
        sumRight += val[i];
#else
        sumLeft += val[i];
#endif
      }
    }
  }
#if REF_SENSOR1_IS_LEFT
  outerLeft = val[0];
  outerRight = val[REF_NOF_SENSORS-1];
#else
  outerLeft = val[REF_NOF_SENSORS-1];
  outerRight = val[0];
#endif
  
  #define MIN_LEFT_RIGHT_SUM   ((((REF_NOF_SENSORS)*2)/3)*REF_MIN_LINE_VAL) /* 2/3 of half the number of the sensors shall see a line */
  
  if (outerLeft<REF_MIN_LINE_VAL && outerRight<REF_MIN_LINE_VAL && nofLines>0) {
    /* both leftmost and rightmost sensors are seeing white, middle is seeing at least one black line */
    return REF_LINE_STRAIGHT; /* straight line forward */
  }

  if (outerLeft>=REF_MIN_LINE_VAL && outerRight<REF_MIN_LINE_VAL && sumLeft>MIN_LEFT_RIGHT_SUM && sumRight<MIN_LEFT_RIGHT_SUM) {
    return REF_LINE_LEFT; /* line going to the left side */
  } else if (outerLeft<REF_MIN_LINE_VAL && outerRight>=REF_MIN_LINE_VAL && sumRight>MIN_LEFT_RIGHT_SUM && sumLeft<MIN_LEFT_RIGHT_SUM) {
    return REF_LINE_RIGHT; /* line going to the right side */
  } else if (outerLeft>=REF_MIN_LINE_VAL && outerRight>=REF_MIN_LINE_VAL && sumRight>MIN_LEFT_RIGHT_SUM && sumLeft>MIN_LEFT_RIGHT_SUM) {
    return REF_LINE_FULL; /* full line */
  } else if (sumRight==0 && sumLeft==0 && sum == 0) {
    return REF_LINE_NONE; /* no line */
  } else { 
    return REF_LINE_STRAIGHT; /* straight line forward */
  }
}

uint16_t REF_LineWidth(void) {
  return refLineWidth;
}

static uint16_t CalculateRefLineWidth(SensorTimeType calib[REF_NOF_SENSORS]) {
  int32_t val;
  int i;

  val = 0;
  for(i=0;i<REF_NOF_SENSORS;i++) {
    if (calib[i]>=REF_MIN_NOISE_VAL) { /* sensor not seeing anything? */
      val += calib[i];
    }
  }
  return (uint16_t)val;
}

static void REF_Measure(void) {
  ReadCalibrated(SensorCalibrated, SensorRaw);
  refCenterLineVal = ReadLine(SensorCalibrated, SensorRaw, REF_USE_WHITE_LINE);
  refLineWidth = CalculateRefLineWidth(SensorCalibrated);
  if (refState==REF_STATE_READY) {
    refLineKind = ReadLineKind(SensorCalibrated);
  }
}

#if REF_START_STOP_CALIB
void REF_CalibrateStartStop(void) {
  REF_Sensor.isEnabled = TRUE;
  if (refState==REF_STATE_NOT_CALIBRATED || refState==REF_STATE_CALIBRATING || refState==REF_STATE_READY) {
    (void)xSemaphoreGive(REF_StartStopSem);
  }
}
#endif

bool REF_CanUseSensor(void) {
  return refState==REF_STATE_READY;
}

uint16_t REF_GetLineValue(bool *onLine) {
  *onLine = refCenterLineVal>0 && refCenterLineVal<REF_MAX_LINE_VALUE;
  return refCenterLineVal;
}

#if PL_CONFIG_USE_SHELL
static uint8_t PrintHelp(const McuShell_StdIOType *io) {
  McuShell_SendHelpStr((unsigned char*)"ref", (unsigned char*)"Group of Reflectance commands\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  help|status", (unsigned char*)"Print help or status information\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  enable|disable", (unsigned char*)"Enables or disables the reflectance measurement\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  calib (start|stop)", (unsigned char*)"Start/Stop calibrating while moving sensor over line\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  save power (on|off)", (unsigned char*)"Saves power with turning off IR between measurements\r\n", io->stdOut);
  return ERR_OK;
}

static unsigned char*REF_GetStateString(void) {
  switch (refState) {
    case REF_STATE_INIT:                return (unsigned char*)"INIT";
    case REF_STATE_NOT_CALIBRATED:      return (unsigned char*)"NOT CALIBRATED";
    case REF_STATE_START_CALIBRATION:   return (unsigned char*)"START CALIBRATION";
    case REF_STATE_CALIBRATING:         return (unsigned char*)"CALIBRATING";
    case REF_STATE_STOP_CALIBRATION:    return (unsigned char*)"STOP CALIBRATION";
    case REF_STATE_SAVE_CALIBRATION:    return (unsigned char*)"SAVE CALIBRATION";
    case REF_STATE_READY:               return (unsigned char*)"READY";
    default:
      break;
  } /* switch */
  return (unsigned char*)"UNKNOWN";
}

static uint8_t PrintStatus(const McuShell_StdIOType *io) {
  unsigned char buf[32];
  int i;

  McuShell_SendStatusStr((unsigned char*)"reflectance", (unsigned char*)"IR line sensor status\r\n", io->stdOut);
  
  McuShell_SendStatusStr((unsigned char*)"  enabled", REF_Sensor.isEnabled?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
#if REF_USE_WHITE_LINE
  McuShell_SendStatusStr((unsigned char*)"  line", (unsigned char*)"white\r\n", io->stdOut);
#else
  McuShell_SendStatusStr((unsigned char*)"  line", (unsigned char*)"black\r\n", io->stdOut);
#endif
  McuShell_SendStatusStr((unsigned char*)"  state", REF_GetStateString(), io->stdOut);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  McuShell_SendStatusStr((unsigned char*)"  save power", REF_Sensor.savePower?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);

  McuUtility_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
  McuUtility_strcatNum16Hex(buf, sizeof(buf), REF_MIN_NOISE_VAL);
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  McuShell_SendStatusStr((unsigned char*)"  min noise", buf, io->stdOut);

  McuUtility_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
  McuUtility_strcatNum16Hex(buf, sizeof(buf), REF_MIN_LINE_VAL);
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  McuShell_SendStatusStr((unsigned char*)"  min line", buf, io->stdOut);
  
  McuUtility_Num16uToStr(buf, sizeof(buf), REF_SENSOR_TIMEOUT_US);
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" us, 0x");
  McuUtility_strcatNum16Hex(buf, sizeof(buf), REF_TIMEOUT_TICKS);
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" ticks\r\n");
  McuShell_SendStatusStr((unsigned char*)"  timeout", buf, io->stdOut);

  McuShell_SendStatusStr((unsigned char*)"  raw val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
  for (i=0;i<REF_NOF_SENSORS;i++) {
    if (i==0) {
#else
  for (i=REF_NOF_SENSORS-1;i>=0;i--) {
    if (i==REF_NOF_SENSORS-1) {
#endif
      McuShell_SendStr((unsigned char*)"0x", io->stdOut);
    } else {
      McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
    }
    buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
    McuShell_SendStr(buf, io->stdOut);
  }
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  
  if (SensorCalibMinMaxPtr!=NULL) { /* have calibration data */
    McuShell_SendStatusStr((unsigned char*)"  min val", (unsigned char*)"", io->stdOut);
  #if REF_SENSOR1_IS_LEFT
    for (i=0;i<REF_NOF_SENSORS;i++) {
      if (i==0) {
  #else
    for (i=REF_NOF_SENSORS-1;i>=0;i--) {
      if (i==REF_NOF_SENSORS-1) {
  #endif
        McuShell_SendStr((unsigned char*)"0x", io->stdOut);
      } else {
        McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
      }
      buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMaxPtr->minVal[i]);
      McuShell_SendStr(buf, io->stdOut);
    }
    McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  }
  if (SensorCalibMinMaxPtr!=NULL) {
    McuShell_SendStatusStr((unsigned char*)"  max val", (unsigned char*)"", io->stdOut);
  #if REF_SENSOR1_IS_LEFT
    for (i=0;i<REF_NOF_SENSORS;i++) {
      if (i==0) {
  #else
    for (i=REF_NOF_SENSORS-1;i>=0;i--) {
      if (i==REF_NOF_SENSORS-1) {
  #endif
        McuShell_SendStr((unsigned char*)"0x", io->stdOut);
      } else {
        McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
      }
      buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMaxPtr->maxVal[i]);
      McuShell_SendStr(buf, io->stdOut);
    }
    McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  }

  if (SensorCalibMinMaxPtr!=NULL) { /* have calibration data */
    McuShell_SendStatusStr((unsigned char*)"  calib val", (unsigned char*)"", io->stdOut);
  #if REF_SENSOR1_IS_LEFT
    for (i=0;i<REF_NOF_SENSORS;i++) {
      if (i==0) {
  #else
    for (i=REF_NOF_SENSORS-1;i>=0;i--) {
      if (i==REF_NOF_SENSORS-1) {
  #endif
        McuShell_SendStr((unsigned char*)"0x", io->stdOut);
      } else {
        McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
      }
      buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
      McuShell_SendStr(buf, io->stdOut);
    }
    McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  }
  
  McuShell_SendStatusStr((unsigned char*)"  line pos", (unsigned char*)"", io->stdOut);
  buf[0] = '\0'; McuUtility_strcatNum16s(buf, sizeof(buf), refCenterLineVal);
  McuShell_SendStr(buf, io->stdOut);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);

  McuShell_SendStatusStr((unsigned char*)"  line width", (unsigned char*)"", io->stdOut);
  buf[0] = '\0'; McuUtility_strcatNum16s(buf, sizeof(buf), refLineWidth);
  McuShell_SendStr(buf, io->stdOut);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);

  McuShell_SendStatusStr((unsigned char*)"  line kind", REF_LineKindStr(refLineKind), io->stdOut);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  return ERR_OK;
}

uint8_t REF_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
  if (McuUtility_strcmp((char*)cmd, McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, "ref help")==0) {
    *handled = TRUE;
    return PrintHelp(io);
  } else if ((McuUtility_strcmp((char*)cmd, McuShell_CMD_STATUS)==0) || (McuUtility_strcmp((char*)cmd, "ref status")==0)) {
    *handled = TRUE;
    return PrintStatus(io);
  } else if (McuUtility_strcmp((char*)cmd, "ref enable")==0) {
    REF_Sensor.isEnabled = TRUE;
    *handled = TRUE;
    return ERR_OK;
  } else if (McuUtility_strcmp((char*)cmd, "ref disable")==0) {
    REF_Sensor.isEnabled = FALSE;
    *handled = TRUE;
    return ERR_OK;
  } else if (McuUtility_strcmp((char*)cmd, "ref calib start")==0) {
    if (refState==REF_STATE_NOT_CALIBRATED || refState==REF_STATE_READY) {
      APP_StateStartCalibrate();
    } else {
      McuShell_SendStr((unsigned char*)"ERROR: cannot start calibration, must not be calibrating or be ready.\r\n", io->stdErr);
      return ERR_FAILED;
    }
    *handled = TRUE;
    return ERR_OK;  
  } else if (McuUtility_strcmp((char*)cmd, "ref calib stop")==0) {
    if (refState==REF_STATE_CALIBRATING) {
      APP_StateStopCalibrate();
    } else {
      McuShell_SendStr((unsigned char*)"ERROR: can only stop if calibrating.\r\n", io->stdErr);
      return ERR_FAILED;
    }
    *handled = TRUE;
    return ERR_OK;
  } else if (McuUtility_strcmp((char*)cmd, "ref save power on")==0) {
    REF_Sensor.savePower = true;
    McuGPIO_SetLow(REF_Sensor.irOn);; /* turn IR LED's off */
    *handled = TRUE;
    return ERR_OK;
  } else if (McuUtility_strcmp((char*)cmd, "ref save power off")==0) {
    REF_Sensor.savePower = false;
    *handled = TRUE;
    return ERR_OK;
  }
  return ERR_OK;
}
#endif /* PL_CONFIG_USE_SHELL */

static void REF_StateMachine(void) {
  int i;

  switch (refState) {
    case REF_STATE_INIT:
#if PL_CONFIG_HAS_NVM_CONFIG
      SensorCalibMinMaxPtr = NVMC_GetReflectanceData();
      if (SensorCalibMinMaxPtr!=NULL) { /* use calibration data from FLASH */
        refState = REF_STATE_READY;
      } else {
#if PL_CONFIG_USE_SHELL
        SHELL_SendString((unsigned char*)"no calibration data present.\r\n");
#endif
        refState = REF_STATE_NOT_CALIBRATED;
      }
#else
      refState = REF_STATE_NOT_CALIBRATED;
#endif
      break;
      
    case REF_STATE_NOT_CALIBRATED:
      vTaskDelay(pdMS_TO_TICKS(80)); /* no need to sample that fast: this gives 80+20=100 ms */
      (void)REF_MeasureRaw(SensorRaw, REF_TIMEOUT_TICKS);
#if REF_START_STOP_CALIB
      if (xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
        refState = REF_STATE_START_CALIBRATION;
      }
#endif
      break;
    
    case REF_STATE_START_CALIBRATION:
#if PL_CONFIG_USE_SHELL
      SHELL_SendString((unsigned char*)"start calibration...\r\n");
#endif
      if (SensorCalibMinMaxTmpPtr!=NULL) {
        refState = REF_STATE_INIT; /* error case */
        break;
      }
      SensorCalibMinMaxTmpPtr = pvPortMalloc(sizeof(SensorCalibT));
      if (SensorCalibMinMaxTmpPtr!=NULL) { /* success */
        for(i=0;i<REF_NOF_SENSORS;i++) {
          SensorCalibMinMaxTmpPtr->minVal[i] = MAX_SENSOR_VALUE;
          SensorCalibMinMaxTmpPtr->maxVal[i] = 0;
          SensorCalibrated[i] = 0;
        }
        refState = REF_STATE_CALIBRATING;
      } else {
        refState = REF_STATE_INIT; /* error case */
      }
      break;
    
    case REF_STATE_CALIBRATING:
      vTaskDelay(pdMS_TO_TICKS(80)); /* no need to sample that fast: this gives 80+20=100 ms */
      if (SensorCalibMinMaxTmpPtr!=NULL) { /* safety check */
        REF_CalibrateMinMax(SensorCalibMinMaxTmpPtr->minVal, SensorCalibMinMaxTmpPtr->maxVal, SensorRaw);
      } else {
        refState = REF_STATE_INIT; /* error case */
        break;
      }
#if PL_CONFIG_USE_BUZZER
      (void)BUZ_Beep(300, 50);
#endif
#if REF_START_STOP_CALIB
      if (xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
        refState = REF_STATE_STOP_CALIBRATION;
      }
#endif
      break;
    
    case REF_STATE_STOP_CALIBRATION:
#if PL_CONFIG_USE_SHELL
      SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
#endif
      refState = REF_STATE_SAVE_CALIBRATION;
      break;
    
    case REF_STATE_SAVE_CALIBRATION:
#if PL_CONFIG_HAS_NVM_CONFIG
      if (NVMC_SaveReflectanceData((void*)SensorCalibMinMaxTmpPtr, sizeof(SensorCalibT))!=ERR_OK) {
#if PL_CONFIG_USE_SHELL
        SHELL_SendString((unsigned char*)"FAILED storing to FLASH!?!\r\n");
#endif
      }
      /* free memory */
      vPortFree(SensorCalibMinMaxTmpPtr);
      SensorCalibMinMaxTmpPtr = NULL;
      SensorCalibMinMaxPtr = NVMC_GetReflectanceData();
      if (SensorCalibMinMaxPtr!=NULL) { /* use calibration data from FLASH */
#if PL_CONFIG_USE_SHELL
        SHELL_SendString((unsigned char*)"calibration data saved.\r\n");
#endif
        refState = REF_STATE_READY;
        break;
      } else {
        refState = REF_STATE_INIT; /* error case */
      }
#else
      refState = REF_STATE_READY;
#endif
      break;
    
    case REF_STATE_READY:
      REF_Measure();
#if REF_START_STOP_CALIB
      if (xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
        refState = REF_STATE_START_CALIBRATION;
      }
#endif
      break;
  } /* switch */
}

static void ReflTask(void *pvParameters) {
  (void)pvParameters; /* not used */
  for(;;) {
    if (REF_Sensor.isEnabled) {
      REF_StateMachine();
      vTaskDelay(pdMS_TO_TICKS(5));
    } else {
      vTaskDelay(pdMS_TO_TICKS(100));
    }
  }
}

void REF_Init(void) {
  McuGPIO_Config_t gpioConfig;
  McuGPIO_HwPin_t hw[REF_NOF_SENSORS] =
  {
      {.gpio=GPIOD, .port=PORTD, .pin=2},
      {.gpio=GPIOD, .port=PORTD, .pin=3},
      {.gpio=GPIOD, .port=PORTD, .pin=4},
      {.gpio=GPIOD, .port=PORTD, .pin=5},
      {.gpio=GPIOD, .port=PORTD, .pin=6},
      {.gpio=GPIOD, .port=PORTD, .pin=7},
  };

  REF_Sensor.isEnabled = true;
  REF_Sensor.savePower = false; /* turn off, as this extends the ref sensor task time by around 200 us */
  McuGPIO_GetDefaultConfig(&gpioConfig);
  /* IR on/off: PTD1, high active */
  gpioConfig.hw.gpio = GPIOD;
  gpioConfig.hw.port = PORTD;
  gpioConfig.hw.pin = 1;
  gpioConfig.isInput = false;
  gpioConfig.isHighOnInit = false;
  REF_Sensor.irOn = McuGPIO_InitGPIO(&gpioConfig);
  REF_LedOn = false;
  /* IR sensors */
  gpioConfig.isInput = true;
  gpioConfig.isHighOnInit = false;
  for(int i=0; i<REF_NOF_SENSORS; i++) {
    gpioConfig.hw = hw[i];
    REF_Sensor.sensor[i] = McuGPIO_InitGPIO(&gpioConfig);
  }

#if REF_START_STOP_CALIB
  vSemaphoreCreateBinary(REF_StartStopSem);
  if (REF_StartStopSem==NULL) { /* semaphore creation failed */
    for(;;){} /* error */
  }
  (void)xSemaphoreTake(REF_StartStopSem, 0); /* empty token */
  vQueueAddToRegistry(REF_StartStopSem, "RefStartStopSem");
#if configUSE_TRACE_HOOKS
  vTraceSetQueueName(REF_StartStopSem, "RefStartStopSem");
#endif
#endif
  refState = REF_STATE_INIT;
#if PL_REMOTE_STOP_LINE
  refLineKind = REF_LINE_FULL; /* need an initial state, assume on black so the robot does not move */
#else
  refLineKind = REF_LINE_NONE;
#endif
  refCenterLineVal = 0;
  if (xTaskCreate(ReflTask, "Refl", 550/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+4, NULL) != pdPASS) {
    for(;;){} /* error */
  }
}
#endif /* PL_CONFIG_USE_LINE_SENSOR */