/*
 * robo_wrapper.c
 *
 *  Created on: 09.12.2022
 *      Author: jonas
 */
#include "robo_wrapper.h"
#include "McuUtility.h"
#include "McuShell.h"
#include "Shell.h"
#include "esp_log.h"

#define RS_CMD_DRIVE_PREFIX "drive "
#define RS_CMD_TURN_PREFIX "turn "
#define BUF_SIZE 50

#define TAG "ROBO_WRAPPER" /* tag for logging with ESP_LOG */

static bool getValueOfStatusResponse (unsigned char* response, const unsigned char* key, unsigned char* valuevalue, size_t valueStringLen);


/* Sets the robot to automatic / manual mode */
bool Robo_Wrapper_SetMode(bool automatic){
	if(automatic){
		/* tbd */
	}else{
		/* set speed to zero */
		unsigned char response[128];
		SHELL_SendToRobotAndGetResponse((const unsigned char*)"drive speed 0 0", response, sizeof(response));
		/* set mode to speed */
		unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
		McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode speed");
		SHELL_SendToRobotAndGetResponse(cmd, response, sizeof(response));
		SHELL_SendToRobotAndGetResponse((const unsigned char*)"app manualdrive", response, sizeof(response));
		McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
	}
	return true;
}

/* Lets the robot turn to a specific angle and then proceed straight with its previous movement */
bool Robo_Wrapper_Nav_Turn(int16_t angle){
	unsigned char cmd[BUF_SIZE] = RS_CMD_TURN_PREFIX;
	unsigned char response[128];
	unsigned char angleA[5] = "";
	McuUtility_Num16sToStr(angleA, sizeof(angleA), angle);
	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)angleA);
	SHELL_SendToRobotAndGetResponse(cmd, response, sizeof(response));
	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
	return true;
}

/* Sets the movementspeed of the robot */
bool Robo_Wrapper_Nav_Move(int16_t speed){
	unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
	unsigned char speedA[5] = "";
	unsigned char response[128];
	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"incSpeed ");
	McuUtility_Num16sToStr(speedA, sizeof(speedA), speed);
	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)speedA);
	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)" ");
	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)speedA);
	SHELL_SendToRobotAndGetResponse(cmd, response, sizeof(response));
	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
	return true;
}

/* Stops the movement of the robot */
bool Robo_Wrapper_Nav_Stop(void){
	unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
	unsigned char response[128];
	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode stop");
	SHELL_SendToRobotAndGetResponse(cmd, response, sizeof(response));
	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
	return true;
}

bool Robo_Wrapper_Calibration(bool start){
	if(start){
		/* start calibration */
		unsigned char response[128];
		SHELL_SendToRobotAndGetResponse((const unsigned char*)"ref calib start", response, sizeof(response));
		McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
	}else{
		/* end calibration */
		unsigned char response[128];
		SHELL_SendToRobotAndGetResponse((const unsigned char*)"ref calib stop", response, sizeof(response));
		McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
	}
	return true;
}

bool Robo_Wrapper_GetCalibrationValues(LineSensorCalibData_t *data){
	unsigned char response[500];
	// set default values
	McuUtility_strcpy(data->state, sizeof(data->state), (unsigned char*)"n/a");
	McuUtility_strcpy(data->values, sizeof(data->values), (unsigned char*)"0 0 0 0 0 0");

	SHELL_SendToRobotAndGetResponse((const unsigned char*)"ref status", response, sizeof(response));
	/* get calibration values */
	getValueOfStatusResponse(response, (unsigned char*)"state", (unsigned char*)&data->state, 20);
	getValueOfStatusResponse(response, (unsigned char*)"calib val", (unsigned char*)&data->values, 50);

	return true;
}

bool Robo_Wrapper_GetBatteryVoltage(unsigned char *voltage){
	unsigned char response[200];
	// set default value
	McuUtility_strcpy(voltage, sizeof(voltage), (unsigned char*)"n/a");
	SHELL_SendToRobotAndGetResponse((const unsigned char*)"battery status", response, sizeof(response));
	/* get value */
	getValueOfStatusResponse(response, (unsigned char*)"Battery", (unsigned char*)voltage, 20);

	return true;
}

static bool getValueOfStatusResponse(unsigned char* response, const unsigned char* key, unsigned char* value, size_t valueStringLen){
	unsigned char *p;
	p = (unsigned char*)response;

	// skip first line (if the keyword would also appear in the headline, this is an issue)
	while(*p!='\n'){
		p++;
	}
	p+=1; // skip newline

	// find key
	int16_t pos = McuUtility_strFind(p, (unsigned char*)key);
	if(pos == -1){   // error string not found = -1
		ESP_LOGE(TAG, "Could not find key %s in response.", key);
		return false;
	}
	p+=pos;  // move pointer to found key

	// skip until colon
	while(true){
		if(*p==':'){
			p+=2;  // colon found, skip colon + 2 space and proceed
			break;
		}
		else if(*p=='\n'){
			ESP_LOGE(TAG, "Reached end of line while skipping colon after key %s.", key);
			return false;  // error, end of line
		}
		else{
			p++;  // check next character
		}
	}

	// extract value
	unsigned char extractedString[50] = "";
	uint8_t i = 0;
	while(true){
		if(*p == '\n'){
			break;   // end of value reached
		}
		else{
			extractedString[i++] = *p;
			p++;  // next char
		}
	}

	// copy to destination
	McuUtility_strcpy(value, sizeof(unsigned char) * valueStringLen, extractedString);

	return true;
}