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

#define RS_CMD_DRIVE_PREFIX "rs sendcmd 0x01 drive "
#define RS_CMD_TURN_PREFIX "rs sendcmd 0x01 turn "
#define BUF_SIZE 50

/* Sets the robot to automatic / manual mode */
bool Robo_Wrapper_SetMode(bool automatic){
	if(automatic){
		/* tbd */
	}else{
		/* set speed to zero */
		Robo_Wrapper_Nav_Move(0);
		/* set mode to speed */
		unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
		unsigned char response[128];
		McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode speed");
		SHELL_SendToESPAndGetResponse(cmd, 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_SendToESPAndGetResponse(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_SendToESPAndGetResponse(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_SendToESPAndGetResponse(cmd, response, sizeof(response));
	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
	return true;
}