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

#include "Remote.h"
#if PL_CONFIG_USE_REMOTE
#include "McuRTOS.h"
#include "McuUtility.h"
#include "Drive.h"
#include "Shell.h"
#include "Buzzer.h"
#include "McuESP32.h"
#include "McuLog.h"

/*! \todo ADIS: This is a (mostly empty) template where the remote commands from the ESP32 could be handled. This module should be extended to send data back to the ESP32 */
#define ESP_MSG_PREFIX "@robot:cmd "
#define ESP_MSG_POSTFIX "!\0"


static QueueHandle_t REMOTE_RxFromESP_Queue;

#define REMOTE_RX_FROM_ESP_QUEUE_LENGTH      32 /* items in queue */
#define REMOTE_RX_FROM_ESP_QUEUE_ITEM_SIZE   1  /* each item is a single character */

/* called by the gateway task to put a char from the ESP into the queue for the remote */
void REMOTE_GatewayRxFromESP(unsigned char ch) {
  if (xQueueSendToBack(REMOTE_RxFromESP_Queue, &ch, pdMS_TO_TICKS(10))!=pdPASS) {
    /* was not possible to put it into the queue: will loose data here */
  }
}

static void RemoteTask(void *pv) {
  unsigned char ch;
  BaseType_t res;
  unsigned char espMsg[McuShell_DEFAULT_SHELL_BUFFER_SIZE] = "";
  uint8_t saveChar = 0;
  uint8_t writeIndex = 0;
  (void)pv; /* not used */
#if 0 /* example making a beep */
  BUZ_Beep(200, 500);
#endif
#if 0 /* example driving the robot */
  DRV_SetMode(DRV_MODE_SPEED);
  DRV_SetSpeed(500, 500);
  vTaskDelay(pdMS_TO_TICKS(1000));
  DRV_SetMode(DRV_MODE_STOP);
#endif
#if 0 /* example receiving character from queue */
  res = xQueueReceive(REMOTE_RxFromESP_Queue, &ch, pdMS_TO_TICKS(10)); /* wait max 10 ms */
  if (res==errQUEUE_EMPTY) {
    ch = '\0'; /* nothing received */
  }
  res = xQueueReceive(REMOTE_RxFromESP_Queue, &ch, portMAX_DELAY); /* wait 'forever' */
  if (res==errQUEUE_EMPTY) {
    ch = '\0'; /* nothing received */
  }
#endif
  for(;;) {
    res = xQueueReceive(REMOTE_RxFromESP_Queue, &ch, pdMS_TO_TICKS(10)); /* wait max 10 ms */
    if (res==errQUEUE_EMPTY) {
	    ch = '\0'; /* nothing received */

    }
    else if(res == pdPASS) {
    	/* something was received */
    	/* handle remote stream from ESP */
//    	uint8_t mCh = (uint8_t)ch;
//    	uint8_t mCh[10] = "Char: ";
//    	unsigned char newLine = '\n';
//    	McuUtility_strcat(mCh, 10, &ch);
//    	McuUtility_strcat(mCh, 10, &newLine);
//    	McuShell_SendStr(mCh, McuShell_GetStdio()->stdOut);
        if(ch == '@'){
        	saveChar = 1;
        	writeIndex = 0;
        	BUZ_Beep(200, 500);
        }
        if(saveChar == 1){
        	espMsg[writeIndex++] = ch;
        }
        if(ch == '!' && saveChar == 1){
        	saveChar = 0;
        	McuShell_SendStr(espMsg, McuShell_GetStdio()->stdOut);
        	if(McuUtility_strcmp((char*)espMsg, ESP_MSG_PREFIX)==0){ /* check prefix */
        		unsigned char *p;
          		p = (unsigned char*)espMsg + sizeof(ESP_MSG_PREFIX) - 1;
          		McuUtility_strCutTail((uint8_t*)p, (uint8_t*)ESP_MSG_POSTFIX);
          		McuShell_SendStr((uint8_t*)p, McuShell_GetStdio()->stdOut);
          		SHELL_ParseCommandWithIO((unsigned char*)espMsg, McuESP32_GetTxToESPStdio());
        	}
        }else if(writeIndex == McuShell_DEFAULT_SHELL_BUFFER_SIZE && saveChar == 1){
        	saveChar = 0;
        	McuShell_SendStr(espMsg, McuShell_GetStdio()->stdOut);
        	McuShell_SendStr((uint8_t*)" Error in cmd.\r\n", McuShell_GetStdio()->stdOut);
        }
    }
  }
}

uint8_t REMOTE_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
  return ERR_OK;
}

void REMOTE_Deinit(void) {
}

void REMOTE_Init(void) {
  if (xTaskCreate(RemoteTask, "Remote", 1024/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+1, NULL) != pdPASS) {
    for(;;){} /* error */
  }
  REMOTE_RxFromESP_Queue = xQueueCreate(REMOTE_RX_FROM_ESP_QUEUE_LENGTH, REMOTE_RX_FROM_ESP_QUEUE_ITEM_SIZE);
  if (REMOTE_RxFromESP_Queue==NULL) {
    for(;;){} /* out of memory? */
  }
  vQueueAddToRegistry(REMOTE_RxFromESP_Queue, "RemoteRxFromESPQueue");
}

#endif /* PL_CONFIG_USE_REMOTE */