/**
 * \file
 * \brief Implements line following of the robot
 * \author Erich Styger, erich.styger@hslu.ch
* \license SPDX-License-Identifier: BSD-3-Clause
 * This is the implementation of the line following.
 */

#include "platform.h"
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
#include "LineFollow.h"
#include "McuRTOS.h"
#include "McuShell.h"
#include "Shell.h"
#include "Motor.h"
#include "Reflectance.h"
#include "McuWait.h"
#include "McuUtility.h"
#include "Pid.h"
#include "Turn.h"
#include "Maze.h"
#include "Shell.h"
#if PL_CONFIG_USE_BUZZER
  #include "Buzzer.h"
#endif
#if PL_CONFIG_USE_DRIVE
  #include "Drive.h"
#endif

#define LINE_FOLLOW_FW      (1)   /* test setting to do forward line following */
#define LINE_DO_DEBUG_WAIT  (0)   /* if set to one, it will add some debug wait to stop the engines */
#if LINE_DO_DEBUG_WAIT
  #define LINE_DO_DEBUG_WAIT_MS  (500) /* waiting time in milliseconds */
#endif

#if LINE_DO_DEBUG_WAIT /* debug mode */
static void DebugStopAndWait(void) {
  static volatile bool waitHere = FALSE;

  TURN_Turn(TURN_STOP, NULL);
  vTaskDelay(LINE_DO_DEBUG_WAIT_MS);
  waitHere = TRUE;
}
#endif

#define PL_TURN_ON_FINISH  (0) /* temporary only */

typedef enum {
  LINE_SPEED_LOW,
  LINE_SPEED_MEDIUM,
  LINE_SPEED_HIGH
} LINE_SpeedMode;

static LINE_SpeedMode lineFollowSpeed = LINE_SPEED_LOW; /* set value in LineTask()! */

typedef enum {
  STATE_IDLE,              /* idle, not doing anything */
  STATE_FOLLOW_SEGMENT,    /* line following segment, going forward */
#if PL_CONFIG_APP_LINE_MAZE
  STATE_TURN,              /* reached an intersection, turning around */
  STATE_FINISHED,          /* reached finish area */
#endif
  STATE_STOP               /* stop the engines */
} StateType;

/* task notification bits */
#define LF_START_FOLLOWING  (1<<0)  /* start line following */
#define LF_STOP_FOLLOWING   (1<<1)  /* stop line following */
static TaskHandle_t LFTaskHandle;

static volatile StateType LF_currState = STATE_IDLE;
#if PL_CONFIG_APP_LINE_MAZE
static uint8_t LF_solvedIdx = 0; /*  index to iterate through the solution, zero is the solution start index */
#endif

void LF_StartFollowing(void) {
  (void)xTaskNotify(LFTaskHandle, LF_START_FOLLOWING, eSetBits);
}

void LF_StopFollowing(void) {
  (void)xTaskNotify(LFTaskHandle, LF_STOP_FOLLOWING, eSetBits);
}

void LF_StartStopFollowing(void) {
  if (LF_IsFollowing()) {
    (void)xTaskNotify(LFTaskHandle, LF_STOP_FOLLOWING, eSetBits);
  } else {
    (void)xTaskNotify(LFTaskHandle, LF_START_FOLLOWING, eSetBits);
  }
}

/*!
 * \brief follows a line segment.
 * \return Returns TRUE if still on line segment
 */
bool LF_FollowSegment(REF_LineKindMode mode, bool forward) {
  uint16_t currLine;
  bool onLine;
  REF_LineKind currLineKind;

  REF_GetLineValue(&onLine);
  currLineKind = REF_GetLineKind(mode);
  if (currLineKind!=REF_LINE_STRAIGHT) {
    SHELL_SendString((unsigned char*)"LF_FollowSegment: ");
    SHELL_SendString(REF_LineKindStr(currLineKind));
    SHELL_SendString((unsigned char*)"\r\n");
  }
#if PL_CONFIG_APP_LINE_FOLLOWING
  if (currLineKind==REF_LINE_STRAIGHT || currLineKind!=REF_LINE_NONE) { /* as long we have something: follow line */
#elif PL_CONFIG_APP_LINE_MAZE
  if (currLineKind==REF_LINE_STRAIGHT) { /* as long we have a line: follow line */
#endif
    currLine = REF_GetLineValue(&onLine);
    PID_Line(currLine, REF_MIDDLE_LINE_VALUE, REF_LineWidth(), forward); /* continue move along the line */
    return TRUE;
  } else { /* stop motors */
    SHELL_SendString((unsigned char*)"LF_FollowSegment stop: ");
    SHELL_SendString(REF_LineKindStr(currLineKind));
    SHELL_SendString((unsigned char*)"\r\n");
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
    vTaskDelay(pdMS_TO_TICKS(20)); /* give time for PWM to change */
    return FALSE; /* intersection/change of direction or not on line any more */
  }
}

/*!
 * \brief Move from outside onto a line/segment to follow it.
 * \return Returns TRUE if still on line segment
 */
bool LF_MoveOnSegment(bool turningLeft) {
  uint16_t currLine, targetLine;
  int tmp;
  bool onLine;
  REF_LineKind currLineKind;
  uint16_t lineWidth;

  currLine = REF_GetLineValue(&onLine);
  currLineKind = REF_GetLineKind(REF_LINE_KIND_MODE_ALL);
  lineWidth = REF_LineWidth();
  if (lineWidth>2000) {
    if (turningLeft) {
      tmp = REF_MIDDLE_LINE_VALUE+lineWidth;
    } else {
      tmp = REF_MIDDLE_LINE_VALUE-lineWidth;
    }
    if (tmp<0) {
      tmp = 0;
    }
    if (tmp>REF_MAX_LINE_VALUE) {
      tmp = REF_MAX_LINE_VALUE;
    }
    targetLine = tmp;
  } else {
    targetLine = REF_MIDDLE_LINE_VALUE;
  }
  if (currLineKind==REF_LINE_STRAIGHT || currLineKind==REF_LINE_LEFT || currLineKind==REF_LINE_RIGHT || currLineKind==REF_LINE_FULL) {
    PID_Line(currLine, targetLine, REF_LineWidth(), TRUE); /* move along the line */
    return TRUE;
  } else {
    return FALSE; /* intersection/change of direction or not on line any more */
  }
}

bool LF_FollowSegmentLinePos(REF_LineKindMode mode, uint16_t setLinePos) {
  uint16_t currLine;
  bool onLine;
  REF_LineKind currLineKind;

  currLine = REF_GetLineValue(&onLine);
  currLineKind = REF_GetLineKind(mode);
  if (currLineKind==REF_LINE_STRAIGHT) {
    PID_Line(currLine, setLinePos, REF_LineWidth(), TRUE); /* move along the line */
    return TRUE;
  } else {
    return FALSE; /* intersection/change of direction or not on line any more */
  }
}

static void StateMachine(void) {
  for(;;) {
    switch (LF_currState) {
      case STATE_IDLE:
        break;
      case STATE_FOLLOW_SEGMENT:
#if PL_CONFIG_APP_LINE_MAZE
        if (!LF_FollowSegment(REF_LINE_KIND_MODE_MAZE, LINE_FOLLOW_FW)) {
          TURN_Turn(TURN_STOP, NULL);
      #if LINE_DO_DEBUG_WAIT /* debug mode */
          DebugStopAndWait();
      #endif
          LF_currState = STATE_TURN; /* make turn */
          continue; /* process next step immediately */
        }
#else
        {
          REF_LineKind currLineKind;

          if (!LF_FollowSegment(REF_LINE_KIND_MODE_LINE_FOLLOW, LINE_FOLLOW_FW)) {
            SHELL_SendString((unsigned char*)"No segment any more!\r\n");
            currLineKind = REF_GetLineKind(REF_LINE_KIND_MODE_LINE_FOLLOW);
            SHELL_SendString((unsigned char*)"Line: ");
            SHELL_SendString(REF_LineKindStr(currLineKind));
            SHELL_SendString((unsigned char*)"\r\n");

            SHELL_SendString((unsigned char*)"stop!\r\n");
            LF_currState = STATE_STOP; /* stop if we do not have a line any more */
          }
        }
#endif
        break;

  #if PL_CONFIG_APP_LINE_MAZE
      case STATE_TURN:
        if (MAZE_IsSolved()) {
          TURN_Kind turn;

          turn = MAZE_GetSolvedTurn(&LF_solvedIdx);
          if (turn==TURN_STOP) { /* last turn reached */
            TURN_Turn(turn, NULL);
          #if PL_CONFIG_USE_DRIVE
            (void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
          #endif
            LF_currState = STATE_STOP;
            SHELL_SendString((unsigned char *)"MAZE: I'm back to the start of maze!\r\n");
          #if PL_CONFIG_USE_BUZZER
            BUZ_PlayTune(1);
          #endif
            MAZE_ClearSolution(); /* clear solution */
  #if PL_TURN_ON_START
            /* turn the robot on start so it is ready to run the maze again */
          #if PL_CONFIG_USE_QUADRATURE
            TURN_Turn(TURN_LEFT180, NULL);
          #else /* not accurate enough without position sensor */
            TURN_Turn(TURN_LEFT90, NULL); /* do turn in two steps */
            TURN_Turn(TURN_LEFT90, NULL);
          #endif
  #endif
            TURN_Turn(TURN_STOP, NULL);
  #if PL_CONFIG_USE_DRIVE
            (void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
  #endif
            /* now ready to solve maze again */
            SHELL_SendString((unsigned char *)"MAZE: ready to start again!\r\n");
          } else { /* perform turning */
            TURN_Turn(TURN_STEP_LINE_FW_AND_POST_LINE, NULL); /* Step over line */
            TURN_Turn(turn, NULL);
        #if LINE_DO_DEBUG_WAIT /* debug mode */
            DebugStopAndWait();
        #endif
        #if PL_CONFIG_USE_DRIVE
            (void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode, so we can do line following (line following is PWM) */
        #endif
            LF_currState = STATE_FOLLOW_SEGMENT;
            continue; /* process next state immediately */
          }
        } else { /* still evaluating maze */
          bool deadEndGoBw = FALSE;
          bool finished = FALSE;

          if (MAZE_EvaluteTurn(&finished, &deadEndGoBw)==ERR_OK) { /* finished turning */
        #if LINE_DO_DEBUG_WAIT /* debug mode */
            DebugStopAndWait();
        #endif
        #if PL_CONFIG_USE_DRIVE
            (void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
        #endif
            if (finished) {
              LF_currState = STATE_FINISHED;
              MAZE_SetSolved();
              LF_solvedIdx = 0; /* set index to start of solution */
            #if PL_CONFIG_HAS_BUZZER
              BUZ_PlayTune(BUZ_TUNE_MAZE_DESTINATION);
            #endif
        #if PL_TURN_ON_FINISH
              /* turn the robot */
            #if PL_CONFIG_USE_QUADRATURE
              TURN_Turn(TURN_LEFT180, NULL);
            #else /* not accurate enough without position sensor */
              TURN_Turn(TURN_LEFT90, NULL); /* do turn in two steps */
              TURN_Turn(TURN_LEFT90, NULL);
            #endif
        #endif
              //TURN_Turn(TURN_STOP, NULL);
        #if LINE_DO_DEBUG_WAIT /* debug mode */
            DebugStopAndWait();
        #endif
        #if PL_CONFIG_USE_DRIVE
              (void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
        #endif
              /* now ready to do line following */
            } else {
              LF_currState = STATE_FOLLOW_SEGMENT;
            }
          } else { /* error case */
            LF_currState = STATE_STOP;
          }
        }
        break;
  #endif
  #if PL_CONFIG_APP_LINE_MAZE
      case STATE_FINISHED:
        SHELL_SendString((unsigned char *)"MAZE: Found maze destination!\r\n");
  #if PL_CONFIG_USE_BUZZER
        BUZ_PlayTune(BUZ_TUNE_MAZE_DESTINATION);
  #endif
        //SHELL_SendString((unsigned char *)"MAZE: Going back to start...\r\n");
        //LF_currState = STATE_FOLLOW_SEGMENT; /* go back to start */
        MAZE_ClearSolution(); /* clear solution */
        LF_currState = STATE_STOP;
        break;
  #endif
      case STATE_STOP:
        SHELL_SendString((unsigned char *)"Stopped!\r\n");
        (void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
        TURN_Turn(TURN_STOP, NULL);
        LF_currState = STATE_IDLE;
        break;
    } /* switch */
    break; /* get out of for loop */
  } /* for */
}

bool LF_IsFollowing(void) {
  return LF_currState!=STATE_IDLE;
}

static const unsigned char *LINE_GetSpeedModeString(LINE_SpeedMode mode) {
  switch(mode) {
  case LINE_SPEED_LOW: return (const unsigned char*)"LOW";
  case LINE_SPEED_MEDIUM: return (const unsigned char*)"MEDIUM";
  case LINE_SPEED_HIGH: return (const unsigned char*)"HIGH";
  default: break;
  }
  return (const unsigned char*)"UNKNOWN";
}

static uint8_t LINE_SetSpeed(LINE_SpeedMode speed) {
  uint8_t res;
  PID_Config *lineFwPid, *posLeftPid, *posRightPid;

  res = PID_GetPIDConfig(PID_CONFIG_LINE_FW, &lineFwPid);
  if (res!=ERR_OK || lineFwPid==NULL) {
    return ERR_FAILED;
  }
  res = PID_GetPIDConfig(PID_CONFIG_POS_LEFT, &posLeftPid);
  if (res!=ERR_OK || posLeftPid==NULL) {
    return ERR_FAILED;
  }
  res = PID_GetPIDConfig(PID_CONFIG_POS_RIGHT, &posRightPid);
  if (res!=ERR_OK || posRightPid==NULL) {
    return ERR_FAILED;
  }
  switch(speed) {
    case LINE_SPEED_LOW:
      lineFwPid->maxSpeedPercent = 15;
      lineFwPid->pFactor100 = 4000;
      lineFwPid->dFactor100 = 500;
      TURN_SetSteps(680, 170, 180);
      posLeftPid->maxSpeedPercent = 30;
      posLeftPid->pFactor100 = 2000;
      posRightPid->pFactor100 = posLeftPid->pFactor100;
      posRightPid->maxSpeedPercent = posLeftPid->maxSpeedPercent;
      break;
    case LINE_SPEED_MEDIUM:
      lineFwPid->maxSpeedPercent = 30; /* 15 => 30 */
      lineFwPid->pFactor100 = 5500;
      lineFwPid->iAntiWindup = 100000;
      TURN_SetSteps(700, 190, 100);
      posLeftPid->maxSpeedPercent = 80;
      posLeftPid->pFactor100 = 2000;
      posRightPid->pFactor100 = posLeftPid->pFactor100;
      posRightPid->maxSpeedPercent = posLeftPid->maxSpeedPercent;
      break;
    case LINE_SPEED_HIGH: /* to be verified */
      lineFwPid->maxSpeedPercent = 40;
      lineFwPid->pFactor100 = 2000;
      lineFwPid->iAntiWindup = 30000;
      TURN_SetSteps(700, 150, 80);
      posLeftPid->maxSpeedPercent = 100;
      posLeftPid->pFactor100 = 2000;
      posRightPid->pFactor100 = posLeftPid->pFactor100;
      posRightPid->maxSpeedPercent = posLeftPid->maxSpeedPercent;
      break;
    default:
      return ERR_FAILED;
  } /* switch */
  lineFollowSpeed = speed;
  SHELL_SendString((unsigned char*)"Changed parameters for speed!\n");
  return ERR_OK;
}

static void LineTask (void *pvParameters) {
  uint32_t notifcationValue;
  BaseType_t notified;

  (void)pvParameters; /* not used */
#if PL_SLOWER_SPEED
  (void)LINE_SetSpeed(LINE_SPEED_LOW);
#else /* check! */
  (void)LINE_SetSpeed(LINE_SPEED_MEDIUM);
#endif
  for(;;) {
    notified = xTaskNotifyWait(0UL, LF_START_FOLLOWING|LF_STOP_FOLLOWING, &notifcationValue, 1); /* check flags, need to wait for one tick */
    if (notified==pdTRUE) { /* received notification */
      if (notifcationValue&LF_START_FOLLOWING) {
        if (REF_CanUseSensor()) {
          PID_Start();
          LF_currState = STATE_FOLLOW_SEGMENT;
      #if PL_CONFIG_USE_DRIVE
          (void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
      #endif
        } else {
          SHELL_SendString((unsigned char*)"Sensors not ready!\r\n");
      #if PL_CONFIG_USE_BUZZER
          (void)BUZ_Beep(500, 500);
      #endif
        }
      }
      if (notifcationValue&LF_STOP_FOLLOWING) {
        LF_currState = STATE_STOP;
      }
    }
    StateMachine();
    if (LF_IsFollowing()) {
      vTaskDelay(5/portTICK_PERIOD_MS);
    } else {
      vTaskDelay(100/portTICK_PERIOD_MS);
    }
  }
}

static void LF_PrintHelp(const McuShell_StdIOType *io) {
  McuShell_SendHelpStr((unsigned char*)"line", (unsigned char*)"Group of line following commands\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  help|status", (unsigned char*)"Shows line help or status\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  speed <mode>", (unsigned char*)"Line following speed, either low, medium or high\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  start|stop", (unsigned char*)"Starts or stops line following\r\n", io->stdOut);
}

static void LF_PrintStatus(const McuShell_StdIOType *io) {
  uint8_t buf[32];

  McuShell_SendStatusStr((unsigned char*)"line follow", (unsigned char*)"Line following status\r\n", io->stdOut);
  switch (LF_currState) {
    case STATE_IDLE: 
      McuShell_SendStatusStr((unsigned char*)"  state", (unsigned char*)"IDLE\r\n", io->stdOut);
      break;
    case STATE_FOLLOW_SEGMENT: 
      McuShell_SendStatusStr((unsigned char*)"  state", (unsigned char*)"FOLLOW_SEGMENT\r\n", io->stdOut);
      break;
    case STATE_STOP: 
      McuShell_SendStatusStr((unsigned char*)"  state", (unsigned char*)"STOP\r\n", io->stdOut);
      break;
#if PL_CONFIG_APP_LINE_MAZE
    case STATE_TURN: 
      McuShell_SendStatusStr((unsigned char*)"  state", (unsigned char*)"TURN\r\n", io->stdOut);
      break;
    case STATE_FINISHED: 
      McuShell_SendStatusStr((unsigned char*)"  state", (unsigned char*)"FINISHED\r\n", io->stdOut);
      break;
#endif
    default: 
      McuShell_SendStatusStr((unsigned char*)"  state", (unsigned char*)"UNKNOWN\r\n", io->stdOut);
      break;
  } /* switch */
  McuUtility_strcpy(buf, sizeof(buf), LINE_GetSpeedModeString(lineFollowSpeed));
  McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  McuShell_SendStatusStr((unsigned char*)"  speed", buf, io->stdOut);
}

uint8_t LF_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
  uint8_t res = ERR_OK;

  if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"line help")==0) {
    LF_PrintHelp(io);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"line status")==0) {
    LF_PrintStatus(io);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"line start")==0) {
    LF_StartFollowing();
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"line stop")==0) {
    LF_StopFollowing();
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"line speed low")==0) {
    (void)LINE_SetSpeed(LINE_SPEED_LOW);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"line speed medium")==0) {
    (void)LINE_SetSpeed(LINE_SPEED_MEDIUM);
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"line speed high")==0) {
    (void)LINE_SetSpeed(LINE_SPEED_HIGH);
    *handled = TRUE;
  }
  return res;
}

void LF_Deinit(void) {
  /* nothing needed */
}

void LF_Init(void) {
  LF_currState = STATE_IDLE;
  if (xTaskCreate(LineTask, "Line", 600/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+2, &LFTaskHandle) != pdPASS) {
    for(;;){} /* error */
  }
}
#endif /* PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE */