/*
 * sonardyneCommand.cpp
 *
 *  Created on: Jan 20, 2015
 *      Author: emartin
 */

#include "sonardyneCommand.h"

sonardyneCommand::sonardyneCommand() {

}

void sonardyneCommand::encodePacket(char* packet, int * packetsize,
		int cmdType) {
	*packetsize = 0; //reset the length
	SCL_RECORD_HEADER hdr;
	hdr.address = 0;
	hdr.cmdtype = cmdType;
	encodePacket(packet,packetsize,&hdr);

}

int sonardyneCommand::encodePacket(char* packet, int * packetsize,
		SCL_RECORD_HEADER * hdr) {
	*packetsize = 0; // reset the length
	encodeHeader(packet,packetsize, hdr);
	if (*packetsize>0) {
		*packetsize += sprintf(packet+*packetsize,"\r");
	}
	return 0;
}

void sonardyneCommand::encodePacket(char* packet, int * packetsize, SCL_RECORD_HEADER *hdr,
		SCL_SMS_INT* sms) {
    // assume a valid sms struct
	*packetsize = 0; // reset the length

	//encode header
	encodeHeader(packet,packetsize,hdr);
	if (packetsize == 0) return; // header encoding failed.

//	encode flags
	if (sms->wakeupTone != 0){
		*packetsize += sprintf(packet+*packetsize,",W%1i",sms->wakeupTone);
	}
	//if (sms->commandNumber !=0){
		*packetsize += sprintf(packet+*packetsize,",C%1i",sms->commandNumber);
	//}
	*packetsize += sprintf(packet+*packetsize, ",P%1i,A%1i,R%1i,B%1i",
    		sms->portNumber,sms->subCode,sms->rangeData,sms->bufferMsg);

	*packetsize += sprintf(packet+*packetsize, "|");
	if (packetsize == 0) return; // flag encoding failed.

    //encode sms
    strncpy(packet+*packetsize, sms->sms, sms->smsLength+1);
    *packetsize += sms->smsLength;
    if (packetsize == 0) return; // sms encoding failed.

    //end message
    *packetsize += sprintf(packet+*packetsize,"\r");

}

void sonardyneCommand::encodePacket(char* packet, int * packetsize,
		SCL_RECORD_HEADER *hdr, SCL_MEASURESLANTRANGE_INT* msr) {
	// assume a valid struct
	*packetsize = 0; // reset the length

	//encode header
	encodeHeader(packet,packetsize,hdr);
	if (packetsize == 0) return; // header encoding failed.

	//interrogationSignalString
	strncpy(packet+*packetsize, msr->interrogationSignalString,msr->interrogationSignalStringLength+1);
	*packetsize+= msr->interrogationSignalStringLength;
	if (packetsize == 0) return; // string encoding failed.

	// encode wakeup tone
	*packetsize += sprintf(packet+*packetsize,";W%1i,", msr->wakeupTone);
	if (packetsize == 0) return; // wakeup encoding failed.

	// encode addresses (must check if they are set)
	// channel 1
	if (msr->channel1DescriptorLength>0) {
		strncpy(packet+*packetsize,msr->channel1Descriptor, msr->channel1DescriptorLength+1);
		*packetsize+= msr->channel1DescriptorLength;
	}
	else {
		*packetsize = 0;// you must interrogate at least one address
		return;
	}

	// channel 2
	if (msr->channel2DescriptorLength>0) {
		*packetsize += sprintf(packet+*packetsize,",");
		strncpy(packet+*packetsize,msr->channel2Descriptor, msr->channel2DescriptorLength+1);
		*packetsize += msr->channel2DescriptorLength;

	} else {
		*packetsize += sprintf(packet+*packetsize,"\r");
		return; // its ok to not have all set.
	}

	// channel 3
	if (msr->channel3DescriptorLength>0) {
		*packetsize += sprintf(packet+*packetsize,",");
		strncpy(packet+*packetsize,msr->channel3Descriptor, msr->channel3DescriptorLength+1);
		*packetsize += msr->channel3DescriptorLength;

	} else {
		*packetsize += sprintf(packet+*packetsize,"\r");
		return; // its ok to not have all set.
	}

	// channel 4
	if (msr->channel4DescriptorLength>0) {
		*packetsize += sprintf(packet+*packetsize,",");
		strncpy(packet+*packetsize,msr->channel4Descriptor, msr->channel4DescriptorLength+1);
		*packetsize += msr->channel4DescriptorLength;

	}; // its ok to not have all set.

	*packetsize += sprintf(packet+*packetsize,"\r");
}


void sonardyneCommand::decodeHeader(char* packet, int * packetsize,
		SCL_RECORD_HEADER* header) {
	//assume valid packet.
	header->address = 0;
	// first decode type.
	if (strncmp(packet,"FV:",3)==0) {
		header->cmdtype = CMD_VERSION;
		//fv is internal only, set no address.
		return;
	}
	else if (strncmp(packet, "BV:",3)==0) {
		header->cmdtype = CMD_BOOTLOADER;
		// bv is internal only, set no address.
		return;
	}
	else if (strncmp(packet, "FS:",3)==0) {
		header->cmdtype = CMD_FIXEDSTATUS;
		// determine address (might be local)
		if (sscanf(packet,"FS:%04i", &(header->address))!=1) {
			header->address = 0;
		}
		return;
	}
	else if (strncmp(packet, "VS:",3)==0) {
		header->cmdtype = CMD_VOLATILESTATUS;
		// determine address (might be local)
		if (sscanf(packet,"VS:%04i", &(header->address))!=1) {
			header->address = 0;
		}
		return;
	}
	else if (strncmp(packet, "CS:",3)==0) {
		header->cmdtype = CMD_CONFIGSTATUS;
		// determine address (might be local)
		if (sscanf(packet,"CS:%04i", &(header->address))!=1) {
			header->address = 0;
		}
		return;
	}
	else if (strncmp(packet, "MR:",3)==0) {
		header->cmdtype = CMD_MEASURERANGE;
		// determine address (might be local)
		if (sscanf(packet,"MR:%04i", &(header->address))!=1) {
			header->address = 0;
		}
		return;
	}
	else if (strncmp(packet, "MSR:",4)==0) {
		header->cmdtype = CMD_MEASURESLANTRANGE;
		return;
	}
	else if (strncmp(packet, "SMS:",4)==0) {
		header->cmdtype = CMD_SMS;
		if (sscanf(packet,"SMS:%04i", &(header->address))!=1) {
					header->address = 0;
		}
		return;
	}
	else {
		header->cmdtype = CMD_UNKNOWN;

	}

}


void sonardyneCommand::decodeSMS(char* packet, int * packetsize,
		SCL_SMS_RESPONSE* smsResponse) {
	// assume we get the whole reply, without the "<" or ">"
	// we also assume a correct header has been given.

	//set the replycode to unknown
	smsResponse->smsReplyCode = SMS_UNKNOWN;

	//first just see if this is a failed sms message
	if (strncmp(packet,"SMS:FAILED",10) ==0 ) {
		smsResponse->smsReplyCode = SMS_FAILED;
		return;
	}

	//parse the header again we need the address.
	SCL_RECORD_HEADER hdr;
	decodeHeader(packet, packetsize, &hdr);

	// find the start of the SMS
		int SMSstart = strcspn(packet,"|");
		if (SMSstart < *packetsize) { //make sure we found it at all.
			SMSstart+=1;
			strcpy(smsResponse->sms,packet+SMSstart);
			smsResponse->smslen = strlen(smsResponse->sms);
			//set status to OK
			smsResponse->smsReplyCode = SMS_SMS;
		}



	int flaglen;
	int flagstart = 8; // SMS:1001

	//test for a comma at flags
	char flag[32];

	if (packet[flagstart]==',') {// There are at least one flag
		flagstart++; // advance and then loop on options.

		while(flagstart != SMSstart && flagstart < *packetsize) {
			//process the flag at hand
			flaglen = strcspn(packet+flagstart,",|");

			strncpy(flag,packet+flagstart,flaglen);
			flag[flaglen+1] = '\0';
			if (strncmp(flag,"ACK",3)==0) {
				smsResponse->smsReplyCode = SMS_ACK;
			}
			else if (strncmp(flag,"NO_REPLY",8)==0) {
				smsResponse->smsReplyCode = SMS_NO_REPLY;
			}

			else if (strncmp(flag,"OK",2)==0) {
				smsResponse->smsReplyCode = SMS_OK;
			}
			else if (strncmp(flag,"R",1)==0) {
			    sscanf(flag,"R%li",&(smsResponse->rangeTime));
			}

			flagstart += flaglen+2;
		}
	}
}

void sonardyneCommand::decodeMeasureRange(char* packet,
		int * packetsize, SCL_MEASURERANGE_RESPONSE* mrResponse) {
	//placeholder // not required
}

void sonardyneCommand::decodeMeasureSlantRange(char* packet,
		int * packetsize,
		SCL_MEASURESLANTRANGE_RESPONSE* msrResponse) {
	//placeholder // not required
}


sonardyneCommand::~sonardyneCommand() {

}

void sonardyneCommand::encodeHeader(char * packet, int *packetsize, SCL_RECORD_HEADER* hdr) {
switch (hdr->cmdtype) {
	case sonardyneCommand::CMD_VERSION:  // Firmware and protocol version
		if (hdr->address >0)
			*packetsize =  sprintf(packet, "FV:%04i", hdr->address);
		else
			*packetsize =  sprintf(packet, "FV");
		break;
	case sonardyneCommand::CMD_BOOTLOADER: // Bootloader Version
		if (hdr->address >0)
			*packetsize =  sprintf(packet, "BV:%04i", hdr->address);
		else
			*packetsize =  sprintf(packet, "BV");
		break;
	case sonardyneCommand::CMD_FIXEDSTATUS: // fixed status request
		if (hdr->address >0)
			*packetsize =  sprintf(packet, "FS:%04i", hdr->address);
		else
			*packetsize =  sprintf(packet, "FS");
		break;
	case sonardyneCommand::CMD_VOLATILESTATUS: // volatile status request
		if (hdr->address >0)
			*packetsize =  sprintf(packet, "VS:%04i", hdr->address);
		else
			*packetsize =  sprintf(packet, "VS");
		break;
	case sonardyneCommand::CMD_CONFIGSTATUS: // configuration status request
		if (hdr->address >0)
			*packetsize =  sprintf(packet, "CS:%04i", hdr->address);
		else
			*packetsize =  sprintf(packet, "CS");
		break;
	case sonardyneCommand::CMD_MEASURERANGE: // measure range to target
		if (hdr->address >0)
			*packetsize =  sprintf(packet, "MR:%04i", hdr->address);
		break; //need address
	case sonardyneCommand::CMD_MEASURESLANTRANGE: // measure slant range to target
		*packetsize =  sprintf(packet, "MSR:");
		break;
	case sonardyneCommand::CMD_SMS: // sonardyne messaging service message
		if (hdr->address >0)
			*packetsize =  sprintf(packet, "SMS:%04i", hdr->address);
		break; //need address
		// do nothing, wrong constructor.
	}
}

void sonardyneCommand::encodeNMEAMsg(char* msg, int* msgsize,
		SCL_NMEA_HEADER_RECORD* hdr, SCL_SAN_RECORD* san) {
	//reset message size
	*msgsize = 0;
	//encode header
	encodeNMEAMsgHeader(msg,msgsize,hdr);
	if (*msgsize <= 0) return;

	//encode the rest of the data.
	*msgsize += sprintf(msg+*msgsize,",%.4f,%.4f,%.4f,%.2f",
			san->wfTime,san->wfLat,san->wfLong,san->wfSlant);

	if (*msgsize <= 0) return;

	//generate the checksum
	unsigned int checksum = 0;
	generateChecksum(msg,msgsize,&checksum);

	//add the checksum
	*msgsize += sprintf(msg+*msgsize,"*%02X",checksum);

	return;
	//encode header
}

void sonardyneCommand::encodeNMEAMsg(char* msg, int* msgsize,
		SCL_NMEA_HEADER_RECORD* hdr, SCL_STATUS_RECORD* status) {
	//reset message size
	*msgsize = 0;

	//encode header
	encodeNMEAMsgHeader(msg,msgsize,hdr);
	if (*msgsize <= 0) return;

	//encode the rest of the data.
	*msgsize += sprintf(msg+*msgsize,",%.4f,%.4f,%.3f,%.2f,%.2f,%.2f,%.2f,%u,%u",
			status->auvLat,status->auvLong,status->auvHeading,status->auvDepth,
			status->auvAlt,status->auvVolt,status->auvHumid,status->auvBehavior,status->auvAbort);

	//generate the checksum
	unsigned int checksum =0;
	generateChecksum(msg,msgsize,&checksum);

	//add the checksum
	*msgsize += sprintf(msg+*msgsize,"*%02X",checksum);

	return;
}

void sonardyneCommand::encodeNMEAMsg(char* msg, int* msgsize,
		SCL_NMEA_HEADER_RECORD* hdr, SCL_CMD_RECORD* cmd) {
	//reset message size
	*msgsize = 0;

	//encode header
	encodeNMEAMsgHeader(msg,msgsize,hdr);
	if (*msgsize <= 0) return;

	//encode the rest of the data.
	*msgsize += sprintf(msg+*msgsize,",%1X",cmd->cmd);

	//generate checksum
	unsigned int checksum = 0;
	generateChecksum(msg,msgsize,&checksum);

	//add the checksum
	*msgsize += sprintf(msg+*msgsize,"*%02X",checksum);

	return;
}

void sonardyneCommand::decodeNMEAMsgHeader(char* msg, int* msgsize,
		SCL_NMEA_HEADER_RECORD* hdr) {
	//again we're assuming a well formed message.
	char lmsg[SCL_STRINGMAX];
	strcpy(lmsg,msg);

	enum hdrposition {
		posTalkerID = 1,
		posType = 3,
		posProtocolVersion = 7
	};
	//talker id
	if (strncmp(lmsg+posTalkerID,"SF",2) == 0)
		hdr->talkerID = TID_SURFACE;
	else if (strncmp(lmsg+posTalkerID,"M1",2) == 0)
		hdr->talkerID = TID_MAPPER1;
	else if (strncmp(lmsg+posTalkerID,"M2",2) == 0)
		hdr->talkerID = TID_MAPPER2;
	else
		hdr->talkerID = TID_UNKNOWN;

	// message type
	if (strncmp(msg+posType,"SAN",3) == 0)
				hdr->msgType = MSG_SAN;
	else if (strncmp(lmsg+posType,"STA",3) == 0)
				hdr->msgType = MSG_STA;
	else if (strncmp(lmsg+posType,"CMD",3) == 0)
				hdr->msgType = MSG_CMD;
	else
		hdr->msgType = MSG_UNKNOWN;

	// protocol version

	char *tok;
	tok = strtok(lmsg+posProtocolVersion,",");
	sscanf(tok,"%u",&(hdr->version));

	tok = strtok(NULL, ",*");
	sscanf(tok,"%lu",&(hdr->ts_sec));

	tok = strtok(NULL, ",*");
	sscanf(tok,"%lu",&(hdr->ts_msec));

	tok = strtok(NULL, ",*");
	hdr->frontLen = tok-lmsg;


	return;

}

void sonardyneCommand::decodeNMEAMsgContent(char* msg, int* msgsize,
		SCL_SAN_RECORD* san) {
	SCL_NMEA_HEADER_RECORD hdr;
	char lmsg[SCL_STRINGMAX];
	strcpy(lmsg,msg);


	decodeNMEAMsgHeader(msg,msgsize,&hdr);

	char* tok;// = lmsg+hdr.frontLen;
	tok = strtok(lmsg+hdr.frontLen,",*");
	sscanf(tok,"%f",&(san->wfTime));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(san->wfLat));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(san->wfLong));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(san->wfSlant));
}

void sonardyneCommand::decodeNMEAMsgContent(char* msg, int* msgsize,
		SCL_STATUS_RECORD* status) {
	SCL_NMEA_HEADER_RECORD hdr;
	char lmsg[SCL_STRINGMAX];
	strcpy(lmsg,msg);

	decodeNMEAMsgHeader(msg,msgsize,&hdr);

	char* tok;// = lmsg+hdr.frontLen;
	tok = strtok(lmsg+hdr.frontLen,",*"); //kicks back whole string
	sscanf(tok,"%f",&(status->auvLat));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(status->auvLong));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(status->auvHeading));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(status->auvDepth));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(status->auvAlt));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(status->auvVolt));

	tok = strtok(NULL,",*");
	sscanf(tok,"%f",&(status->auvHumid));

	tok = strtok(NULL,",*");
	sscanf(tok,"%u",&(status->auvBehavior));

	tok = strtok(NULL,",*");
	sscanf(tok,"%u",&(status->auvAbort));

}

void sonardyneCommand::decodeNMEAMsgContent(char* msg, int* msgsize,
		SCL_CMD_RECORD* cmd) {
	char lmsg[SCL_STRINGMAX];
	strcpy(lmsg,msg);

	SCL_NMEA_HEADER_RECORD hdr;

	decodeNMEAMsgHeader(msg,msgsize,&hdr);

	char* tok; //= lmsg+nmeahdr.frontLen;
	tok = strtok(lmsg+hdr.frontLen,",*");
	sscanf(tok,"%x",&(cmd->cmd));

}

void sonardyneCommand::validateNMEAchecksum(char* msg, int* msgsize,
		unsigned int* valid) {
	char testmsg[256]; int testmsglen = 0;
	char csmsg[4];
	int testChecksum = 0;
	unsigned int calcChecksum = 0;
	*valid = 0;

	// scan string for asterisk
	int idx = strcspn(msg,"*");
	if (idx>=*msgsize | idx <= 0) return; //test strcpn


	memcpy(testmsg,msg,idx);
	testmsg[idx] = '\0'; //null terminate

	testmsglen = idx;
	if (testmsglen <=0) return;

	memcpy(csmsg,&msg[idx+1],2);
	csmsg[2] = '\0';

	sscanf(csmsg,"%x",&testChecksum);

	// calculate checksum
	generateChecksum(testmsg, &testmsglen, &calcChecksum);

	//test
	if (calcChecksum == testChecksum)
		*valid = 1;

	return;

}

void sonardyneCommand::generateChecksum(char* msg, int* msgsize,
		unsigned int* checksum) {
	*checksum = 0;
	//assume the dollar sign is at first position.
	for (int i = 1; i < *msgsize ; i++) {
		*checksum ^= *(msg+i);
	}

	return;
}

void sonardyneCommand::encodeNMEAMsgHeader(char* msg, int* msgsize,
		SCL_NMEA_HEADER_RECORD* hdr) {
	switch (hdr->talkerID) {
	case TID_MAPPER1:
		*msgsize = sprintf(msg,"$M1");
		break;
	case TID_MAPPER2:
		*msgsize = sprintf(msg,"$M2");
		break;
	case TID_SURFACE:
		*msgsize = sprintf(msg,"$SF");
		break;
	case TID_UNKNOWN:
	default:
		*msgsize = sprintf(msg,"$UN");
		break;

	}

	switch (hdr->msgType) {
	case MSG_CMD:
		*msgsize += sprintf(msg+*msgsize,"CMD");
		break;
	case MSG_SAN:
		*msgsize += sprintf(msg+*msgsize,"SAN");
		break;
	case MSG_STA:
		*msgsize += sprintf(msg+*msgsize,"STA");
		break;
	case MSG_UNKNOWN:
	default:
		*msgsize += sprintf(msg+*msgsize,"UNK");
		break;
	}

	// print the rest of the fields, do not end with a comma!
	*msgsize += sprintf(msg+*msgsize,",%1x,%i,%i",hdr->version,hdr->ts_sec,hdr->ts_msec);

	return;
}