/*
 * application.s
 *
 *  Created on: 25.11.2022
 *      Author: jonas
 */


#include <stdbool.h>
#include "challenge_app.h"
#include "platform.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "myMqtt.h"
#include "wifi.h"
#include "McuUtility.h"
#include "challenge_com.h"
#include "challenge_nvs.h"
#include "led.h"

/* LOCAL Var */
/* identifies robot Mode for the challenge: true = robot is stationary, false = robot is moveable */
static bool _robotModeStationary = false;

static TaskHandle_t appTaskHandle;   /* task handle */
#define TAG "CHALLENGE_APP" /* tag for logging with ESP_LOG */

/* local function declarations */
static void Challenge_App_SetRobotMode(bool modeStationary, bool storeToNvs);
static void Challenge_App_RebootEsp32(void);

static void appTask(void *pv){
	/*Task argument not used
	 * if(pv != NULL){
		printf("task argument: %s\n", (char*)pv);
	}*/

	bool myMqttInitSuccess = false;
	ESP_LOGI(TAG, "Challenge application was started. Waiting for WiFi connection....");
	while(WiFi_isConnected() == false){
		vTaskDelay(pdMS_TO_TICKS(1000));
	}
	// wifi now connected
	ESP_LOGI(TAG, "Challenge Application task detected that WiFi is connected.");
	// endless loop: always reconnect to MQTT when something fails
	while(1){

		ESP_LOGI(TAG, "MyMqtt Initializing....");
		myMqttInitSuccess = MyMqtt_Init();
		if(myMqttInitSuccess){
			ESP_LOGI(TAG, "MyMqtt Initialization successful.");
			ESP_LOGI(TAG, "Challenge Application started.");

			Challenge_Com_SubscribeToAllTopics();

			// endless loop
			for(;;){
				vTaskDelay(pdMS_TO_TICKS(5000));
				// auto publish battery voltage
				Challenge_Com_PublishBatteryVoltage();
			}
		}

		ESP_LOGE(TAG, "Challenge Mainloop failure: %s", myMqttInitSuccess==true?"Something in challenge app failed. Re-initializing MQTT in 5s.":"Init of MyMqtt failed! Retrying in 5s.");
		MyMqtt_Deinit();
		if(myMqttInitSuccess == false) { vTaskDelay(5000); } // only wait if the init is the issue
	}
}


BaseType_t res;
void Challenge_App_Init(void){
	Challenge_Nvs_Init();

	bool retrievedMode = false;
	Challenge_Nvs_GetRobotModeFromNVS(&retrievedMode);
	Challenge_App_SetRobotMode(retrievedMode, false);  // do not store to NVS, not required

	res = xTaskCreate(appTask, "ChallengeAppTask", 4096/sizeof(StackType_t), (void*)"ARG_0", tskIDLE_PRIORITY, &appTaskHandle);
	if(res != pdPASS){
		ESP_LOGE(TAG, "Creating appTask failed");
	}
}

bool Challenge_App_GetRobotMode(void){
	return _robotModeStationary;
}

static void Challenge_App_SetRobotMode(bool modeStationary, bool storeToNvs){
	_robotModeStationary = modeStationary;
	ESP_LOGI(TAG, "Changed robot mode to %s", _robotModeStationary?(unsigned char*)"stationary":(unsigned char*)"mobile");
	// update led behavior to be able to differentiate the robots
	if(modeStationary){
		LED_SetOnOffTime(5000, 1000);
	} else{
		LED_SetOnOffTime(500, 500);
	}

	if(storeToNvs){
		Challenge_Nvs_StoreRobotModeToNVS(modeStationary);
	}
}

static void Challenge_App_RebootEsp32(void){
    fflush(stdout);
    esp_restart();
}

/*********/
/* SHELL */
/*********/
#if PL_CONFIG_USE_SHELL
static uint8_t PrintStatus(const McuShell_StdIOType *io) {
  McuShell_SendStatusStr((unsigned char*)"challenge", (unsigned char*)"ESP32 Challenge status\r\n", io->stdOut);
  McuShell_SendStatusStr((unsigned char*)"  robotMode", Challenge_App_GetRobotMode()?(unsigned char*)"stationary\r\n":(unsigned char*)"mobile\r\n", io->stdOut);
  return ERR_OK;
}

static uint8_t PrintHelp(const McuShell_StdIOType *io) {
  McuShell_SendHelpStr((unsigned char*)"challenge", (unsigned char*)"Group of ESP32 Challenge commands\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  help|status", (unsigned char*)"Shows Challenge help or status\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  setMode s(tationary)", (unsigned char*)"Sets the mode of this robot to stationary\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  setMode m(mobile)", (unsigned char*)"Sets the mode of this robot to mobile\r\n", io->stdOut);
  McuShell_SendHelpStr((unsigned char*)"  reboot", (unsigned char*)"Reboots the ESP32\r\n", io->stdOut);
  return ERR_OK;
}

uint8_t Challenge_ParseShellCommand(const unsigned char* cmd, bool *handled, const McuShell_StdIOType *io) {
  if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"challenge help")==0) {
    *handled = TRUE;
    return PrintHelp(io);
  } else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"challenge status")==0) {
    *handled = TRUE;
    return PrintStatus(io);
  } else if (McuUtility_strcmp((char*)cmd, (char*)"challenge setMode stationary")==0 ||
		     McuUtility_strcmp((char*)cmd, (char*)"challenge setMode s")==0) {
    *handled = TRUE;
    Challenge_App_SetRobotMode(true, true); // set stationary
  } else if (McuUtility_strcmp((char*)cmd, (char*)"challenge setMode mobile")==0  ||
		     McuUtility_strcmp((char*)cmd, (char*)"challenge setMode m")==0 ) {
    *handled = TRUE;
    Challenge_App_SetRobotMode(false, true);  // set mobile
  }else if (McuUtility_strcmp((char*)cmd, (char*)"challenge reboot")==0) {
      *handled = TRUE;
      Challenge_App_RebootEsp32();
    }
  return ERR_OK;
}
#endif /* PL_CONFIG_USE_SHELL */