#include "cmdInterpreter.h"

#define CMD_M_MAX_LENGTH	121		// 120 channels + 1 pwm value
#define CMD_N_MAX_LENGTH	240		// 120 channels + 120 pwm values


// Constructor
cmdInterpreter::cmdInterpreter() {
	this->prepareForNextCommand();
	this->debug = false;
}


/**
 * prepare everything for the next command
 */
void cmdInterpreter::prepareForNextCommand(void) {
	memset((void *) this->cmdBuffer, 0x00, CMD_BUFFER_LENGTH);
	this->cmdBuffer_ptr = this->cmdBuffer;
}



/**
 * analyse command
 */
return_code_t cmdInterpreter::interprete(pca9635 pwm_driver[], uint8_t cnt, uint8_t oe_pin) {
	uint8_t cmd_len = (uint8_t) strlen((char *) this->cmdBuffer);
	uint8_t *cmd_buf_pntr = this->cmdBuffer;
	cmd_buf_pntr++;        // skip command identifier
	return_code_t returncode = RETURN_ERROR;

	// check if all chars are valid hex chars
	while (*cmd_buf_pntr) {
		if(*cmd_buf_pntr == CMD_CR){
			*cmd_buf_pntr = 68;
		}

		if (!isxdigit(*cmd_buf_pntr)) {
			return RETURN_ERROR;
		}
		++cmd_buf_pntr;
	}
	cmd_buf_pntr = this->cmdBuffer;    // reset pointer

	switch(*cmd_buf_pntr) {
		// set single channel PWM
		case 'S': {
			uint8_t data[2] = {0}; //parameter 0: channel, parameter 1: pwm
			returncode = readParameters(cmd_buf_pntr, cmd_len, data, 2);

			if (returncode != RETURN_CR) {
				return returncode;
			}

			if (data[0] >= 120) {
				return RETURN_ERROR;
			}

			char s[64] = {'\0'};
			if (this->debug) {
				sprintf(s, " set channel %d to %d", data[0], data[1]);
				doubleS.writeSerial(s);
			}
		
			uint8_t dev = data[0] / 10;
			uint8_t port = data[0] % 10;

			if (this->debug) {
				sprintf(s, " (dev: %d ch %d)", dev, port);
				doubleS.writeSerial(s);
			}
			if (dev < cnt && pwm_driver[dev].isAvailable()) {
				if (pwm_driver[dev].set_led_pwm(port, data[1])) {
					if (this->debug)
						doubleS.writeSerial(" channel is set!");
				} else {
					if (this->debug)
						doubleS.writeSerial(" error while setting channel!");
					return RETURN_ERROR;
				}
			} else {
				if (this->debug)
					doubleS.writeSerial(" dev not available!");
				return RETURN_ERROR;
			}
			break;
		}


		// read single channel PWM
		case 'R': {
			uint8_t data[1] = {0}; //parameter 0: channel
			returncode = readParameters(cmd_buf_pntr, cmd_len, data, 1);

			if (returncode != RETURN_CR) {
				return returncode;
			}

			if (data[0] >= 120) {
				return RETURN_ERROR;
			}

			uint8_t dev = data[0] / 10;
			uint8_t port = data[0] % 10;

			
		
			if (dev < cnt && pwm_driver[dev].isAvailable()) {
				uint8_t pwm = pwm_driver[dev].getPWMValue(port);
				
				char s[64] = {'\0'};
				sprintf(s, " pwm value of channel %d is %d (hex: %X)", data[0],pwm, pwm);
				doubleS.writeSerial(s);
				
			}
			break;
		}
		
		// set the output enable (1 means enable, 0 means disalble outputs)
		case 'O': {
			uint8_t data[1] = {0}; //parameter 0: value
			returncode = readParameters(cmd_buf_pntr, cmd_len, data, 1);
			
			if (returncode != RETURN_CR) {
				return returncode;
			}

			digitalWrite(oe_pin, !data[0]);

			break;
		}

		// set the blinking interval
		case 'I': {
			uint8_t data[1] = {0}; //parameter 0: value
			returncode = readParameters(cmd_buf_pntr, cmd_len, data, 1);
			
			if (returncode != RETURN_CR) {
				return returncode;
			}
			
			char s[64] = {'\0'};
			if (this->debug) {
				sprintf(s, " set group interval to : %d (%dms)", data[0], (416*(data[0]+1))/10);
				doubleS.writeSerial(s);
			}
			for (uint8_t i = 0; i < cnt; i++) {
				if (pwm_driver[i].isAvailable()) {
					if (pwm_driver[i].set_group_blinking(data[0])) {
						if (this->debug)
							doubleS.writeSerial(" set!");
					} else {
						if (this->debug)
							doubleS.writeSerial(" error while setting!");
						return RETURN_ERROR;
					}
				}
			}
			
			
			break;
		}

		// set the blinking dutycycle
		case 'i': {
			uint8_t data[1] = {0}; //parameter 0: value
			returncode = readParameters(cmd_buf_pntr, cmd_len, data, 1);
			
			if (returncode != RETURN_CR) {
				return returncode;
			}
			
			char s[64] = {'\0'};
			if (this->debug) {
				sprintf(s, " set group duty to : %d", data[0]);
				doubleS.writeSerial(s);
			}
			for (uint8_t i = 0; i < cnt; i++) {
				if (pwm_driver[i].isAvailable()) {
					if (pwm_driver[i].set_group_dimming(data[0])) {
						if (this->debug)
							doubleS.writeSerial(" set!");
					} else {
						if (this->debug)
							doubleS.writeSerial(" error while setting!");
						return RETURN_ERROR;
					}
				}
			}
			
			
			break;
		}

		// enable/disable the group blinking for a given channel
		case 'G': {
			uint8_t data[2] = {0}; //parameter 0: channel, parameter 1: bool
			returncode = readParameters(cmd_buf_pntr, cmd_len, data, 2);

			if (returncode != RETURN_CR) {
				return returncode;
			}

			if (data[0] >= 120) {
				return RETURN_ERROR;
			}

			uint8_t dev = data[0] / 10;
			uint8_t port = data[0] % 10;

			if (data[1] > 1) {
				data[1] = 1;
			}
			
			char s[64] = {'\0'};
			if (this->debug) {
				sprintf(s, " set group blinking of channel %d (dev: %d port: %d) to %d", data[0], dev, port, data[1]);
				doubleS.writeSerial(s);
			}
			if (pwm_driver[dev].isAvailable()) {
				if (pwm_driver[dev].set_led_mode(port, data[1]+2)) {
					if (this->debug)
						doubleS.writeSerial(" set!");
				} else {
					if (this->debug)
						doubleS.writeSerial(" error while setting!");
					return RETURN_ERROR;
				}
			} else {
				if (this->debug)
					doubleS.writeSerial(" dev not available!");
				return RETURN_ERROR;
			}
			
			
			break;
		}


		// set pwm value to multiple pwm channels
		case 'm': {
			uint8_t data[CMD_M_MAX_LENGTH] = {0}; //parameter n-1: channels, parameter last: pwm
			uint8_t length = CMD_M_MAX_LENGTH;
			char tmp[50];
			returncode = readParametersWithLength(cmd_buf_pntr, cmd_len, data, &length, MODE_SINGLE_VALUE);

			if (returncode != RETURN_CR) {
				return returncode;
			}

			if (this->debug) {
				doubleS.writeSerial(" Data: ");
				PrintHex8(tmp, data, length);
				doubleS.writeSerial(tmp);
				sprintf(tmp, " array length: %d", length);
				doubleS.writeSerial(tmp);
			}

			// sort all channels
			boolean change_pwm[12*10] = {false};
			uint8_t new_pwm[12*10] = {0};
			for (uint8_t i = 0; i < data[0]; i++) {
				uint8_t channel = data[i+1];
				if (!pwm_driver[channel/10].isAvailable()) {
					return RETURN_ERROR;
				}
				new_pwm[channel] = data[length-1];
				change_pwm[channel] = true;
			}

			
			for (uint8_t i = 0; i < cnt; i++) {
				int8_t channel_start = -1;
				int8_t channel_end = -1;
				//find first and last change
				for (uint8_t j = 0; j < 10; j++) {
					if (change_pwm[i*10+j]) {
						if (channel_start == -1) 
							channel_start = j;
						channel_end = j;
					}
				}

				// if no change for current device jump to next device
				if (channel_start == -1 && channel_end == -1) {
					continue;
				}
				if (this->debug) {
					sprintf(tmp, "\rdev: %d changes: ", i);
					doubleS.writeSerial(tmp);
					PrintHex8(tmp, &change_pwm[i*10], 10);
					doubleS.writeSerial(tmp);

					sprintf(tmp, " start: %d end %d", channel_start, channel_end);
					doubleS.writeSerial(tmp);
					
					doubleS.writeSerial("\r        values: ");
					PrintHex8(tmp, &new_pwm[i*10], 10);
					doubleS.writeSerial(tmp);
				}
				// read current values from devices (drivers internal copy)
				for (uint8_t j = channel_start; j <= channel_end; j++) {
					if (!change_pwm[i*10+j]) {
						new_pwm[i*10+j] = pwm_driver[i].getPWMValue(j);
					}
				}
				if (this->debug) {
					doubleS.writeSerial("\rfilled  values: ");
					PrintHex8(tmp, &new_pwm[i*10], 10);
					doubleS.writeSerial(tmp);
				}

				//write block to current device
				boolean ret = pwm_driver[i].set_all_led_pwm(channel_start, channel_end-channel_start+1, &new_pwm[i*10+channel_start]);
				if (!ret) {
					return RETURN_ERROR;
				}
			}
			
			break;
		}


		// set pwm values to pwm channels
		case 'n': {
			uint8_t data[CMD_N_MAX_LENGTH] = {0}; //parameters: n channels, n pwm values
			uint8_t length = CMD_N_MAX_LENGTH;
			char tmp[50];
			returncode = readParametersWithLength(cmd_buf_pntr, cmd_len, data, &length, MODE_MULTIPLE_VALUE);

			if (returncode != RETURN_CR) {
				return returncode;
			}

			if (this->debug) {
				doubleS.writeSerial(" Data: ");
				PrintHex8(tmp, data, length);
				doubleS.writeSerial(tmp);
				sprintf(tmp, " array length: %d", length);
				doubleS.writeSerial(tmp);
			}

			// sort all channels
			boolean change_pwm[12*10] = {false};
			uint8_t new_pwm[12*10] = {0};
			for (uint8_t i = 0; i < data[0]; i++) {
				uint8_t channel = data[i+1];
				if (!pwm_driver[channel/10].isAvailable()) {
					return RETURN_ERROR;
				}
				new_pwm[channel] = data[i+1+data[0]];
				change_pwm[channel] = true;
			}

			
			for (uint8_t i = 0; i < cnt; i++) {
				int8_t channel_start = -1;
				int8_t channel_end = -1;
				//find first and last change
				for (uint8_t j = 0; j < 10; j++) {
					if (change_pwm[i*10+j]) {
						if (channel_start == -1) 
							channel_start = j;
						channel_end = j;
					}
				}

				// if no change for current device jump to next device
				if (channel_start == -1 && channel_end == -1) {
					continue;
				}
				if (this->debug) {
					sprintf(tmp, "\rdev: %d changes: ", i);
					doubleS.writeSerial(tmp);
					PrintHex8(tmp, &change_pwm[i*10], 10);
					doubleS.writeSerial(tmp);

					sprintf(tmp, " start: %d end %d", channel_start, channel_end);
					doubleS.writeSerial(tmp);
					
					doubleS.writeSerial("\r        values: ");
					PrintHex8(tmp, &new_pwm[i*10], 10);
					doubleS.writeSerial(tmp);
				}
				// read current values from devices (drivers internal copy)
				for (uint8_t j = channel_start; j <= channel_end; j++) {
					if (!change_pwm[i*10+j]) {
						new_pwm[i*10+j] = pwm_driver[i].getPWMValue(j);
					}
				}
				if (this->debug) {
					doubleS.writeSerial("\rfilled  values: ");
					PrintHex8(tmp, &new_pwm[i*10], 10);
					doubleS.writeSerial(tmp);
				}

				//write block to current device
				boolean ret = pwm_driver[i].set_all_led_pwm(channel_start, channel_end-channel_start+1, &new_pwm[i*10+channel_start]);
				if (!ret) {
					return RETURN_ERROR;
				}
			}
			
			break;
		}


		// set group blinking for multiple channels
		case 'g': {
			uint8_t data[CMD_N_MAX_LENGTH] = {0}; //parameter: n channels, n boolean group blinking
			uint8_t length = CMD_N_MAX_LENGTH;
			char tmp[50];
			returncode = readParametersWithLength(cmd_buf_pntr, cmd_len, data, &length, MODE_MULTIPLE_VALUE);

			if (returncode != RETURN_CR) {
				return returncode;
			}

			if (this->debug) {
				doubleS.writeSerial(" Data: ");
				PrintHex8(tmp, data, length);
				doubleS.writeSerial(tmp);
				sprintf(tmp, " array length: %d", length);
				doubleS.writeSerial(tmp);
			}

			// sort all channels
			uint8_t new_mode[12][10] = {0};
			uint8_t new_led[12][10] = {0};
			uint8_t change_cnt[12] = {0};
			for (uint8_t i = 0; i < data[0]; i++) {
				uint8_t channel = data[i+1];
				if (!pwm_driver[channel/10].isAvailable()) {
					return RETURN_ERROR;
				}
				new_mode[channel/10][change_cnt[channel/10]] = data[i+1+data[0]] + 2;
				new_led[channel/10][change_cnt[channel/10]] = channel%10;
				change_cnt[channel/10]++;	
			}

			
			for (uint8_t i = 0; i < cnt; i++) {
				// if no change for current device jump to next device
				if (change_cnt[i] == 0) {
					continue;
				}
				if (this->debug) {
					sprintf(tmp, "\rdev: %d leds: ", i);
					doubleS.writeSerial(tmp);
					PrintHex8(tmp, new_led[i], 10);
					doubleS.writeSerial(tmp);

					sprintf(tmp, " cnt: %d\r     values: ", change_cnt[i]);
					doubleS.writeSerial(tmp);
					PrintHex8(tmp, new_mode[i], 10);
					doubleS.writeSerial(tmp);
				}
				
				//write block to current device
				boolean ret = pwm_driver[i].set_all_led_modes(change_cnt[i], new_led[i], new_mode[i]);
				if (!ret) {
					return RETURN_ERROR;
				}
			}
			
			break;
		}


		// dump all available devices
		case 'L': {
			returncode = readParameters(cmd_buf_pntr, cmd_len, NULL, 0);

			if (returncode != RETURN_CR) {
				return returncode;
			}
			
			char s[64] = {'\0'};

			for (uint8_t i = 0; i < cnt; i++) {
				if (pwm_driver[i].isAvailable()) {
					sprintf(s, "dev %d available (channel: %d to: %d hex: %.2X to %.2X)\r\n", i, i*10, i*10+9, i*10, i*10+9);
					doubleS.writeSerial(s);
				}
			}
			break;
		}
		

		// toogle debug messages
		case 'D': {
			returncode = readParameters(cmd_buf_pntr, cmd_len, NULL, 0);

			if (returncode != RETURN_CR) {
				return returncode;
			}

			this->debug = !this->debug;
			break;
		}
		
		
		default: {
			// end with error on unknown commands
			return RETURN_ERROR;
		}	
	}
	return RETURN_CR;
}


return_code_t cmdInterpreter::readParameters(uint8_t *cmd_buf_pntr, uint8_t cmd_len, uint8_t parameters[], uint8_t num_parameters) {
	if (cmd_len != (2+2*num_parameters)) {
		return RETURN_ERROR;
	}

	for (uint8_t i = 0; i < num_parameters; i++) {
		parameters[i] = 0;
		parameters[i] = ascii2byte(++cmd_buf_pntr) << 4;
		parameters[i] += ascii2byte(++cmd_buf_pntr);
		
	}
	return RETURN_CR;
}

/**
 * this commands reads from a array and checks if the first hex block is a length that matches the rest of the data
 * all parameters from string are converted to byte and stored in paramters array.
 * the num_parameter pointer reads the maximal length of the parameters array and contains the written paraeters to parameters after the method.
 * m controls the meode this method calculated the amount of expected parameters:
 * - MODE_SINGLE_VALUE means there is an length field, length*bytes and a value field
 * - MODE_MULTUPLE_VALUE means there is an length field, length*bytes and again length*value fields
 */
return_code_t cmdInterpreter::readParametersWithLength(uint8_t *cmd_buf_pntr, uint8_t cmd_len, uint8_t parameters[], uint8_t *num_parameters, cmdInterpreter::interpreteMode_t m) {
	//check if length is available
	if (cmd_len < (2+2)) {
		return RETURN_ERROR;
	}
	parameters[0] = 0;
	parameters[0] = ascii2byte(++cmd_buf_pntr) << 4;
	parameters[0] += ascii2byte(++cmd_buf_pntr);

	//check if length and command has the same length
	uint8_t dataLength = 0;
	switch(m) {
		case MODE_SINGLE_VALUE:
			dataLength = 2*parameters[0] + 2;
			break;
		case MODE_MULTIPLE_VALUE:
			dataLength = 2*parameters[0] + 2*parameters[0];
			break;
		default:
			break;
	}
	
	if (cmd_len != (2+2+dataLength) || (dataLength/2+1) > *num_parameters) {
		return RETURN_ERROR;
	}

	//interprete parameters from string
	for (uint8_t i = 1; i < dataLength+1; i++) {
		parameters[i] = 0;
		parameters[i] = ascii2byte(++cmd_buf_pntr) << 4;
		parameters[i] += ascii2byte(++cmd_buf_pntr);
	}
	*num_parameters = dataLength/2+1;
	return RETURN_CR;
}


/**
 * add to buffer and check if end of command is detected
 * will return true if a end of command is detected
 */
boolean cmdInterpreter::addToBuffer(uint8_t d) {
	*this->cmdBuffer_ptr = d;
	if (d == CMD_CR) {	//check if the last character is the end character
		if (this->debug) {
			doubleS.writeSerial("Buffer: ");
			char tmp[50];
			PrintHex8(tmp, this->cmdBuffer, 20);
			doubleS.writeSerial(tmp);
		}
		return true;
	} else {	//if not, move pointer forward to next array address
		(this->cmdBuffer_ptr)++;
	}
	return false;
}


/**
 * prints 8-bit data in hex with leading zeroes
 */
void PrintHex8(char *dstptr, uint8_t *srcdata, uint8_t length) {
	for (int i=0; i<length; i++) {
		sprintf(dstptr+(i*2), "%.2X",srcdata[i]);
	}
}

/**
 * prints boolean data
 */
void PrintHex8(char *dstptr, boolean *srcdata, uint8_t length) {
	for (int i=0; i<length; i++) {
		sprintf(dstptr+(i*2), " %.1X",srcdata[i]);
	}
}


/**
 * takes a pointer to a string and convert the current characters to a binary
 */
uint8_t ascii2byte(const uint8_t *val) {
	uint8_t temp = *val;

	if (temp > 0x60)
		temp -= 0x27;        // convert chars a-f
	else if (temp > 0x40)
		temp -= 0x07;        // convert chars A-F
	temp -= 0x30;        // convert chars 0-9

	return (uint8_t) (temp & 0x0F);
}