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

#include "platform.h"
#include "Application.h"
#include "McuRTOS.h"
#include "McuUtility.h"
#if PL_HAS_ULTRASONIC
  #include "Ultrasonic.h"
#endif
#if PL_CONFIG_USE_LEDS
  #include "Leds.h"
  #include "McuLED.h"
#endif
#include "McuWait.h"
#include "McuUtility.h"
#include "Shell.h"
#include "Motor.h"
#if PL_CONFIG_USE_LINE_SENSOR
  #include "Reflectance.h"
#endif
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
  #include "LineFollow.h"
#endif
#include "Motor.h"
#if PL_HAS_TSS
  #include "TSS1.h"
#endif
#if PL_CONFIG_USE_BUZZER
  #include "Buzzer.h"
#endif
#include "Turn.h"
#if PL_HAS_USER_BUTTON
  #include "SW1.h"
#endif
#if PL_CONFIG_USE_LEDS
  #include "leds.h"
  #include "McuLED.h"
#endif
#if PL_CONFIG_APP_LINE_MAZE
  #include "Maze.h"
#endif
#if PL_HAS_RADIO
  #include "RStack.h"
#endif
#if PL_CONFIG_USE_REMOTE
  #include "Remote.h"
#endif
#if PL_HAS_SHELL_QUEUE
  #include "ShellQueue.h"
#endif
#if PL_HAS_DISTANCE_SENSOR
  #include "Distance.h"
#endif
#if PL_HAS_ACCEL_SENSOR
  #include "Accel.h"
  #include "MMA1.h"
#endif
#if PL_CONFIG_APP_SUMO
  #include "Sumo.h"
#endif
#if PL_CONFIG_USE_PID
  #include "PID.h"
#endif
#if PL_CONFIG_USE_EVENTS
  #include "Event.h"
#endif
#if PL_HAS_TEST
  #include "Test.h"
#endif
#if configUSE_TRACE_HOOKS
  #include "PTRC1.h"
#endif
#if PL_CONFIG_USE_TACHO
  #include "Tacho.h"
#endif
#if PL_HAS_RADIO
  #include "Radio.h"
  #include "RNet_App.h"
#endif
#if PL_HAS_SHELL_TRACE
  #include "ShellTrace.h"
#endif
#if PL_HAS_FREEMASTER
  #include "FMSTR1.h"
#endif
#if PL_CONFIG_USE_DRIVE
  #include "Drive.h"
#endif
#if PL_CONFIG_USE_I2C_SPY
  #include "I2CSPY1.h"
#endif
#if PL_HAS_MCP4728
  #include "MCP4728.h"
#endif
#if PL_CONFIG_HAS_BATTERY_ADC
  #include "Battery.h"
#endif
#if PL_HAS_ROBO_SHIELD
  #include "RoboShield.h"
#endif
#if PL_HAS_MUSIC_SHIELD
  #include "Music.h"
#endif
#if PL_HAS_SPI
  #include "SPI.h"
#endif
#if PL_HAS_LASER
  #include "Laser.h"
#endif
#if PL_CONFIG_USE_QUADRATURE
  #include "QuadCounter.h"
#endif
#if PL_CONFIG_USE_IDENTIFY
  #include "Identify.h"
#endif
#if PL_HAS_MINT
  #include "MintRobot.h"
#endif
#if PL_HAS_MINT_REMOTE
  #include "MintRemote.h"
#endif
#if PL_HAS_MINT_LED
  #include "MintLed.h"
#endif
#if PL_HAS_TOF_SENSOR
  #include "VL6180X.h"
#endif
#if PL_CONFIG_HAS_NVM_CONFIG
  #include "NVM_Config.h"
#endif
#if PL_CONFIG_HAS_LCD
  #include "LCD.h"
  #include "GDisp1.h"
#endif
#include "McuEvents.h"
#if PL_CONFIG_HAS_DEBOUNCE
  #include "KeyDebounce.h"
#endif
#include "McuArmTools.h"
#if PL_HAS_MIDI
  #include "MidiMusic.h"
#endif

typedef enum {
  APP_STATE_STARTUP,
  APP_STATE_INIT,
#if PL_CONFIG_USE_LINE_SENSOR
  APP_STATE_CALIBRATE,
#endif
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
  APP_STATE_FOLLOW_LINE,
#endif
#if PL_APP_FOLLOW_OBSTACLE
  APP_STATE_FOLLOW_OBSTACLE, /* follow obstacle */
#endif
#if PL_DO_MIDI
  APP_STATE_PLAY_MIDI,
#endif
#if PL_CONFIG_APP_SUMO
  APP_STATE_RUN_SUMO, /* Sumo fight */
#endif
  APP_STATE_IDLE
} AppStateType;

static AppStateType appState = APP_STATE_STARTUP;
#if PL_APP_FOLLOW_OBSTACLE
static bool followObstacle = FALSE;
#endif
#if PL_APP_AVOID_OBSTACLE
static bool avoidObstacle = FALSE;
#endif

void APP_DebugPrint(unsigned char *str) {
#if PL_CONFIG_USE_SHELL
  SHELL_SendString(str);
#endif
}

#if PL_CONFIG_USE_SHELL
static unsigned char *AppStateString(AppStateType state) {
  switch(state) {
    case APP_STATE_STARTUP: return (unsigned char*)"STARTUP";
    case APP_STATE_INIT: return (unsigned char*)"INIT";
  #if PL_CONFIG_USE_LINE_SENSOR
    case APP_STATE_CALIBRATE: return (unsigned char*)"CALIBRATE";
  #endif
  #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
    case APP_STATE_FOLLOW_LINE: return (unsigned char*)"FOLLOW_LINE";
  #endif
  #if PL_APP_FOLLOW_OBSTACLE
    case APP_STATE_FOLLOW_OBSTACLE: return (unsigned char*)"FOLLOW_OBSTACLE";
  #endif
  #if PL_DO_MIDI
    case APP_STATE_PLAY_MIDI: return (unsigned char*)"PLAY_MIDI";
  #endif
  #if PL_CONFIG_APP_SUMO
    case APP_STATE_RUN_SUMO: return (unsigned char*)"RUN_SUMO";
  #endif
    case APP_STATE_IDLE: return (unsigned char*)"IDLE";
    default:
      break;
  }
  return (unsigned char*)"unknown?";
}
#endif

static void StateMachine(bool buttonPress) {
#if PL_CONFIG_USE_LINE_SENSOR
  static uint8_t cnt;
#endif
  
  switch (appState) {
    case APP_STATE_STARTUP:
#if PL_CONFIG_USE_BUZZER && PL_DO_MINT
      BUZ_PlayTune(BUZ_TUNE_BUTTON); /* only short beep */
#elif PL_CONFIG_USE_BUZZER
      //BUZ_Beep(300, 800);
      //BUZ_PlayTune(BUZ_TUNE_WELCOME);
      BUZ_PlayTune(BUZ_TUNE_BUTTON);
#endif
#if PL_HAS_MUSIC_POWERUP
      McuWait_WaitOSms(300);
      MUSIC_PlayTheme(MUSIC_THEME_IMPERIAL);
#endif
      appState = APP_STATE_INIT;
      break;

    case APP_STATE_INIT:
#if PL_CONFIG_USE_LEDS
      McuLED_On(LEDS_Left);
      McuLED_Off(LEDS_Right);
#endif
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE || PL_CONFIG_APP_SUMO
      if (REF_CanUseSensor()) {
        appState = APP_STATE_IDLE;
      }
      if (buttonPress) {
        APP_StateStartCalibrate(); 
      }
#elif PL_APP_FOLLOW_OBSTACLE
      appState = APP_STATE_IDLE;
#endif
      break;

#if PL_APP_FOLLOW_OBSTACLE
    case APP_STATE_FOLLOW_OBSTACLE:
      cnt++;
      if (cnt>50) {
        cnt = 0;
#if PL_HAS_LED_RED
        McuLED_Toggle(LEDS_Left);
#endif
      }
      if (buttonPress) {
        followObstacle = FALSE;
        appState = APP_STATE_IDLE;
      }
      break;
#endif

#if PL_CONFIG_USE_LINE_SENSOR
    case APP_STATE_CALIBRATE:
      cnt++;
      if (cnt>50) {
        cnt = 0;
#if PL_CONFIG_USE_LEDS
        McuLED_Toggle(LEDS_Left);
#endif
      }
      if (buttonPress) {
        APP_StateStopCalibrate(); 
      }
      break;
#endif

    case APP_STATE_IDLE:
#if PL_CONFIG_USE_LEDS
      //McuLED_Off(LEDS_Left);
      //McuLED_On(LEDS_Right);
#endif
      if (buttonPress) {
#if (PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE) && !PL_APP_MINT /* Mint application handles buttons itself */
        if (LF_IsFollowing()) {
          LF_StopFollowing();
          appState = APP_STATE_IDLE;
        } else {
          LF_StartFollowing();
          appState = APP_STATE_FOLLOW_LINE;
        }
#elif PL_APP_FOLLOW_OBSTACLE
        followObstacle = TRUE;
        appState = APP_STATE_FOLLOW_OBSTACLE;
#elif 0 && PL_DO_MIDI
        MM_PlayMusic(-1); /* play current song */
        appState = APP_STATE_PLAY_MIDI;
#elif PL_CONFIG_APP_SUMO
        SUMO_StartCountdown();
        appState = APP_STATE_RUN_SUMO;
#endif
#if PL_HAS_MUSIC_START_SUMO
        MUSIC_PlayTheme(MUSIC_THEME_DARK_SIDE);
#endif
      }
      break;
      
#if 0 && PL_DO_MIDI
    case APP_STATE_PLAY_MIDI:
      if (buttonPress || !MM_IsPlaying()) {
        MM_StopPlaying();
        appState = APP_STATE_IDLE;
      }
      break;
#endif

#if PL_CONFIG_APP_SUMO
    case APP_STATE_RUN_SUMO:
#if PL_IS_INTRO_ZUMO_K22 || PL_IS_INTRO_ZUMO_K22_V2
      /* no LED signaling */
#else
  #if PL_CONFIG_USE_LEDS
      McuLED_Off(LEDS_Left);
      McuLED_Off(LEDS_Right);
  #endif
#endif
      if (!SUMO_IsDoingSumo()) {
        appState = APP_STATE_IDLE;
      } else {
#if PL_HAS_MUSIC_RUN_SUMO
        if (!MUSIC_IsPlaying()) {
          MUSIC_PlayTheme(MUSIC_THEME_BREATHING);
        }
#endif
      }
      if (buttonPress) {
        SUMO_StopSumo();
        appState = APP_STATE_IDLE;
      }
      break;
#endif
      
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
    case APP_STATE_FOLLOW_LINE:
#if PL_CONFIG_USE_LEDS
      McuLED_Off(LEDS_Left);
      McuLED_Off(LEDS_Right);
#endif
      if (!LF_IsFollowing()) {
        appState = APP_STATE_IDLE;
      }
      if (buttonPress) {
        LF_StopFollowing(); 
        appState = APP_STATE_IDLE;
      }
      break;
#endif
    default:
      break;
  } /* switch */
}

void APP_StateStartCalibrate(void) {
#if PL_CONFIG_USE_LINE_SENSOR
  REF_CalibrateStartStop();
  appState = APP_STATE_CALIBRATE;
#endif
}

bool APP_StateIsCalibrating(void) {
#if PL_CONFIG_USE_LINE_SENSOR
  return appState == APP_STATE_CALIBRATE;
#else
  return FALSE;
#endif
}

void APP_StateStopCalibrate(void) {
  appState = APP_STATE_IDLE;
#if PL_CONFIG_USE_LINE_SENSOR
  REF_CalibrateStartStop();
#endif
}

#if PL_APP_AVOID_OBSTACLE
static void AvoidObstacle(void) {
  if (DIST_10cmLeftOn() && DIST_5cmLeftOn() && !DIST_10cmRightOn() && !DIST_5cmRightOn()) {
    /* turn right */
    TURN_TurnAngle(10);
  } else if (!DIST_10cmLeftOn() && !DIST_5cmLeftOn() && DIST_10cmRightOn() && DIST_5cmRightOn()) {
    /* turn left */
    TURN_TurnAngle(-10);
  }
}
#endif

#if PL_APP_FOLLOW_OBSTACLE
static void FollowObstacle(void) {
  #define DUTY_SLOW   16
  #define DUTY_MEDIUM 20
  #define DUTY_FAST   23
  static uint8_t cnt;
  //uint16_t cm, us;
  
  cnt++; /* get called with 100 Hz, reduce to 10 Hz */
  if (cnt==10) {
    //us = US_Measure_us();
    cnt = 0;
    if (followObstacle) {
      cm = US_usToCentimeters(us, 22);
      if (cm<10) { /* back up! */
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -DUTY_SLOW);
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -DUTY_SLOW);
      } else if (cm>=10 && cm<=20) { /* stand still */
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
        //TURN_Turn(TURN_RIGHT45); /* try to avoid obstacle */
      } else if (cm>20 && cm<=40) { /* forward slowly */
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_MEDIUM);
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_MEDIUM);
      } else if (cm>40 && cm<60) { /* forward fast */
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_FAST);
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_FAST);
      } else { /* nothing in range */
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
      }
    }
  }
}
#endif

#if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
static uint8_t AutoCalibrateReflectance(void) {
#if PL_CONFIG_USE_SHELL
  SHELL_SendString((unsigned char*)"start auto-calibration...\r\n");
#endif
  /* perform automatic calibration */
  APP_StateStartCalibrate();
  TURN_Turn(TURN_LEFT90, NULL);
  McuWait_WaitOSms(500); /* wait some time */
  TURN_Turn(TURN_RIGHT90, NULL);
  McuWait_WaitOSms(500); /* wait some time */
  TURN_Turn(TURN_RIGHT90, NULL);
  McuWait_WaitOSms(500); /* wait some time */
  TURN_Turn(TURN_LEFT90, NULL);
  McuWait_WaitOSms(500); /* wait some time */
  TURN_Turn(TURN_STOP, NULL);
  APP_StateStopCalibrate();
#if PL_CONFIG_USE_SHELL
  SHELL_SendString((unsigned char*)"auto-calibration finished.\r\n");
#endif
  return ERR_OK;
}
#endif

#if 0 /* do not use this any more, use proper debouncing! */
static void CheckButton(void) {
#if PL_HAS_USER_BUTTON
  uint32_t timeTicks; /* time in ticks */
  #define BUTTON_CNT_MS  100  /* iteration count for button */
#if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
  bool autoCalibrate = FALSE;
#endif
  
  if (SW1_GetVal()==0) { /* button pressed */
    /* short press (1 beep): start or stop line following if calibrated 
     * 1 s press   (2 beep): calibrate manually
     * 2 s press   (3 beep): calibrate with auto-move
     * 3 s press   (4 beep or more): clear path
     * */
    vTaskDelay(50/portTICK_PERIOD_MS); /* simple debounce */
    if (SW1_GetVal()==0) { /* still pressed */
#if PL_CONFIG_USE_LEDS
      McuLED_On(LEDS_Left);
      McuLED_On(LEDS_Right);
#endif
      timeTicks = 0;
      while(SW1_GetVal()==0 && timeTicks<=6000/BUTTON_CNT_MS) { 
        vTaskDelay(BUTTON_CNT_MS/portTICK_PERIOD_MS);
        if ((timeTicks%(1000/BUTTON_CNT_MS))==0) {
#if PL_CONFIG_USE_BUZZER
          (void)BUZ_Beep(300, 500);
#elif PL_CONFIG_USE_LEDS
          McuLED_Toggle(LEDS_Right);
#endif
        }
        timeTicks++;
      } /* wait until released */
#if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
      autoCalibrate = FALSE;
#endif
      if (timeTicks<1000/BUTTON_CNT_MS) { /* less than 1 second */
#if PL_CONFIG_USE_SHELL
        SHELL_SendString((unsigned char*)"button press.\r\n");
#endif
        StateMachine(TRUE); /* <1 s, short button press, according to state machine */
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE || PL_CONFIG_APP_SUMO
      } else if (timeTicks>=(1000/BUTTON_CNT_MS) && timeTicks<(2000/BUTTON_CNT_MS)) {
#if PL_CONFIG_USE_SHELL
        SHELL_SendString((unsigned char*)"calibrate.\r\n");
#endif
        APP_StateStartCalibrate(); /* 1-2 s: start calibration by hand */
      } else if (timeTicks>=(2000/BUTTON_CNT_MS) && timeTicks<(3000/BUTTON_CNT_MS)) {
#if PL_CONFIG_USE_SHELL
        SHELL_SendString((unsigned char*)"auto calibrate.\r\n");
#endif
        autoCalibrate = TRUE; /* 2-3 s: start auto calibration */
#endif
#if PL_CONFIG_APP_LINE_MAZE
      } else if (timeTicks>=(3000/BUTTON_CNT_MS)) {
        MAZE_ClearSolution();
        SHELL_SendString((unsigned char*)"Deleted solution.\r\n");
#if PL_CONFIG_USE_BUZZER
        McuWait_WaitOSms(500);
        (void)BUZ_Beep(300, 100);
        McuWait_WaitOSms(100);
#endif
        #endif
      }
      while (SW1_GetVal()==0) { /* wait until button is released */
        vTaskDelay(BUTTON_CNT_MS/portTICK_PERIOD_MS);
      }
#if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
      if (autoCalibrate) {
        /* perform automatic calibration */
        McuWait_WaitOSms(1500); /* wait some time so the user can remove the finger from the button */
        (void)AutoCalibrateReflectance();
      }
#endif
    }
  } /* if */
#endif
}
#endif

#if PL_HAS_ACCEL_STOP
static void CheckAccelerometer(void) {
  int16_t x, y, z;
  bool isEnabled;
  uint8_t res;
  static uint8_t cnt = 0;

#if PL_HAS_ACCEL_STOP_INVERTED /* board with sensor mounted head-over */
  #define ACCEL_OK_CONDITION(x,y,z) ((z)<0)
#else
  #define ACCEL_OK_CONDITION(x,y,z) (-900<=(z) && (z)<5000)
#endif

  if ((cnt%10)==0) { /* check not too often */
    res = ACCEL_isEnabled(&isEnabled);
  } else {
    res = ERR_OK;
    isEnabled = TRUE;
  }
  cnt++;
  if (res==ERR_OK && isEnabled) {
    ACCEL_GetValues(&x, &y, &z);
    if (!ACCEL_OK_CONDITION(x,y,z)) { /* measure again */
      vTaskDelay(100/portTICK_PERIOD_MS);
      ACCEL_GetValues(&x, &y, &z);
      if (!ACCEL_OK_CONDITION(x,y,z)) {
    #if PL_CONFIG_USE_DRIVE
        DRV_Stop(1000);
    #else
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
        MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
    #endif
    #if PL_DO_LINE_MAZE || PL_DO_LINE_FOLLOWING
        LF_StopFollowing();
    #endif
    #if PL_DO_LINE_MAZE
        MAZE_ClearSolution();
    #endif
    #if PL_CONFIG_APP_SUMO
        SUMO_Stop();
    #endif
    #if PL_HAS_MUSIC_ACCEL_STOP
        MUSIC_PlayTheme(MUSIC_THEME_DAVE);
    #endif
        SHELL_SendString((unsigned char*)"Accel: Engines stopped!\r\n");
#if 1
        {
          uint8_t buf[32];

          McuUtility_strcpy(buf, sizeof(buf), "x: ");
          McuUtility_strcatNum16s(buf, sizeof(buf), x);
          McuUtility_strcat(buf, sizeof(buf), " y: ");
          McuUtility_strcatNum16s(buf, sizeof(buf), y);
          McuUtility_strcat(buf, sizeof(buf), " z: ");
          McuUtility_strcatNum16s(buf, sizeof(buf), z);
          McuUtility_strcat(buf, sizeof(buf), "\r\n");
          SHELL_SendString(buf);
        }
#endif
  #if PL_CONFIG_USE_BUZZER
        (void)BUZ_Beep(300, 500);
  #endif
  #if PL_HAS_MUSIC_ACCEL_STOP
        MUSIC_PlayTheme(MUSIC_THEME_DAVE);
  #endif
      }
    }
  } else {
    SHELL_SendString((unsigned char*)"Accelerometer disabled?\r\n");
    (void)ACCEL_Enable();
  }
}
#endif /* PL_HAS_ACCEL_STOP */

#if PL_CONFIG_USE_EVENTS
static void MapButtonsForDisplay(EVNT_Handle event) {
#if PL_CONFIG_HAS_LCD
  if (GDisp1_GetDisplayOrientation()==GDisp1_ORIENTATION_LANDSCAPE) {
    switch(event) {
      case EVNT_SW1_PRESSED:
        LCD_SetEvent(LCD_BTN_RIGHT);
        break;
      case EVNT_SW1_PRESSED_REPEAT:
        LCD_SetEvent(LCD_BTN_RIGHT_REPEAT);
        break;
      case EVNT_SW2_PRESSED:
        LCD_SetEvent(LCD_BTN_LEFT);
        break;
      case EVNT_SW2_PRESSED_REPEAT:
        LCD_SetEvent(LCD_BTN_LEFT_REPEAT);
        break;
      case EVNT_SW3_PRESSED:
        LCD_SetEvent(LCD_BTN_DOWN);
         break;
      case EVNT_SW3_PRESSED_REPEAT:
        LCD_SetEvent(LCD_BTN_DOWN_REPEAT);
        break;
      case EVNT_SW4_PRESSED:
        LCD_SetEvent(LCD_BTN_CENTER);
        break;
      case EVNT_SW4_PRESSED_REPEAT:
        LCD_SetEvent(LCD_BTN_CENTER_REPEAT);
        break;
      case EVNT_SW5_PRESSED:
        LCD_SetEvent(LCD_BTN_UP);
        break;
      case EVNT_SW5_PRESSED_REPEAT:
        LCD_SetEvent(LCD_BTN_UP_REPEAT);
        break;
      case EVNT_SW6_PRESSED:
        LCD_SetEvent(LCD_SIDE_BTN_DOWN);
        break;
      case EVNT_SW6_PRESSED_REPEAT:
        LCD_SetEvent(LCD_SIDE_BTN_DOWN_REPEAT);
        break;
      case EVNT_SW7_PRESSED:
        LCD_SetEvent(LCD_SIDE_BTN_UP);
        break;
      case EVNT_SW7_PRESSED_REPEAT:
        LCD_SetEvent(LCD_SIDE_BTN_UP_REPEAT);
        break;
      case EVNT_SW1_RELEASED:
      case EVNT_SW1_LRELEASED:
      case EVNT_SW2_RELEASED:
      case EVNT_SW2_LRELEASED:
      case EVNT_SW3_RELEASED:
      case EVNT_SW3_LRELEASED:
      case EVNT_SW4_RELEASED:
      case EVNT_SW4_LRELEASED:
      case EVNT_SW5_RELEASED:
      case EVNT_SW5_LRELEASED:
      case EVNT_SW6_RELEASED:
      case EVNT_SW6_LRELEASED:
      case EVNT_SW7_RELEASED:
      case EVNT_SW7_LRELEASED:
        LCD_SetEvent(LCD_BTN_RELEASED);
        break;
      default: break;
    } /* case */
  } else {
    switch(event) {
      case EVNT_SW1_PRESSED: LCD_SetEvent(LCD_BTN_LEFT); break;
      case EVNT_SW2_PRESSED: LCD_SetEvent(LCD_BTN_RIGHT); break;
      case EVNT_SW3_PRESSED: LCD_SetEvent(LCD_BTN_UP); break;
      case EVNT_SW4_PRESSED: LCD_SetEvent(LCD_BTN_CENTER); break;
      case EVNT_SW5_PRESSED: LCD_SetEvent(LCD_BTN_DOWN); break;
      case EVNT_SW6_PRESSED: LCD_SetEvent(LCD_SIDE_BTN_UP); break;
      case EVNT_SW7_PRESSED: LCD_SetEvent(LCD_SIDE_BTN_DOWN); break;
      default: break;
    } /* case */
  }
#else
  (void)event; /* not used */
#endif
}
#endif /* PL_CONFIG_USE_EVENTS */

#if PL_CONFIG_USE_EVENTS
static void APP_EventHandler(EVNT_Handle event) {
  MapButtonsForDisplay(event);
  switch(event) {
  case EVNT_STARTUP:
#if PL_CONFIG_USE_LEDS
    McuLED_On(LEDS_Left); /* just do something */
#endif
#if PL_CONFIG_HAS_BUZZER
    BUZ_PlayTune(BUZ_TUNE_WELCOME);
#endif
    break;

  case EVNT_LED_HEARTBEAT:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Left);
#endif
    break;

#if PL_CONFIG_USE_BUTTONS

#if PL_CONFIG_NOF_BUTTONS>=1
  case EVNT_SW1_PRESSED:
  #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
    LF_StartStopFollowing();
  #endif
  #if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
  #endif
    SHELL_SendString((uint8_t*)"SW1 pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('a'); /* A */
    #endif
  #if PL_DO_MIDI
    if (MM_IsPlaying()) {
      MM_StopPlaying();
    } else {
      MM_Play(1);
    }
  #elif PL_CONFIG_APP_SUMO
    #if PL_CONFIG_HAS_BUZZER
    BUZ_PlayTune(BUZ_TUNE_BUTTON);
    #endif
    if (!SUMO_IsDoingSumo()) {
      SUMO_StartCountdown();
    } else {
      SUMO_StopSumo();
    }
  #endif
    break;

  case EVNT_SW1_PRESSED_REPEAT:
    SHELL_SendString((uint8_t*)"SW1 repeat\r\n");
    break;

  case EVNT_SW1_RELEASED:
  #if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
  #endif
    SHELL_SendString((uint8_t*)"SW1 released\r\n");
    break;

  case EVNT_SW1_LPRESSED:
  #if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
  #endif
    SHELL_SendString((uint8_t*)"SW1 long pressed\r\n");
    #if PL_CONFIG_HAS_BUZZER
    BUZ_PlayTune(BUZ_TUNE_BUTTON_LONG);
    #endif
    break;

  case EVNT_SW1_LRELEASED:
  #if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
  #endif
    SHELL_SendString((uint8_t*)"SW1 long released\r\n");
    #if PL_CONFIG_HAS_BUZZER
    BUZ_PlayTune(BUZ_TUNE_BUTTON_LONG);
    #endif
    break;
#endif /* PL_CONFIG_NOF_BUTTONS>=1 */

  #if PL_CONFIG_NOF_BUTTONS>=2
  case EVNT_SW2_PRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW2 pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('b'); /* B */
    #endif
    break;
  case EVNT_SW2_PRESSED_REPEAT:
    SHELL_SendString((uint8_t*)"SW2 repeat\r\n");
    break;
  case EVNT_SW2_LPRESSED:
  case EVNT_SW2_RELEASED:
    break;
    #endif


    #if PL_CONFIG_NOF_BUTTONS>=3
  case EVNT_SW3_PRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW3 pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('c'); /* C */
    #endif
    break;
  case EVNT_SW3_PRESSED_REPEAT:
    SHELL_SendString((uint8_t*)"SW3 repeat\r\n");
    break;
  case EVNT_SW3_LPRESSED:
  case EVNT_SW3_RELEASED:
    break;
  #endif

    #if PL_CONFIG_NOF_BUTTONS>=4
  case EVNT_SW4_PRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW4 pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('d'); /* D */
    #endif
    break;
  case EVNT_SW4_PRESSED_REPEAT:
    SHELL_SendString((uint8_t*)"SW4 repeat\r\n");
    break;
  case EVNT_SW4_LPRESSED:
  case EVNT_SW4_RELEASED:
    break;
  #endif

    #if PL_CONFIG_NOF_BUTTONS>=5
  case EVNT_SW5_PRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW5 pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('e');
    #endif
    break;

  case EVNT_SW5_LPRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW5L pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('E');
    #endif
    break;
  case EVNT_SW5_PRESSED_REPEAT:
    SHELL_SendString((uint8_t*)"SW5 repeat\r\n");
    break;
  case EVNT_SW5_RELEASED:
    break;
  #endif

    #if PL_CONFIG_NOF_BUTTONS>=6
  case EVNT_SW6_PRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW6 pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('f');
    #endif
    break;

  case EVNT_SW6_LPRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW6L pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('F');
    #endif
    break;
  case EVNT_SW6_PRESSED_REPEAT:
    SHELL_SendString((uint8_t*)"SW6 repeat\r\n");
    break;
  case EVNT_SW6_RELEASED:
    break;
  #endif

    #if PL_CONFIG_NOF_BUTTONS>=7
  case EVNT_SW7_PRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW7 pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('g');
    #endif
   break;

  case EVNT_SW7_LPRESSED:
#if PL_CONFIG_USE_LEDS
    McuLED_Toggle(LEDS_Right);
#endif
    SHELL_SendString((uint8_t*)"SW7L pressed\r\n");
    #if PL_CONFIG_CONTROL_SENDER
    (void)REMOTE_SendButton('G'); /* G */
    #endif
   break;
  case EVNT_SW7_PRESSED_REPEAT:
    SHELL_SendString((uint8_t*)"SW7 repeat\r\n");
    break;
  case EVNT_SW7_RELEASED:
    break;
  #endif
#endif
  default:
    break;
  } /* switch */
}
#endif /* PL_CONFIG_USE_EVENTS */

static void MainTask(void *pvParameters) {
#if PL_HAS_ACCEL_STOP || (PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS)
  uint32_t cnt = 0;
#endif
  
  (void)pvParameters; /* not used */
  vTaskDelay(pdMS_TO_TICKS(100)); /* wait some time to get hardware (SW1) pull-up effective */
#if PL_HAS_ACCEL_SENSOR
  if (ACCEL_LowLevelInit()!=ERR_OK) { /* always enable accelerometer */
#if PL_CONFIG_USE_SHELL
    SHELL_SendString((unsigned char*)"ERR: Accel sensor init failed!\r\n");
#endif
  }
#endif
#if PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS
  McuLED_On(LEDS_Left);
#endif
#if PL_HAS_TSS
  Configure(); /* initialize TSS library */
#endif
#if PL_HAS_MINT_LED
  MINTLED_SetMode(MINTLED_MODE_WAIT_BALL_REMOVAL); /* initial demo mode */
#endif
  for(;;) {
#if PL_CONFIG_HAS_DEBOUNCE
    KEYDBNC_Process(); /* scan and debounce polled keys */
#endif
#if PL_CONFIG_USE_EVENTS
    EVNT_HandleEvent(APP_EventHandler, TRUE);
#endif
#if PL_HAS_TSS
    TSS_Task(); /* call TSS library to process touches */
#elif 0 && !PL_DO_TEST
    CheckButton();
#endif
#if PL_APP_AVOID_OBSTACLE
    AvoidObstacle();
#endif
#if PL_APP_FOLLOW_OBSTACLE
    FollowObstacle();
#endif
    StateMachine(FALSE);
#if 0 && PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS
    if ((cnt%48)==0) {
      McuLED_Toggle(LEDS_Left);
    }
#endif
#if PL_HAS_FREEMASTER
    FMSTR1_Poll();
    FMSTR1_Recorder();
#endif
#if PL_HAS_ACCEL_STOP
    if ((cnt%10)==0) { /* every 100 ms */
      CheckAccelerometer();
    }
#endif /* PL_HAS_ACCEL_STOP */
#if PL_HAS_LASER
    {
      bool isLeft, isRight;

      LASER_LeftRightStatus(&isLeft, &isRight);
      USER_LED2_Put(isLeft);
      USER_LED_Put(isRight);
    }
#endif
#if PL_HAS_ACCEL_STOP || (PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS)
    cnt++;
#endif
    vTaskDelay(10/portTICK_PERIOD_MS);
  } /* for */
}

#if PL_CONFIG_USE_SHELL
static uint8_t APP_PrintHelp(const McuShell_StdIOType *io) {
  McuShell_SendHelpStr((unsigned char*)"app", (unsigned char*)"Group of app commands\r\n", io->stdOut);
#if PL_DO_MINT
  McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"Short press (1 beep): enter automatic mode, simulate ball received/removed.\r\n", io->stdOut);
#elif PL_IS_ROBOT
  McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"Short press (1 beep): start or stop line following if calibrated\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"1 s press   (2 beep): calibrate manually\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"2 s press   (3 beep): calibrate with auto-move\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"3 s press   (4 beep or more): clear path\r\n", io->stdOut);
#endif
  McuShell_SendHelpStr((unsigned char*)"  help|status", (unsigned char*)"Shows app help or status\r\n", io->stdOut);
#if PL_APP_FOLLOW_OBSTACLE
  McuShell_SendHelpStr((unsigned char*)"  follow (on|off)", (unsigned char*)"Obstacle following on or off\r\n", io->stdOut);
#endif
#if PL_APP_AVOID_OBSTACLE
  McuShell_SendHelpStr((unsigned char*)"  avoid (on|off)", (unsigned char*)"Obstacle avoidance on or off\r\n", io->stdOut);
#endif
#if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
  McuShell_SendHelpStr((unsigned char*)"  autocalib", (unsigned char*)"Automatically calibrate line sensor\r\n", io->stdOut);
#endif
  return ERR_OK;
}

static uint8_t APP_PrintStatus(const McuShell_StdIOType *io) {
  McuShell_SendStatusStr((unsigned char*)"app", (unsigned char*)"Application status\r\n", io->stdOut);
#if PL_CONFIG_APP_SUMO
  McuShell_SendStatusStr((unsigned char*)"  mode", (unsigned char*)"SUMO\r\n", io->stdOut);
#endif
#if PL_APP_REMOTE_CONTROL
  McuShell_SendStatusStr((unsigned char*)"  mode", (unsigned char*)"REMOTE CONTROL\r\n", io->stdOut);
#endif
#if  PL_CONFIG_APP_LINE_FOLLOWING
  McuShell_SendStatusStr((unsigned char*)"  mode", (unsigned char*)"LINE FOLLOWING\r\n", io->stdOut);
#endif
#if PL_CONFIG_APP_LINE_MAZE
  McuShell_SendStatusStr((unsigned char*)"  mode", (unsigned char*)"LINE MAZE\r\n", io->stdOut);
#endif
#if PL_APP_FOLLOW_OBSTACLE
  McuShell_SendStatusStr((unsigned char*)"  follow", followObstacle?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
#endif
#if PL_HAS_USER_BUTTON
  McuShell_SendStatusStr((unsigned char*)"  button", SW1_GetVal()==0?(unsigned char*)"pressed\r\n":(unsigned char*)"released\r\n", io->stdOut);
#endif
  McuShell_SendStatusStr((unsigned char*)"  App State", AppStateString(appState), io->stdOut);
  McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
  return ERR_OK;
}

uint8_t APP_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*)"app help")==0) {
    *handled = TRUE;
    return APP_PrintHelp(io);
  } else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"app status")==0) {
    *handled = TRUE;
    return APP_PrintStatus(io);
#if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
  } else if (McuUtility_strcmp((char*)cmd, (char*)"app autocalib")==0) {
    *handled = TRUE;
    return AutoCalibrateReflectance();
#endif
#if PL_APP_AVOID_OBSTACLE
  } else if (McuUtility_strcmp((char*)cmd, (char*)"app avoid on")==0) {
    avoidObstacle = TRUE;
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"app avoid off")==0) {
    avoidObstacle = FALSE;
    *handled = TRUE;
#endif
#if PL_APP_FOLLOW_OBSTACLE
  } else if (McuUtility_strcmp((char*)cmd, (char*)"app follow on")==0) {
    followObstacle = TRUE;
    *handled = TRUE;
  } else if (McuUtility_strcmp((char*)cmd, (char*)"app follow off")==0) {
    followObstacle = FALSE;
    *handled = TRUE;
#endif
  }
  return res;
}
#endif /* PL_CONFIG_USE_SHELL */

#if configUSE_HEAP_SCHEME==5
  static __attribute__ ((used,section(".noinit.$SRAM_LOWER.Heap5"))) uint8_t heap_sram_lower[64*1024]; /* placed in in no_init section inside SRAM_LOWER */
  static __attribute__ ((used,section(".noinit_Heap5"))) uint8_t heap_sram_upper[16*1024]; /* placed in in no_init section inside SRAM_UPPER */
  static const HeapRegion_t xHeapRegions[] =
  {
   { &heap_sram_lower[0], sizeof(heap_sram_lower)},
   { &heap_sram_upper[0], sizeof(heap_sram_upper)},
   { NULL, 0 } /* << Terminates the array. */
  };
#endif

void APP_Run(void) {
#if configUSE_HEAP_SCHEME==5
  vPortDefineHeapRegions(xHeapRegions); /* Pass the array into vPortDefineHeapRegions(). Must be called first! */
#endif
  PL_Init();
  appState = APP_STATE_STARTUP;
  if (xTaskCreate(MainTask, "Main", 600/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+1, NULL) != pdPASS) {
    for(;;){} /* error */
  }
  vTaskStartScheduler();
}
