/*
 * 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 "esp_system.h"
#include "nvs_flash.h"
#include "nvs.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 */

/* function declarations */
static void Challenge_App_SetRobotMode(bool modeStationary, bool storeToNvs);
static bool Challenge_App_StoreRobotModeToNVS(bool modeStationary);
static bool Challenge_App_GetRobotModeFromNVS(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();

			MyMqtt_Subscribe("scada/status");

			// endless loop
			for(;;){
				MyMqtt_Publish("scada/status", "Testmessage from ESP");

				vTaskDelay(pdMS_TO_TICKS(10000));
			}
		}


		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){
	// Initialize NVS
	esp_err_t err = nvs_flash_init();
	if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
		// NVS partition was truncated and needs to be erased
		// Retry nvs_flash_init
		ESP_ERROR_CHECK(nvs_flash_erase());
		err = nvs_flash_init();
	}
	ESP_ERROR_CHECK( err );

	Challenge_App_GetRobotModeFromNVS();

	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");
	if(storeToNvs){
		Challenge_App_StoreRobotModeToNVS(modeStationary);
	}
}

// returns true on success
static bool Challenge_App_StoreRobotModeToNVS(bool modeStationary){
	bool success = false;

	// Open
	ESP_LOGI(TAG, "Opening Non-Volatile Storage (NVS) handle... ");
	nvs_handle_t my_handle;
	esp_err_t err = nvs_open("storage", NVS_READWRITE, &my_handle);
	if (err != ESP_OK) {
		ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));
		return false;
	}

	// Write
	uint8_t value = modeStationary==true?1:0;
	ESP_LOGI(TAG, "Updating robotMode in NVS ... ");
	err = nvs_set_u8(my_handle, "robotMode", value);
	if(err != ESP_OK){
		ESP_LOGE(TAG, "Failed to update robotMode in NVS. Error=(%s)", esp_err_to_name(err));
		success = false;
	}else{ // successfully set
		// Commit written value.
		// After setting any values, nvs_commit() must be called to ensure changes are written
		// to flash storage. Implementations may write to storage at other times,
		// but this is not guaranteed.
		ESP_LOGI(TAG, "Committing updates in NVS ... ");
		err = nvs_commit(my_handle);
		if(err != ESP_OK){
			ESP_LOGE(TAG, "Failed to commit to NVS. Error=(%s)", esp_err_to_name(err));
			success = false;
		}else{
			ESP_LOGI(TAG, "Successfully committed changes to NVS.");
			success = true;
		}
	}

	// Close
	nvs_close(my_handle);

	return success;
}

// returns true on success
static bool Challenge_App_GetRobotModeFromNVS(void){
	bool success = false;
	bool notInitialized = false;

    // Open
    ESP_LOGI(TAG, "Opening Non-Volatile Storage (NVS) handle... ");
    nvs_handle_t my_handle;
    esp_err_t err = nvs_open("storage", NVS_READWRITE, &my_handle);
    if (err != ESP_OK) {
    	ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));
    	return false;
    }

    // Read
    ESP_LOGI(TAG, "Reading robot mode from NVS ... ");
    uint8_t value = 0;
	err = nvs_get_u8(my_handle, "robotMode", &value);
	switch (err) {
		case ESP_OK:
			ESP_LOGI(TAG, "Reading robot mode done. Read robotMode = %i", value);
			Challenge_App_SetRobotMode(value==1?true:false, false); // do not store to nvs (reading rn)
			success = true;
			break;
		case ESP_ERR_NVS_NOT_FOUND:
			ESP_LOGI(TAG, "Reading robot mode done. The value is not initialized yet!");
			notInitialized = true;
			break;
		default :
			ESP_LOGI(TAG, "Error reading robot mode. Error=(%s)", esp_err_to_name(err));
			success = false;
	}

	// Close
	nvs_close(my_handle);

	if(notInitialized){
		success = Challenge_App_StoreRobotModeToNVS(false);  // initialize to false
	}

	return success;
}


/*********/
/* 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);
  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
  }
  return ERR_OK;
}
#endif /* PL_CONFIG_USE_SHELL */