/**
 * \file
 * \brief Motor driver implementation.
 * \author Erich Styger, erich.styger@hslu.ch
 * \license SPDX-License-Identifier: BSD-3-Clause
 * This module a driver for up to two small DC motors.
 */

#include "platform.h"
#if PL_CONFIG_USE_MOTORS
#include "Motor.h"
#include "McuGPIO.h"
#include "Timer.h"
#include "McuUtility.h"
#if PL_HAS_MOTOR_BRAKE
  #include "BrakeL.h"
  #include "BrakeR.h"
#endif
#if PL_HAS_MOTOR_CURRENT_SENSE
  #include "SNS0.h"
#endif
#if PL_HAS_MOTOR_INAB
  #include "DIRRB.h"
  #include "DIRLB.h"
#endif

static MOT_MotorDevice motorL, motorR;
static bool isMotorOn = TRUE;
static McuGPIO_Handle_t MOT_DirL, MOT_DirR;

MOT_MotorDevice *MOT_GetMotorHandle(MOT_MotorSide side) {
  if (side==MOT_MOTOR_LEFT) {
    return &motorL;
  } else {
    return &motorR;
  }
}

#if PL_HAS_MOTOR_BRAKE
void MOT_SetBrake(MOT_MotorDevice *motor, bool on) {
  if (on && !motor->brake) {
    motor->BrakePutVal(1);
    motor->brake = TRUE;
  } else if (!on && motor->brake) {
    motor->BrakePutVal(0);
    motor->brake = FALSE;
  }
}

bool MOT_GetBrake(MOT_MotorDevice *motor) {
  return motor->brake;
}
#endif

#if PL_HAS_MOTOR_CURRENT_SENSE
void MOT_MeasureCurrent(MOT_MotorDevice *motorA, MOT_MotorDevice *motorB) {
  #define SAMPLE_GROUP_SIZE 1U
  SNS0_TResultData MeasuredValues[SAMPLE_GROUP_SIZE];
  LDD_ADC_TSample SampleGroup[SAMPLE_GROUP_SIZE];

  SampleGroup[0].ChannelIdx = 0U;  /* Create one-sample group */
  (void)SNS0_CreateSampleGroup(motorA->SNSdeviceData, (LDD_ADC_TSample *)SampleGroup, SAMPLE_GROUP_SIZE);  /* Set created sample group */
  (void)SNS0_StartSingleMeasurement(motorA->SNSdeviceData);           /* Start continuous measurement */
  while (!SNS0_GetMeasurementCompleteStatus(motorA->SNSdeviceData)) {}; /* Wait for conversion completeness */
  (void)SNS0_GetMeasuredValues(motorA->SNSdeviceData, (LDD_TData *)MeasuredValues);  /* Read measured values */
  motorA->currentValue = MeasuredValues[0];

  SampleGroup[0].ChannelIdx = 1U;  /* Create one-sample group */
  (void)SNS0_CreateSampleGroup(motorB->SNSdeviceData, (LDD_ADC_TSample *)SampleGroup, SAMPLE_GROUP_SIZE);  /* Set created sample group */
  (void)SNS0_StartSingleMeasurement(motorB->SNSdeviceData);           /* Start continuous measurement */
  while (!SNS0_GetMeasurementCompleteStatus(motorB->SNSdeviceData)) {}; /* Wait for conversion completeness */
  (void)SNS0_GetMeasuredValues(motorB->SNSdeviceData, (LDD_TData *)MeasuredValues);  /* Read measured values */
  motorB->currentValue = MeasuredValues[0];
}

static void WriteCurrent(MOT_MotorDevice *motor, const McuShell_StdIOType *io) {
  unsigned char buf[26];
  uint16_t val;

  buf[0] = '\0';
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", current val 0x");
  McuUtility_strcatNum16Hex(buf, sizeof(buf), motor->currentValue);
  McuShell_SendStr(buf, io->stdOut);

  val = motor->currentValue/(0xFFFF/3300); /* 3300 mV full scale */
  buf[0] = '\0';
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", mV ");
  McuUtility_strcatNum16u(buf, sizeof(buf), val);
  McuShell_SendStr(buf, io->stdOut);

  val = motor->currentValue/(0xFFFF/2000); /* 2000 mA full scale */
  buf[0] = '\0';
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", mA ");
  McuUtility_strcatNum16u(buf, sizeof(buf), val);
  McuShell_SendStr(buf, io->stdOut);
}
#endif

static void PWMLSetRatio16(uint16_t ratio) {
  /* lower ratio is higher speed */
  ratio = 0xffff-ratio; /* because already inverted */
  ratio = ratio/(0xffff/100); /* scale to 0...100 */
  TMR_SetLeftMotorPWMDutyPercent(ratio);
}

static void PWMRSetRatio16(uint16_t ratio) {
  ratio = 0xffff-ratio; /* because already inverted */
  ratio = ratio/(0xffff/100); /* scale to 0...100 */
  TMR_SetRightMotorPWMDutyPercent(ratio);
}

static void DirLPutVal(bool val) {
  McuGPIO_SetValue(MOT_DirL, val);
}

static void DirRPutVal(bool val) {
  McuGPIO_SetValue(MOT_DirR, val);
}

#if PL_HAS_MOTOR_BRAKE
static void BrakeLPutVal(bool val) {
  BrakeL_PutVal(val);
}

static void BrakeRPutVal(bool val) {
  BrakeR_PutVal(val);
}
#endif /* PL_HAS_MOTOR_BRAKE */

void MOT_SetVal(MOT_MotorDevice *motor, uint16_t val) {
  if (isMotorOn) {
#if PL_HAS_MOTOR_BRAKE
    if (val==0xffff) { /* stop */
      motor->currPWMvalue = 0;
      motor->SetRatio16(0); /* for max brake */
      MOT_SetBrake(motor, TRUE);
    } else {
      motor->currPWMvalue = val;
      MOT_SetBrake(motor, FALSE);
      motor->SetRatio16(val);
    }
#else
    motor->currPWMvalue = val;
    (void)motor->SetRatio16(val);
#endif
  } else { /* have motor stopped */
    motor->currPWMvalue = 0xFFFF;
    (void)motor->SetRatio16(0xFFFF);
  }
}

void MOT_OnOff(bool on) {
  isMotorOn = on;
  if (!on) {
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
  }
}

uint16_t MOT_GetVal(MOT_MotorDevice *motor) {
  return motor->currPWMvalue;
}

#if MOTOR_HAS_INVERT
void MOT_Invert(MOT_MotorDevice *motor, bool inverted) {
  motor->inverted = inverted;
}
#endif


void MOT_SetSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent percent) {
  uint32_t val;

  if (percent>100) { /* make sure we are within 0..100 */
    percent = 100;
  } else if (percent<-100) {
    percent = -100;
  }
  motor->currSpeedPercent = percent; /* store value */
  if (percent<0) {
    MOT_SetDirection(motor, MOT_DIR_BACKWARD);
    percent = -percent; /* make it positive */
  } else {
    MOT_SetDirection(motor, MOT_DIR_FORWARD);
  }
  val = ((100-percent)*0xffff)/100; /* H-Bridge is low active! */
  MOT_SetVal(motor, (uint16_t)val);
}

void MOT_UpdatePercent(MOT_MotorDevice *motor, MOT_Direction dir) {
  motor->currSpeedPercent = ((0xffff-motor->currPWMvalue)*100)/0xffff;
  if (dir==MOT_DIR_BACKWARD) {
    motor->currSpeedPercent = -motor->currSpeedPercent;
  }
}

MOT_SpeedPercent MOT_GetSpeedPercent(MOT_MotorDevice *motor) {
  return motor->currSpeedPercent;
}

void MOT_ChangeSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent relPercent) {
  relPercent += motor->currSpeedPercent; /* make absolute number */
  if (relPercent>100) { /* check for overflow */
    relPercent = 100;
  } else if (relPercent<-100) { /* and underflow */
    relPercent = -100;
  }
  MOT_SetSpeedPercent(motor, relPercent);  /* set speed. This will care about the direction too */
}

void MOT_SetDirection(MOT_MotorDevice *motor, MOT_Direction dir) {
  if (dir==MOT_DIR_FORWARD ) {
#if MOTOR_HAS_INVERT
    motor->DirPutVal(motor->inverted?0:1);
#else
    motor->DirPutVal(1);
#endif
    if (motor->currSpeedPercent<0) {
      motor->currSpeedPercent = -motor->currSpeedPercent;
    }
  } else if (dir==MOT_DIR_BACKWARD) {
#if MOTOR_HAS_INVERT
    motor->DirPutVal(motor->inverted?1:0);
#else
    motor->DirPutVal(0);
#endif
    if (motor->currSpeedPercent>0) {
      motor->currSpeedPercent = -motor->currSpeedPercent;
    }
  }
}

MOT_Direction MOT_GetDirection(MOT_MotorDevice *motor) {
  if (motor->currSpeedPercent<0) {
    return MOT_DIR_BACKWARD;
  } else {
    return MOT_DIR_FORWARD;
  }
}

#if PL_CONFIG_USE_SHELL
static void MOT_PrintHelp(const McuShell_StdIOType *io) {
  McuShell_SendHelpStr((unsigned char*)"motor", (unsigned char*)"Group of motor commands\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  help|status", (unsigned char*)"Shows motor help or status\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  on|off", (unsigned char*)"Enables or disables motor\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  (L|R) forward|backward", (unsigned char*)"Change motor direction\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  (L|R) duty <number>", (unsigned char*)"Change motor PWM (-100..+100)%\r\n", io->stdOut);
#if PL_HAS_MOTOR_BRAKE
  McuShell_SendHelpStr((unsigned char*)"  (L|R) brake (on|off)", (unsigned char*)"Enable/disable brake for motor\r\n", io->stdOut);
#endif
}

static void MOT_PrintStatus(const McuShell_StdIOType *io) {
  unsigned char buf[32];

  McuShell_SendStatusStr((unsigned char*)"Motor", (unsigned char*)"Motor status\r\n", io->stdOut);
#ifdef MOTOR_HAS_INVERT
  McuShell_SendStatusStr((unsigned char*)"  inverted L", MOT_GetMotorHandle(MOT_MOTOR_LEFT)->inverted?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
  McuShell_SendStatusStr((unsigned char*)"  inverted R", MOT_GetMotorHandle(MOT_MOTOR_RIGHT)->inverted?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
#endif
  McuShell_SendStatusStr((unsigned char*)"  on/off", isMotorOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
  McuShell_SendStatusStr((unsigned char*)"  motor L", (unsigned char*)"", io->stdOut);
  buf[0] = '\0';
  McuUtility_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorL.currSpeedPercent, ' ', 4);
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"% 0x");
  McuUtility_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorL));
  McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorL)==MOT_DIR_FORWARD?", fw":", bw"));
#if PL_HAS_MOTOR_BRAKE
  McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetBrake(&motorL)?", brake on":", brake off"));
#endif
  McuShell_SendStr(buf, io->stdOut);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);

  McuShell_SendStatusStr((unsigned char*)"  motor R", (unsigned char*)"", io->stdOut);
  buf[0] = '\0';
  McuUtility_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorR.currSpeedPercent, ' ', 4);
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"% 0x");
  McuUtility_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorR));
  McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorR)==MOT_DIR_FORWARD?", fw":", bw"));
#if PL_HAS_MOTOR_BRAKE
  McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetBrake(&motorR)?", brake on":", brake off"));
#endif
  McuShell_SendStr(buf, io->stdOut);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);

#if PL_HAS_MOTOR_CURRENT_SENSE
  WriteCurrent(&motorA, io);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  WriteCurrent(&motorB, io);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
#endif
}

uint8_t MOT_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
  uint8_t res = ERR_OK;
  int32_t val;
  const unsigned char *p;

  if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"motor help")==0) {
    MOT_PrintHelp(io);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"motor status")==0) {
    MOT_PrintStatus(io);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor L forward")==0) {
    MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor R forward")==0) {
    MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor L backward")==0) {
    MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor R backward")==0) {
    MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
    *handled = TRUE;
  } else if (McuUtility_strncmp((char*)cmd, (char*)"motor L duty ", sizeof("motor L duty ")-1)==0) {
    if (!isMotorOn) {
      McuShell_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
      res = ERR_FAILED;
    } else {
      p = cmd+sizeof("motor L duty");
      if (McuUtility_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
        MOT_SetSpeedPercent(&motorL, (MOT_SpeedPercent)val);
        *handled = TRUE;
      } else {
        McuShell_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
        res = ERR_FAILED;
      }
    }
  } else if (McuUtility_strncmp((char*)cmd, (char*)"motor R duty ", sizeof("motor R duty ")-1)==0) {
    if (!isMotorOn) {
      McuShell_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
      res = ERR_FAILED;
    } else {
      p = cmd+sizeof("motor R duty");
      if (McuUtility_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
        MOT_SetSpeedPercent(&motorR, (MOT_SpeedPercent)val);
        *handled = TRUE;
      } else {
        McuShell_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
        res = ERR_FAILED;
      }
    }
  } else if (McuUtility_strncmp((char*)cmd, (char*)"motor on", sizeof("motor on")-1)==0) {
    isMotorOn = TRUE;
    *handled = TRUE;
  } else if (McuUtility_strncmp((char*)cmd, (char*)"motor off", sizeof("motor off")-1)==0) {
    MOT_SetSpeedPercent(&motorL, 0);
    MOT_SetSpeedPercent(&motorR, 0);
    isMotorOn = FALSE;
    *handled = TRUE;
#if PL_HAS_MOTOR_BRAKE
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor L brake on")==0) {
    MOT_SetBrake(&motorL, TRUE);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor L brake off")==0) {
    MOT_SetBrake(&motorL, FALSE);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor R brake on")==0) {
    MOT_SetBrake(&motorR, TRUE);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"motor R brake off")==0) {
    MOT_SetBrake(&motorR, FALSE);
    *handled = TRUE;
#endif
  }
  return res;
}
#endif /* PL_CONFIG_USE_SHELL */

void MOT_Deinit(void) {
  MOT_DirL = McuGPIO_DeinitGPIO(MOT_DirL);
  MOT_DirR = McuGPIO_DeinitGPIO(MOT_DirR);
}

static void MOT_DirPinInit(void) {
  McuGPIO_Config_t gpioConfig;

  /* DIRL: PTC9
   * DIRR: PTC8
   */
  McuGPIO_GetDefaultConfig(&gpioConfig);
  gpioConfig.hw.gpio = GPIOC;
  gpioConfig.hw.port = PORTC;
  gpioConfig.hw.pin = 9;
  gpioConfig.isInput = false;
  gpioConfig.isHighOnInit = true;
  MOT_DirL = McuGPIO_InitGPIO(&gpioConfig);
  gpioConfig.hw.pin = 8;
  MOT_DirR = McuGPIO_InitGPIO(&gpioConfig);
}

void MOT_Init(void) {
  MOT_DirPinInit();
#if PL_HAS_MOTOR_BRAKE
  motorL.BrakePutVal = BrakeLPutVal;
  motorR.BrakePutVal = BrakeRPutVal;
#endif
#if MOTOR_HAS_INVERT
  motorL.inverted = FALSE;
  motorR.inverted = FALSE;
#endif
  motorL.DirPutVal = DirLPutVal;
  motorR.DirPutVal = DirRPutVal;
  motorL.SetRatio16 = PWMLSetRatio16;
  motorR.SetRatio16 = PWMRSetRatio16;
  MOT_SetSpeedPercent(&motorL, 0);
  MOT_SetSpeedPercent(&motorR, 0);
  TMR_MotorPWMStart();
}
#endif /* PL_CONFIG_USE_MOTORS */