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

#include "platform.h"
#include "Identify.h"
#include "Motor.h"
#include "QuadCounter.h"
#include "buttons.h"
#include <stdbool.h>

typedef enum {
  ROBOT_HW_V1,
  ROBOT_HW_V2
} RobotHW_Version;

typedef struct {
  ID_Robot_e id;
  RobotHW_Version version;
  bool invertDirLeft, invertDirRight;
  bool swapQuadLeft, swapQuadRight;
} RobotHW_Config;

static const RobotHW_Config RobotHWConfigTable[] =
{ /*! \todo ADIS: The following table needs to be completed and verified for all robots */
  {.id=ID_ROBOT_E0,  .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
  {.id=ID_ROBOT_E1,  .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
  {.id=ID_ROBOT_E2,  .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E3,  .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=FALSE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified*/
  {.id=ID_ROBOT_E4,  .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E5,  .version=ROBOT_HW_V2, .invertDirLeft=TRUE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, 
  {.id=ID_ROBOT_E6,  .version=ROBOT_HW_V1, .invertDirLeft=FALSE,  .invertDirRight=FALSE,  .swapQuadLeft=TRUE,.swapQuadRight=TRUE }, /* verified */
  {.id=ID_ROBOT_E7,  .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E8,  .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E9,  .version=ROBOT_HW_V1, .invertDirLeft=TRUE,  .invertDirRight=FALSE,  .swapQuadLeft=TRUE,.swapQuadRight=FALSE }, /* verified */
  {.id=ID_ROBOT_E10, .version=ROBOT_HW_V1, .invertDirLeft=TRUE,  .invertDirRight=FALSE,  .swapQuadLeft=TRUE,.swapQuadRight=FALSE }, /* verified */
  {.id=ID_ROBOT_E12, .version=ROBOT_HW_V1, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E13, .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E14, .version=ROBOT_HW_V2, .invertDirLeft=true,   .invertDirRight=false, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E17, .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E18, .version=ROBOT_HW_V1, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_E27, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },

  {.id=ID_ROBOT_L0,  .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_L1,  .version=ROBOT_HW_V1, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_L3,  .version=ROBOT_HW_V1, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_L17, .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_L20, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },   /* verified */

  {.id=ID_ROBOT_R0,  .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R8,  .version=ROBOT_HW_V1, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R9,  .version=ROBOT_HW_V1, .invertDirLeft=FALSE,   .invertDirRight=FALSE,  .swapQuadLeft=TRUE,.swapQuadRight=FALSE },	/* verified */
  {.id=ID_ROBOT_R23, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R27, .version=ROBOT_HW_V2, .invertDirLeft=FALSE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R28, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R29, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R32, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R33, .version=ROBOT_HW_V2, .invertDirLeft=FALSE,  .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },   /* verified */
  {.id=ID_ROBOT_R34, .version=ROBOT_HW_V2, .invertDirLeft=FALSE,   .invertDirRight=FALSE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
  {.id=ID_ROBOT_R36, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
  {.id=ID_ROBOT_R37, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=FALSE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
  {.id=ID_ROBOT_R44, .version=ROBOT_HW_V2, .invertDirLeft=TRUE,   .invertDirRight=TRUE,  .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
};

static const RobotHW_Config *GetRobotHWDesc(ID_Robot_e id) {
  unsigned int i;

  for(i=0; i<sizeof(RobotHWConfigTable)/sizeof(RobotHWConfigTable[0]); i++) {
    if (RobotHWConfigTable[i].id == id) {
      return &RobotHWConfigTable[i]; /* found it */
    }
  }
  return NULL;
}

void ADOPT_AdoptToHardware(void) {
#if PL_CONFIG_USE_IDENTIFY
#if PL_CONFIG_USE_MOTORS
  ID_Robot_e robot;
  const RobotHW_Config *config;

  robot = ID_WhichDevice();
  if (robot!=ID_ROBOT_NONE) {
    config = GetRobotHWDesc(robot);
    if (config!=NULL) {
      if (config->invertDirLeft) {
        MOT_Invert(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TRUE); /* invert motor */
      }
      if (config->invertDirRight) {
        MOT_Invert(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TRUE); /* invert motor */
      }
      if (config->swapQuadLeft) {
        QuadCounter_SwapPinsLeft(true);
      }
      if (config->swapQuadRight) {
        QuadCounter_SwapPinsRight(true);
      }
    }
    if (config->version==ROBOT_HW_V2) {
      BTN_EnablePullup(); /* enable internal pull-up for push button */
      QuadCounter_EnablePullups(); /* pull-ups for Quadrature Encoder Pins */
    }
  }
#endif
#endif
}