/*
  CmdMessenger - library that provides command based messaging
  
	Permission is hereby granted, free of charge, to any person obtaining
	a copy of this software and associated documentation files (the
	"Software"), to deal in the Software without restriction, including
	without limitation the rights to use, copy, modify, merge, publish,
	distribute, sublicense, and/or sell copies of the Software, and to
	permit persons to whom the Software is furnished to do so, subject to
	the following conditions:

	The above copyright notice and this permission notice shall be
	included in all copies or substantial portions of the Software.

	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
	EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
	MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
	NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
	LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
	OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
	WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

    Initial Messenger Library - Thomas Ouellet Fredericks.
    CmdMessenger Version 1    - Neil Dudman.
    CmdMessenger Version 2    - Dreamcat4.
	CmdMessenger Version 3    - Thijs Elenbaas.
	  3.6  - Fixes
	       - Better compatibility between platforms
		   - Unit tests
	  3.5  - Fixes, speed improvements for Teensy
	  3.4  - Internal update
	  3.3  - Fixed warnings
	       - Some code optimization
	  3.2  - Small fixes and sending long argument support
	  3.1  - Added examples
	  3.0  - Bugfixes on 2.2
	       - Wait for acknowlegde
		   - Sending of common type arguments (float, int, char)
	       - Multi-argument commands
           - Escaping of special characters
           - Sending of binary data of any type (uses escaping)
 */

extern "C" {
#include <stdlib.h>
#include <stdarg.h>
}
#include <stdio.h>
#include <CmdMessenger.h>

#define _CMDMESSENGER_VERSION 3_6 // software version of this library

// **** Initialization **** 

/**
 * CmdMessenger constructor
 */
CmdMessenger::CmdMessenger(Stream &ccomms, const char fld_separator, const char cmd_separator, const char esc_character)
{
    init(ccomms,fld_separator,cmd_separator, esc_character);
}

/**
 * Enables printing newline after a sent command
 */
void CmdMessenger::init(Stream &ccomms, const char fld_separator, const char cmd_separator, const char esc_character)
{
    default_callback = NULL;
    comms            = &ccomms;
    print_newlines    = false;
    field_separator   = fld_separator;
    command_separator = cmd_separator;
    escape_character  = esc_character;
    bufferLength      = MESSENGERBUFFERSIZE;
    bufferLastIndex   = MESSENGERBUFFERSIZE -1;
    reset();

    default_callback  = NULL;
    for (int i = 0; i < MAXCALLBACKS; i++)
        callbackList[i] = NULL;

    pauseProcessing   = false;
}

/**
 * Resets the command buffer and message state
 */
void CmdMessenger::reset()
{
    bufferIndex = 0;
    current     = NULL;
    last        = NULL;
    dumped      = true;
}

/**
 * Enables printing newline after a sent command
 */
void CmdMessenger::printLfCr(bool addNewLine)
{
    print_newlines = addNewLine;
}

/**
 * Attaches an default function for commands that are not explicitly attached
 */
void CmdMessenger::attach(messengerCallbackFunction newFunction)
{
    default_callback = newFunction;
}

/**
 * Attaches a function to a command ID
 */
void CmdMessenger::attach(byte msgId, messengerCallbackFunction newFunction)
{
    if (msgId >= 0 && msgId < MAXCALLBACKS)
        callbackList[msgId] = newFunction;
}

// **** Command processing ****

/**
 * Feeds serial data in CmdMessenger
 */
void CmdMessenger::feedinSerialData()
{
    while ( !pauseProcessing && comms->available() )
	{	   
		// The Stream class has a readBytes() function that reads many bytes at once. On Teensy 2.0 and 3.0, readBytes() is optimized. 
		// Benchmarks about the incredible difference it makes: http://www.pjrc.com/teensy/benchmark_usb_serial_receive.html

		size_t bytesAvailable = min(comms->available(),MAXSTREAMBUFFERSIZE);
		comms->readBytes(streamBuffer, bytesAvailable); 
		
		// Process the bytes in the stream buffer, and handles dispatches callbacks, if commands are received
		for (size_t byteNo = 0; byteNo < bytesAvailable ; byteNo++) 
		{   
		    int messageState = processLine(streamBuffer[byteNo]);

			// If waiting for acknowledge command
			if ( messageState == kEndOfMessage ) 
			{
				handleMessage();
			}
		}
	}
}

/**
 * Processes bytes and determines message state
 */
uint8_t CmdMessenger::processLine(char serialChar)
{
    messageState = kProccesingMessage;
    //char serialChar = (char)serialByte;
    bool escaped = isEscaped(&serialChar,escape_character,&CmdlastChar);
	if((serialChar == command_separator) && !escaped) {
		commandBuffer[bufferIndex]=0;
		if(bufferIndex > 0) {
			messageState = kEndOfMessage;
			current = commandBuffer;
			CmdlastChar='\0';
		}
		reset();
	} else {
		commandBuffer[bufferIndex]=serialChar;
		bufferIndex++;
		if (bufferIndex >= bufferLastIndex) reset();
	}
    return messageState;
}

/**
 * Dispatches attached callbacks based on command
 */
void CmdMessenger::handleMessage()
{
    lastCommandId = readInt16Arg();
    // if command attached, we will call it
    if (lastCommandId >= 0 && lastCommandId < MAXCALLBACKS && ArgOk && callbackList[lastCommandId] != NULL)
        (*callbackList[lastCommandId])();
    else // If command not attached, call default callback (if attached)
        if (default_callback!=NULL) (*default_callback)();
}

/**
 * Waits for reply from sender or timeout before continuing
 */
bool CmdMessenger::blockedTillReply(unsigned long timeout, int ackCmdId)
{
    unsigned long time  = millis();
    unsigned long start = time;
    bool receivedAck    = false;
    while( (time - start ) < timeout && !receivedAck) {
        time = millis();
        receivedAck = CheckForAck(ackCmdId);
    }
    return receivedAck;
}

/**
 *   Loops as long data is available to determine if acknowledge has come in 
 */
bool CmdMessenger::CheckForAck(int AckCommand)
{
    while (  comms->available() ) {
		//Processes a byte and determines if an acknowlegde has come in
		int messageState = processLine(comms->read());
		if ( messageState == kEndOfMessage ) {
			int id = readInt16Arg();
			if (AckCommand==id && ArgOk) {
				return true;
			} else {
				return false;
			}
		}
		return false;
    }
    return false;
}

/**
 * Gets next argument. Returns true if an argument is available
 */
bool CmdMessenger::next()
{
    char * temppointer= NULL;
    // Currently, cmd messenger only supports 1 char for the field seperator
    switch (messageState) {
    case kProccesingMessage:
        return false;
    case kEndOfMessage:
        temppointer = commandBuffer;
        messageState = kProcessingArguments;
    default:
        if (dumped)
            current = split_r(temppointer,field_separator,&last);
        if (current != NULL) {
            dumped = true;
            return true;
        }
    }
    return false;
}

/**
 * Returns if an argument is available. Alias for next()
 */
bool CmdMessenger::available()
{
    return next();
}

/**
 * Returns if the latest argument is well formed. 
 */
bool CmdMessenger::isArgOk ()
{
	return ArgOk;
}

/**
 * Returns the CommandID of the current command
 */
uint8_t CmdMessenger::CommandID()
{
    return lastCommandId;
}

// ****  Command sending ****

/**
 * Send start of command. This makes it easy to send multiple arguments per command
 */
void CmdMessenger::sendCmdStart(int cmdId)
{
    if (!startCommand) {
		startCommand   = true;
		pauseProcessing = true;
		comms->print(cmdId);
	}
}

/**
 * Send an escaped command argument
 */
void CmdMessenger::sendCmdEscArg(char* arg)
{
    if (startCommand) {
        comms->print(field_separator);
        printEsc(arg);
    }
}

/**
 * Send formatted argument.
 *  Note that floating points are not supported and resulting string is limited to 128 chars
 */
void CmdMessenger::sendCmdfArg(char *fmt, ...)
{
	const int maxMessageSize = 128;
    if (startCommand) {
        char msg[maxMessageSize];
        va_list args;
        va_start (args, fmt );
        vsnprintf(msg, maxMessageSize, fmt, args);
        va_end (args);

        comms->print(field_separator);
        comms->print(msg);
    }
}

/**
 * Send double argument in scientific format.
 *  This will overcome the boundary of normal float sending which is limited to abs(f) <= MAXLONG
 */
void CmdMessenger::sendCmdSciArg (double arg, int n)
{
if (startCommand)
  {
	comms->print (field_separator);
	printSci (arg, n);
  }
}

/**
 * Send end of command
 */
bool CmdMessenger::sendCmdEnd(bool reqAc, int ackCmdId, int timeout)
{
    bool ackReply = false;
    if (startCommand) {
        comms->print(command_separator);
        if(print_newlines)
            comms->println(); // should append BOTH \r\n
        if (reqAc) {
            ackReply = blockedTillReply(timeout, ackCmdId);
        }
    }
    pauseProcessing = false;
    startCommand   = false;
    return ackReply;
}

/**
 * Send a command without arguments, with acknowledge
 */
bool CmdMessenger::sendCmd (int cmdId, bool reqAc, int ackCmdId)
{
    if (!startCommand) {
        sendCmdStart (cmdId);
        return sendCmdEnd (reqAc, ackCmdId, DEFAULT_TIMEOUT);
    }
    return false;
}

/**
 * Send a command without arguments, without acknowledge
 */
bool CmdMessenger::sendCmd (int cmdId)
{
    if (!startCommand) {
        sendCmdStart (cmdId);
        return sendCmdEnd (false, 1, DEFAULT_TIMEOUT);
    }
    return false;
}

// **** Command receiving ****

/**
 * Find next argument in command
 */
int CmdMessenger::findNext(char *str, char delim)
{
    int pos = 0;
    bool escaped = false;
	bool EOL = false;
    ArglastChar = '\0';
    while (true) {
        escaped = isEscaped(str,escape_character,&ArglastChar);
		EOL = (*str == '\0' && !escaped);
		if (EOL) { 
			return pos;
		}
        if (*str==field_separator && !escaped) {
            return pos;
        } else {
            str++;
            pos++;
        }
    }
    return pos;
}

/**
 * Read the next argument as int
 */
int16_t CmdMessenger::readInt16Arg()
{
    if (next()) {
        dumped = true;
		ArgOk  = true;
        return atoi(current);
    }
	ArgOk  = false;
    return 0;
}

/**
 * Read the next argument as int
 */
int32_t CmdMessenger::readInt32Arg()
{
    if (next()) {
        dumped = true;
		ArgOk  = true;
        return atol(current);
    }
	ArgOk  = false;
    return 0L;
}

/**
 * Read the next argument as bool
 */
bool CmdMessenger::readBoolArg()
{
	return (readInt16Arg()!=0)?true:false;   
}

/**
 * Read the next argument as char
 */
char CmdMessenger::readCharArg()
{
    if (next()) {
        dumped = true;
		ArgOk  = true;
        return current[0];
    }
	ArgOk  = false;
    return 0;
}

/**
 * Read the next argument as float
 */
float CmdMessenger::readFloatArg()
{
    if (next()) {
        dumped = true;
		ArgOk  = true;
        //return atof(current);
		return strtod(current,NULL);
    }
	ArgOk  = false;
    return 0;
}

/**
 * Read the next argument as double
 */
double CmdMessenger::readDoubleArg()
{
    if (next()) {
        dumped = true;
		ArgOk  = true;
        return strtod(current,NULL);
    }
	ArgOk  = false;
    return 0;
}

/**
 * Read next argument as string.
 * Note that the String is valid until the current command is replaced
 */
char* CmdMessenger::readStringArg()
{
    if (next()) {
        dumped = true;
		ArgOk  = true;
        return current;
    }
	ArgOk  = false;
    return '\0';
}

/**
 * Return next argument as a new string
 * Note that this is useful if the string needs to be persisted
 */
void CmdMessenger::copyStringArg(char *string, uint8_t size)
{
    if (next()) {
        dumped = true;
		ArgOk  = true;
        strlcpy(string,current,size);
    } else {
		ArgOk  = false;
        if ( size ) string[0] = '\0';
    }
}

/**
 * Compare the next argument with a string
 */
uint8_t CmdMessenger::compareStringArg(char *string)
{
    if (next()) {
        if ( strcmp(string,current) == 0 ) {
            dumped = true;
			ArgOk  = true;
            return 1;
        } else {
			ArgOk  = false;
            return 0;
        }
    }
	return 0;
}

// **** Escaping tools ****

/**
 * Unescapes a string
 * Note that this is done inline
 */
void CmdMessenger::unescape(char *fromChar)
{
    // Move unescaped characters right
    char *toChar = fromChar;
    while (*fromChar != '\0') {
        if (*fromChar==escape_character) {
            fromChar++;
        }
        *toChar++=*fromChar++;
    }
    // Pad string with \0 if string was shortened
    for (; toChar<fromChar; toChar++) {
        *toChar='\0';
    }
}

/**
 * Split string in different tokens, based on delimiter
 * Note that this is basically strtok_r, but with support for an escape character
 */
char* CmdMessenger::split_r(char *str, const char delim, char **nextp)
{
    char *ret;
    // if input null, this is not the first call, use the nextp pointer instead
    if (str == NULL) {
        str = *nextp;
    }
    // Strip leading delimiters
    while (findNext(str, delim)==0 && *str) {
        str++;
    }
    // If this is a \0 char, return null
    if (*str == '\0') {
        return NULL;
    }
    // Set start of return pointer to this position
    ret = str;
    // Find next delimiter
    str += findNext(str, delim);
    // and exchange this for a a \0 char. This will terminate the char
    if (*str) {
        *str++ = '\0';
    }
    // Set the next pointer to this char
    *nextp = str;
    // return current pointer
    return ret;
}

/**
 * Indicates if the current character is escaped
 */
bool CmdMessenger::isEscaped(char *currChar, const char escapeChar, char *lastChar)
{
    bool escaped;
    escaped   = (*lastChar==escapeChar);
    *lastChar = *currChar;

    // special case: the escape char has been escaped:
    if (*lastChar == escape_character && escaped) {
        *lastChar = '\0';
    }
    return escaped;
}

/**
 * Escape and print a string
 */
void CmdMessenger::printEsc(char *str)
{
    while (*str != '\0') {
        printEsc(*str++);
    }
}

/**
 * Escape and print a character
 */
void CmdMessenger::printEsc(char str)
{
    if (str==field_separator || str==command_separator || str==escape_character || str=='\0') {
        comms->print(escape_character);
    }
    comms->print(str);
}

/**
 * Print float and double in scientific format
 */
void CmdMessenger::printSci(double f, unsigned int digits)
{
  // handle sign
  if (f < 0.0)
  {
    Serial.print('-');
    f = -f;
  } 
  
  // handle infinite values
  if (isinf(f))
  {
    Serial.print("INF");
    return;
  }
  // handle Not a Number
  if (isnan(f)) 
  {
    Serial.print("NaN");
    return;
  }

  // max digits
  if (digits > 6) digits = 6;
  long multiplier = pow(10, digits);     // fix int => long

  int exponent;
  if (abs(f) < 10.0) { 
	exponent = 0;
  }	else {
	exponent = int(log10(f));
  }
  float g = f / pow(10, exponent);
  if ((g < 1.0) && (g != 0.0))      
  {
    g *= 10;
    exponent--;
  }
 
  long whole = long(g);                     // single digit
  long part = long((g-whole)*multiplier+0.5);  // # digits
  // Check for rounding above .99:
  if (part == 100) {
	  whole++;
	  part = 0;
  }
  char format[16];
  sprintf(format, "%%ld.%%0%dldE%%+d", digits);
  char output[16];
  sprintf(output,format, whole, part, exponent);
  comms->print(output);
}