/*
 * Copyright (c) 2021, Erich Styger
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "QuadCounter.h"
#include "McuUtility.h"
#include "McuShell.h"
#include "McuGPIO.h"

#define QuadCounter_GET_C1_PIN_LEFT()      (McuGPIO_GetValue(C1_Left))
#define QuadCounter_GET_C1_PIN_RIGHT()     (McuGPIO_GetValue(C1_Right))
#define QuadCounter_GET_C2_PIN_LEFT()      (McuGPIO_GetValue(C2_Left))
#define QuadCounter_GET_C2_PIN_RIGHT()     (McuGPIO_GetValue(C2_Right))

#define QuadCounter_GET_C1_PIN_LEFT()      (McuGPIO_GetValue(C1_Left))
#define QuadCounter_GET_C1_PIN_RIGHT()     (McuGPIO_GetValue(C1_Right))
#define QuadCounter_GET_C2_PIN_LEFT()      (McuGPIO_GetValue(C2_Left))
#define QuadCounter_GET_C2_PIN_RIGHT()     (McuGPIO_GetValue(C2_Right))

#if QuadCounter_SWAP_PINS
  #define QuadCounter_GET_C1_C2_PINS_LEFT()               ((QuadCounter_GET_C2_PIN_LEFT()?2:0)|(QuadCounter_GET_C1_PIN_LEFT()?1:0))
  #define QuadCounter_GET_C1_C2_PINS_RIGHT()              ((QuadCounter_GET_C2_PIN_RIGHT()?2:0)|(QuadCounter_GET_C1_PIN_RIGHT()?1:0))
  #define QuadCounter_GET_C1_C2_PINS_SWAPPED_LEFT()       ((QuadCounter_GET_C1_PIN_LEFT()?2:0)|(QuadCounter_GET_C2_PIN_LEFT()?1:0))
  #define QuadCounter_GET_C1_C2_PINS_SWAPPED_RIGHT()      ((QuadCounter_GET_C1_PIN_RIGHT()?2:0)|(QuadCounter_GET_C2_PIN_RIGHT()?1:0))
#else
  #define QuadCounter_GET_C1_C2_PINS_LEFT()               ((QuadCounter_GET_C1_PIN_LEFT()?2:0)|(QuadCounter_GET_C2_PIN_LEFT()?1:0))
  #define QuadCounter_GET_C1_C2_PINS_RIGHT()              ((QuadCounter_GET_C1_PIN_RIGHT()?2:0)|(QuadCounter_GET_C2_PIN_RIGHT()?1:0))
  #define QuadCounter_GET_C1_C2_PINS_SWAPPED_LEFT()       ((QuadCounter_GET_C2_PIN_LEFT()?2:0)|(QuadCounter_GET_C1_PIN_LEFT()?1:0))
  #define QuadCounter_GET_C1_C2_PINS_SWAPPED_RIGHT()      ((QuadCounter_GET_C2_PIN_RIGHT()?2:0)|(QuadCounter_GET_C1_PIN_RIGHT()?1:0))
#endif

static McuGPIO_Handle_t C1_Left, C2_Left, C1_Right, C2_Right;
#if QuadCounter_SWAP_PINS_AT_RUNTIME
  static bool QuadCounter_swappedPins_Left = FALSE;
  static bool QuadCounter_swappedPins_Right = FALSE;
#endif

/* The decoder has 4 different states, together with the previous state the table has 16 entries.
   The value in the table (0,1,-1) indicates the steps taken since previous sample. */
#define QUAD_ERROR  3 /*!< Value to indicate an error in impulse detection. Has to be different from 0,1,-1 */

static const signed char QuadCounter_Quad_Table[4][4] =
  {               /* prev   new    */
    {             /* c1 c2  c1 c2  */
     0,           /* 0  0   0  0  no change or missed a step? */
     1,           /* 0  0   0  1   */
     -1,          /* 0  0   1  0   */
     QUAD_ERROR   /* 0  0   1  1  error, lost impulse */
     },
    {             /* c1 c2  c1 c2  */
     -1,          /* 0  1   0  0   */
     0,           /* 0  1   0  1   no change or missed a step? */
     QUAD_ERROR,  /* 0  1   1  0   error, lost impulse */
     1            /* 0  1   1  1   */
     },
    {             /* c1 c2  c1 c2  */
     1,           /* 1  0   0  0   */
     QUAD_ERROR,  /* 1  0   0  1   error, lost impulse */
     0,           /* 1  0   1  0   no change or missed a step? */
     -1           /* 1  0   1  1   */
     },
    {             /* c1 c2  c1 c2  */
     QUAD_ERROR,  /* 1  1   0  0   error, lost impulse */
     -1,          /* 1  1   0  1   */
     1,           /* 1  1   1  0   */
     0            /* 1  1   1  1   no change or missed a step? */
     }
  };
static uint8_t QuadCounter_last_quadrature_value_left; /*! Value of left C1&C2 during last round. */
static uint8_t QuadCounter_last_quadrature_value_right; /*! Value of right C1&C2 during last round. */

static QuadCounter_QuadCntrType QuadCounter_currPos_Left = 0; /*!< Current left position */
static QuadCounter_QuadCntrType QuadCounter_currPos_Right = 0; /*!< Current right position */

static uint32_t QuadCounter_errorsLeft, QuadCounter_errorsRight;

uint32_t QuadCounter_GetErrorsLeft(void) {
  return QuadCounter_errorsLeft;
}

uint32_t QuadCounter_GetErrorsRight(void) {
  return QuadCounter_errorsRight;
}

/*
** ===================================================================
**     Method      :  SetPos (component QuadCounter)
**
**     Description :
**         Sets the position information. Can be used as well to reset
**         the position information.
**     Parameters  :
**         NAME            - DESCRIPTION
**         pos             - Position value to be set.
**     Returns     : Nothing
** ===================================================================
*/
void QuadCounter_SetPosLeft(QuadCounter_QuadCntrType pos)
{
  QuadCounter_currPos_Left = pos;
}

void QuadCounter_SetPosRight(QuadCounter_QuadCntrType pos)
{
  QuadCounter_currPos_Right = pos;
}

/*
** ===================================================================
**     Method      :  GetPos (component QuadCounter)
**
**     Description :
**         Returns the current position based on the encoder tracking.
**     Parameters  : None
**     Returns     :
**         ---             - position
** ===================================================================
*/
QuadCounter_QuadCntrType QuadCounter_GetPosLeft(void)
{
  return QuadCounter_currPos_Left;
}

QuadCounter_QuadCntrType QuadCounter_GetPosRight(void)
{
  return QuadCounter_currPos_Right;
}

#if QuadCounter_SWAP_PINS_AT_RUNTIME
void QuadCounter_SwapPinsLeft(bool swap) {
  QuadCounter_swappedPins_Left = swap;
}

void QuadCounter_SwapPinsRight(bool swap) {
  QuadCounter_swappedPins_Right = swap;
}
#endif

/*
** ===================================================================
**     Method      :  GetVal (component QuadCounter)
**
**     Description :
**         Returns the quadrature value (0, 1, 2 or 3)
**     Parameters  : None
**     Returns     :
**         ---             - Quadrature value (0-3)
** ===================================================================
*/
uint8_t QuadCounter_GetValLeft(void)
{
#if QuadCounter_SWAP_PINS_AT_RUNTIME
  if (QuadCounter_swappedPins_Left) {
    return QuadCounter_GET_C1_C2_PINS_SWAPPED_LEFT();
  } else {
    return QuadCounter_GET_C1_C2_PINS_LEFT();
  }
#else
  return QuadCounter_GET_C1_C2_PINS_LEFT();
#endif
}

uint8_t QuadCounter_GetValRight(void)
{
#if QuadCounter_SWAP_PINS_AT_RUNTIME
  if (QuadCounter_swappedPins_Right) {
    return QuadCounter_GET_C1_C2_PINS_SWAPPED_RIGHT();
  } else {
    return QuadCounter_GET_C1_C2_PINS_RIGHT();
  }
#else
  return QuadCounter_GET_C1_C2_PINS_RIGHT();
#endif
}

/*
** ===================================================================
**     Method      :  Sample (component QuadCounter)
**
**     Description :
**         Call this method to periodically sample the signals.
**     Parameters  : None
**     Returns     : Nothing
** ===================================================================
*/
void QuadCounter_Sample(void)
{
  signed char new_step;
  uint8_t c12; /* value of the two sensor input */

  /* left */
  c12 = QuadCounter_GetValLeft();
  new_step = QuadCounter_Quad_Table[QuadCounter_last_quadrature_value_left][c12];
  QuadCounter_last_quadrature_value_left = c12;
  if (new_step == QUAD_ERROR) {
    QuadCounter_errorsLeft++;
  } else if (new_step != 0) {
    QuadCounter_currPos_Left += new_step;
  }
  /* right */
  c12 = QuadCounter_GetValRight();
  new_step = QuadCounter_Quad_Table[QuadCounter_last_quadrature_value_right][c12];
  QuadCounter_last_quadrature_value_right = c12;
  if (new_step == QUAD_ERROR) {
    QuadCounter_errorsRight++;
  } else if (new_step != 0) {
    QuadCounter_currPos_Right += new_step;
  }
}

/*
** ===================================================================
**     Method      :  ParseCommand (component QuadCounter)
**
**     Description :
**         Handler to process shell commands
**     Parameters  :
**         NAME            - DESCRIPTION
**         cmd             - Command string to be parsed
**       * handled         - Pointer to boolean. The handler
**                           sets this variable to TRUE if command was
**                           handled, otherwise let it untouched.
**         io              - Pointer to I/O structure
**     Returns     :
**         ---             - Error code
** ===================================================================
*/
/*!
 * \brief Parses a command
 * \param cmd Command string to be parsed
 * \param handled Sets this variable to TRUE if command was handled
 * \param io I/O stream to be used for input/output
 * \return Error code, ERR_OK if everything was fine
 */
uint8_t QuadCounter_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io)
{
  uint8_t res=ERR_OK;

  if (McuUtility_strcmp((const char*)cmd, McuShell_CMD_HELP)==0 || McuUtility_strcmp((const char *)cmd, "QuadCounter help")==0) {
    McuShell_SendHelpStr((const unsigned char*)"QuadCounter", (const unsigned char*)"QuadCounter command group\r\n", io->stdOut);
    McuShell_SendHelpStr((const unsigned char*)"  help|status", (const unsigned char*)"Print help or status information\r\n", io->stdOut);
    McuShell_SendHelpStr((const unsigned char*)"  reset", (const unsigned char*)"Reset the current position counter\r\n", io->stdOut);
    *handled = TRUE;
  } else if (McuUtility_strcmp((const char*)cmd, McuShell_CMD_STATUS)==0 || McuUtility_strcmp((const char*)cmd, "QuadCounter status")==0) {
    McuShell_SendStatusStr((const unsigned char*)"QuadCounter", (const unsigned char*)"Quadrature counter status", io->stdOut);
    McuShell_SendStatusStr((const unsigned char*)"  pos left", (const unsigned char*)"", io->stdOut);
    McuShell_SendNum32s((int32_t)QuadCounter_currPos_Left, io->stdOut);
    McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);

    McuShell_SendStatusStr((const unsigned char*)"  error left", (const unsigned char*)"", io->stdOut);
    McuShell_SendNum32s((int32_t)QuadCounter_errorsLeft, io->stdOut);
    McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);

    McuShell_SendStatusStr((const unsigned char*)"  left C1 C2", (const unsigned char*)"", io->stdOut);
    if (QuadCounter_GET_C1_PIN_LEFT()) {
      McuShell_SendStr((const unsigned char*)"1 ", io->stdOut);
    } else {
      McuShell_SendStr((const unsigned char*)"0 ", io->stdOut);
    }
    if (QuadCounter_GET_C2_PIN_LEFT()) {
      McuShell_SendStr((const unsigned char*)"1\r\n", io->stdOut);
    } else {
      McuShell_SendStr((const unsigned char*)"0\r\n", io->stdOut);
    }
    McuShell_SendStatusStr((const unsigned char*)"  pos right", (const unsigned char*)"", io->stdOut);
    McuShell_SendNum32s((int32_t)QuadCounter_currPos_Right, io->stdOut);
    McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);

    McuShell_SendStatusStr((const unsigned char*)"  error right", (const unsigned char*)"", io->stdOut);
    McuShell_SendNum32s((int32_t)QuadCounter_errorsRight, io->stdOut);
    McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);

    McuShell_SendStatusStr((const unsigned char*)"  right C1 C2", (const unsigned char*)"", io->stdOut);
    if (QuadCounter_GET_C1_PIN_RIGHT()) {
      McuShell_SendStr((const unsigned char*)"1 ", io->stdOut);
    } else {
      McuShell_SendStr((const unsigned char*)"0 ", io->stdOut);
    }
    if (QuadCounter_GET_C2_PIN_RIGHT()) {
      McuShell_SendStr((const unsigned char*)"1\r\n", io->stdOut);
    } else {
      McuShell_SendStr((const unsigned char*)"0\r\n", io->stdOut);
    }
    *handled = TRUE;
  } else if (McuUtility_strcmp((const char*)cmd, "QuadCounter reset")==0) {
    QuadCounter_SetPosLeft(0);
    QuadCounter_SetPosRight(0);
    *handled = TRUE;
  }
  return res;
}

/*
** ===================================================================
**     Method      :  Deinit (component QuadCounter)
**
**     Description :
**         Module de-initialization method
**     Parameters  : None
**     Returns     : Nothing
** ===================================================================
*/
void QuadCounter_Deinit(void)
{
  C1_Left = McuGPIO_DeinitGPIO(C1_Left);
  C2_Left = McuGPIO_DeinitGPIO(C2_Left);
  C1_Right = McuGPIO_DeinitGPIO(C1_Right);
  C2_Right = McuGPIO_DeinitGPIO(C2_Right);
}

void QuadCounter_EnablePullups(void) {
  McuGPIO_SetPullResistor(C1_Left, McuGPIO_PULL_UP);
  McuGPIO_SetPullResistor(C1_Right, McuGPIO_PULL_UP);
  McuGPIO_SetPullResistor(C2_Left, McuGPIO_PULL_UP);
  McuGPIO_SetPullResistor(C2_Right, McuGPIO_PULL_UP);
}

static void QuadCounter_PinInit(void) {
  McuGPIO_Config_t gpioConfig;

  /*
   * Left: PTC16, PTC17
   * Right: PTC10, PTC11
   */
  McuGPIO_GetDefaultConfig(&gpioConfig);
  gpioConfig.hw.gpio = GPIOC;
  gpioConfig.hw.port = PORTC;
  gpioConfig.hw.pin = 16;
  gpioConfig.isInput = true;
  C1_Left = McuGPIO_InitGPIO(&gpioConfig);
  gpioConfig.hw.pin = 17;
  C2_Left = McuGPIO_InitGPIO(&gpioConfig);
  gpioConfig.hw.pin = 10;
  C1_Right = McuGPIO_InitGPIO(&gpioConfig);
  gpioConfig.hw.pin = 11;
  C2_Right = McuGPIO_InitGPIO(&gpioConfig);
}

/*
** ===================================================================
**     Method      :  Init (component QuadCounter)
**
**     Description :
**         Module initialization method
**     Parameters  : None
**     Returns     : Nothing
** ===================================================================
*/
void QuadCounter_Init(void)
{
  QuadCounter_PinInit();
  QuadCounter_currPos_Left = 0;
  QuadCounter_currPos_Right = 0;
  QuadCounter_last_quadrature_value_left = QuadCounter_GET_C1_C2_PINS_LEFT();
  QuadCounter_last_quadrature_value_right = QuadCounter_GET_C1_C2_PINS_RIGHT();
#if QuadCounter_SWAP_PINS_AT_RUNTIME
  QuadCounter_swappedPins_Left = FALSE;
  QuadCounter_swappedPins_Right = FALSE;
#endif
}


/* END QuadCounter. */

/*!
** @}
*/
