//============================================================================
/// Summary  : Reson 6046 Commands
/// Filename : Reson6046Commands.cc
/// Author   : Reed Christenson, reed@bluefinrobotics.com
/// Project  : Reson 6046 Driver
/// Revision : 0.0
/// Created  : 2002.04.15
/// Modified : 2002.04.15
//============================================================================
// From RESON SeaBat 7k DATA FORMAT INTERFACE CONTROL DOCUMENT 
//      Draft Version 0.32

#include "Reson6046Commands.h"

Reson6046Commands::Reson6046Commands()
{
    // Set some defaults in the command structure.
    cmdStruct.sizeNetworkFrame = sizeof(Reson6046NetworkFrame);
    cmdStruct.sizeNetworkFrameAndData = 0;
    cmdStruct.sizeDataRecordFrame = sizeof(Reson6046DataRecordFrame);
    cmdStruct.sizeDataRecordFrameAndData = 0;
    cmdStruct.sizePlcCommandFrame = sizeof(Reson6046PayloadControllerCommand);
    cmdStruct.sizePlcCommandFrameAndData = 0;
    cmdStruct.nfPtr = NULL;
    cmdStruct.drfPtr = NULL;
    cmdStruct.plccfPtr = NULL;
    cmdStruct.responsePtr = NULL;
    //
    Reson6046Commands::initPlcCommandFrame();
    Reson6046Commands::initDataRecordFrame();
    Reson6046Commands::initNetworkFrame();
}

Reson6046Commands::~Reson6046Commands()
{
    // Clear up pointers and acquired memory.

    clean(cmdStruct.nfPtr);
    clean(cmdStruct.drfPtr);
    clean(cmdStruct.plccfPtr);
    clean(cmdStruct.responsePtr);
}

// Setup some default values for NetworkFrame structure.
void Reson6046Commands::initNetworkFrame()
{
    U32 size;
    cmdStruct.networkFrame.version = NETWORK_FRAME_VERSION;
    cmdStruct.networkFrame.offset = cmdStruct.sizeNetworkFrame;
    cmdStruct.networkFrame.totalPackets = 1;
    //cmdStruct.networkFrame.totalRecords = 0;
    //cmdStruct.networkFrame.transmissionIdentifier = 0;
    cmdStruct.networkFrame.packetSize = 0;
    //cmdStruct.networkFrame.totalSize = 0;
    cmdStruct.networkFrame.sequenceNumber = 0;
}


// Set Network Frame.
int Reson6046Commands::setNetworkFrame(//U16 transmissionIdentifier,
                                       U32 totalPackets,
                                       //U16 totalRecords,
                                       //U32 totalSize,
                                       U32 sequenceNumber)
{
    unsigned int index = 0, size = 0;
//     //unsigned int sizeFullMessage, sizeNetworkFrame, sizeRecordFrame;

    //Syslog::write("DBG: setNetworkFrame...");

    if(cmdStruct.drfPtr == NULL)
    {
        Syslog::write("ERROR: No message provided for Reson 6046 Network Frame!");
        return(-1);
    }

    if(cmdStruct.nfPtr != NULL)
    {
        // Make sure we don't leak memory!
        clean(cmdStruct.nfPtr );
    }
//     //sizeRecordFrame = sizeof(recordFrame);
//     //sizeNetworkFrame = sizeof(networkFrame);
    cmdStruct.sizeNetworkFrameAndData =
        cmdStruct.sizeDataRecordFrameAndData + cmdStruct.sizeNetworkFrame;

    cmdStruct.nfPtr = new unsigned char[cmdStruct.sizeNetworkFrameAndData];
    if(cmdStruct.nfPtr == NULL)
    {
	Syslog::write("setNetworkFrame: Unable to allocate space for Network Frame And Message!");
	return(-1);
    }
    memset(cmdStruct.nfPtr, 0, cmdStruct.sizeNetworkFrameAndData);

    size = 0;
    // U16 version (set in constructor)
    size += sizeU16;
    // U16 offset (set in constructor)
    size += sizeU16;
    // U32 totalPackets
    cmdStruct.networkFrame.totalPackets = totalPackets;
    size += sizeU32;
    // U16 totalRecords
    //cmdStruct.networkFrame.totalRecords = totalRecords;
    //size += sizeU16;
    // U16 trasmissionIdentifier
    //cmdStruct.networkFrame.transmissionIdentifier = transmissionIdentifier;
    //size += sizeU16;
    // U32 packetSize
    cmdStruct.networkFrame.packetSize = cmdStruct.sizeNetworkFrameAndData;
    size += sizeU32;
    // U32 totalSize
    //if(totalPackets > 1)
    //{
    //   cmdStruct.networkFrame.totalSize = totalSize;
    //}
    //else
    //{
    //   cmdStruct.networkFrame.totalSize = cmdStruct.sizeDataRecordFrameAndData;
    //}
    //size += sizeU32;
    // U32 sequenceNumber
    cmdStruct.networkFrame.sequenceNumber = sequenceNumber;
    size += sizeU32;

    //Syslog::write("Writing NF, %d bytes...\n",size);
    index = 0;
    memcpy(&cmdStruct.nfPtr[index], &cmdStruct.networkFrame.version, size);
    index += size;
    
    // DYNAMIC DATA
    //Syslog::write("Adding DRF to NF, index at %d...\n",index);
    memcpy(&cmdStruct.nfPtr[index], &cmdStruct.drfPtr[0], cmdStruct.sizeDataRecordFrameAndData);
    // index += cmdStruct.sizeRecordFrameAndData;
    
    return(0);
}

// Display Record Frame.
void Reson6046Commands::displayNetworkFrame(U32 showFrame) const
{
    U32 size;
    // This output can be interupted by other processes also using Syslog::write().
    Syslog::write("_____Reson TCP and UDP Network Frame (NF)_____");
    Syslog::write("                version = %d",cmdStruct.networkFrame.version);
    Syslog::write("                 offset = %d",cmdStruct.networkFrame.offset);
    Syslog::write("          total packets = %d",cmdStruct.networkFrame.totalPackets);
    //Syslog::write("          total records = %d",cmdStruct.networkFrame.totalRecords);
    //Syslog::write("transmission identifier = %d",cmdStruct.networkFrame.transmissionIdentifier);
    Syslog::write("            packet size = %d",cmdStruct.networkFrame.packetSize);
    //Syslog::write("             total size = %d",cmdStruct.networkFrame.totalSize);
    Syslog::write("        sequence number = %d",cmdStruct.networkFrame.sequenceNumber);

    if(cmdStruct.nfPtr != NULL)
    {
	switch(showFrame)
	{
	case ShowFrameOnly:
	    size = cmdStruct.sizeNetworkFrame;
	    Reson6046Commands::showRawFrame(cmdStruct.nfPtr, size);
	    break;
	case ShowFrameAndData:
	    size = cmdStruct.sizeNetworkFrameAndData;
	    Reson6046Commands::showRawFrame(cmdStruct.nfPtr, size);
	    break;
	case ShowDataOnly:
	    size = cmdStruct.sizeNetworkFrameAndData - cmdStruct.sizeNetworkFrame;
	    Reson6046Commands::showRawFrame(&cmdStruct.nfPtr[cmdStruct.sizeNetworkFrame],
					    size);
	    break;
	case None:
	default:
	    size = 0;
	}
    }
}

// Setup some default values for DataRecordFrame structure.
void Reson6046Commands::initDataRecordFrame()
{
    // Setup some default values for DataRecordFrame structure.
    cmdStruct.dataRecordFrame.version = DATA_RECORD_FRAME_VERSION;
    // Offset doesn't include                                         version,   offset,  checksum.
    cmdStruct.dataRecordFrame.offset = cmdStruct.sizeDataRecordFrame - sizeU16 - sizeU16 - sizeU32;
    cmdStruct.dataRecordFrame.syncPattern = DATA_RECORD_FRAME_SYNC_PATTERN;
    cmdStruct.dataRecordFrame.size = 0;
    cmdStruct.dataRecordFrame.optionalDataOffset = 0;
    cmdStruct.dataRecordFrame.optionalDataIdentifier = 0;
    Reson6046Commands::set7kTime(&cmdStruct.dataRecordFrame.time7k);
    //cmdStruct.dataRecordFrame.reserved1 = 0;
    cmdStruct.dataRecordFrame.recordTypeIdentifier = 0;
    cmdStruct.dataRecordFrame.deviceIdentifier = 0;
    cmdStruct.dataRecordFrame.subsystemIdentifier = 0;
    cmdStruct.dataRecordFrame.systemEnumerator = 0;
    cmdStruct.dataRecordFrame.dataSetNumber = 0;
    cmdStruct.dataRecordFrame.sequentialRecordCounter = 0;
    cmdStruct.dataRecordFrame.pointerRecordPrevious.raw[0] = 0xFFFFFFFF;
    cmdStruct.dataRecordFrame.pointerRecordPrevious.raw[1] = 0xFFFFFFFF;
    cmdStruct.dataRecordFrame.pointerRecordNext.raw[0] = 0xFFFFFFFF;
    cmdStruct.dataRecordFrame.pointerRecordNext.raw[1] = 0xFFFFFFFF;
    cmdStruct.dataRecordFrame.flags = 0;
    //cmdStruct.dataRecordFrame.reserved2 = 0;
    cmdStruct.dataRecordFrame.checksum = 0;
}

// Set Data Record Frame.
int Reson6046Commands::setDataRecordFrame( U32 recordTypeIdentifier,
					   U32 deviceIdentifier,
					   U16 subsystemIdentifier,
					   U16 systemEnumerator,
					   U32 dataSetNumber,
					   TimeIF::TimeSpec *setTime,
					   U32 checksum,
					   U16 flags,
					   U32 sequentialRecordCounter,
					   //I64 pointerRecordPrevious,
					   //I64 pointerRecordNext,
					   U32 optionalDataOffset,
					   U32 optionalDataIdentifier) //,
    //U16 reserved1,
    //U16 reserved2)
{
    unsigned int index, size;

    if(cmdStruct.plccfPtr == NULL)
    {
	Syslog::write("ERROR: setDataRecordFrame: No message provided for Data Record Frame!");
	return(-1);
    }

    if(cmdStruct.drfPtr != NULL)
    {
	// Make sure we don't leak memory!
	delete[] cmdStruct.drfPtr;
	cmdStruct.drfPtr = NULL;
    }
    cmdStruct.sizeDataRecordFrameAndData =
	cmdStruct.sizePlcCommandFrameAndData + cmdStruct.sizeDataRecordFrame;
    cmdStruct.drfPtr = new unsigned char[cmdStruct.sizeDataRecordFrameAndData];
    if(cmdStruct.drfPtr == NULL)
    {
	Syslog::write("setDataRecordFrame: Unable to allocate space for Data Record Frame And Data!");
	return(-1);
    }
    memset(cmdStruct.drfPtr, 0, cmdStruct.sizeDataRecordFrameAndData);

    index = 0;
    size = 0;
    // version (set in constructor) U16
    size += sizeU16;
    // offset (set in constructor) U16
    size += sizeU16;
    // syncPattern (set in constructor) U32
    size += sizeU32;
    // size U32
    cmdStruct.dataRecordFrame.size = cmdStruct.sizeDataRecordFrameAndData;
    size += sizeU32;
    // optionalDataOffset U32
    cmdStruct.dataRecordFrame.optionalDataOffset = optionalDataOffset;
    size += sizeU32;
    // optionalDataIdentifier U32
    cmdStruct.dataRecordFrame.optionalDataIdentifier = optionalDataIdentifier;
    size += sizeU32;
    // time7k T7K
    Reson6046Commands::set7kTime(&cmdStruct.dataRecordFrame.time7k, setTime);
    size += sizeT7K;
    // reserved U16
    //cmdStruct.dataRecordFrame.reserved1 = reserved1;
    //size += sizeU16;
    // recordTypeIdentifier U32
    cmdStruct.dataRecordFrame.recordTypeIdentifier = recordTypeIdentifier;
    size += sizeU32;
    // deviceIdentifier U32
    cmdStruct.dataRecordFrame.deviceIdentifier = deviceIdentifier;
    size += sizeU32;
    // subsystemIdentifier U16
    cmdStruct.dataRecordFrame.subsystemIdentifier = subsystemIdentifier;
    size += sizeU16;
    // systemEnumerator U16
    cmdStruct.dataRecordFrame.systemEnumerator = systemEnumerator;
    size += sizeU16;
    // dataSetNumber U32
    cmdStruct.dataRecordFrame.dataSetNumber = dataSetNumber;
    size += sizeU32;
    // sequentialRecordCounter U32
    cmdStruct.dataRecordFrame.sequentialRecordCounter = sequentialRecordCounter;
    size += sizeU32;
    // pointerRecordPrevious (set in constructor) I64
    size += sizeI64;
    // pointerRecordNext (set in constructor) I64
    size += sizeI64;
    // flags U16
    cmdStruct.dataRecordFrame.flags = flags;
    size += sizeU16;
    // reserved U16
    //cmdStruct.dataRecordFrame.reserved2 = reserved2;
    //size += sizeU16;

    //Syslog::write("Writing dataRecordFrame, %d bytes...\n", size);
    index = 0;
    // Copy everything from version through reserved 2.
    memcpy(&cmdStruct.drfPtr[index], &cmdStruct.dataRecordFrame.version, size);
    index += size;
    //Syslog::write("Writing command frame and data at %d...\n",index);

    // DATA SECTION
    memcpy(&cmdStruct.drfPtr[index], cmdStruct.plccfPtr, cmdStruct.sizePlcCommandFrameAndData);
    index += cmdStruct.sizePlcCommandFrameAndData;
    
    cmdStruct.dataRecordFrame.checksum = checksum;
    memcpy(&cmdStruct.drfPtr[index], &cmdStruct.dataRecordFrame.checksum, sizeU32);
    //index += sizeU32;

    return(0);
};

// Display Data Record Frame.
void Reson6046Commands::displayDataRecordFrame(U32 showFrame) const
{
    U32 size;
    // This output can be interupted by other processes also using Syslog::write().
    Syslog::write("_____Data Record Frame (DRF)_____");
    Syslog::write("                  version = %d",cmdStruct.dataRecordFrame.version);
    Syslog::write("                   offset = %d",cmdStruct.dataRecordFrame.offset);
    Syslog::write("             sync pattern = 0x%08X",cmdStruct.dataRecordFrame.syncPattern);
    Syslog::write("     optional data offset = %d",cmdStruct.dataRecordFrame.optionalDataOffset);
    Syslog::write(" optional data identifier = %d",cmdStruct.dataRecordFrame.optionalDataIdentifier);
    Syslog::write("                  7K time = %4d.%03d %02d:%02d:%9.6f",
		  cmdStruct.dataRecordFrame.time7k.year,
		  cmdStruct.dataRecordFrame.time7k.day,
		  cmdStruct.dataRecordFrame.time7k.hours,
		  cmdStruct.dataRecordFrame.time7k.minutes,
		  cmdStruct.dataRecordFrame.time7k.seconds);
    //Syslog::write("                 reserved = %d",cmdStruct.dataRecordFrame.reserved1);
    Syslog::write("   record type identifier = %d",cmdStruct.dataRecordFrame.recordTypeIdentifier);
    Syslog::write("        device identifier = %d",cmdStruct.dataRecordFrame.deviceIdentifier); 
    Syslog::write("     subsystem identifier = %d",cmdStruct.dataRecordFrame.subsystemIdentifier);
    Syslog::write("        system enumerator = %d",cmdStruct.dataRecordFrame.systemEnumerator);
    Syslog::write("          data set number = %d",cmdStruct.dataRecordFrame.dataSetNumber);
    Syslog::write("sequential record counter = %d",cmdStruct.dataRecordFrame.sequentialRecordCounter);
    Syslog::write("  pointer record previous = 0x%08X%08X",
		  cmdStruct.dataRecordFrame.pointerRecordPrevious.raw[0],
		  cmdStruct.dataRecordFrame.pointerRecordPrevious.raw[1]);
    Syslog::write("      pointer record next = 0x%08X%08X",
		  cmdStruct.dataRecordFrame.pointerRecordNext.raw[0],
		  cmdStruct.dataRecordFrame.pointerRecordNext.raw[1]);
    Syslog::write("                    flags = %d",cmdStruct.dataRecordFrame.flags);
    //Syslog::write("               reservered = %d",cmdStruct.dataRecordFrame.reserved2);
    Syslog::write("                 checksum = %d",cmdStruct.dataRecordFrame.checksum);

    if(cmdStruct.drfPtr != NULL)
    {
	switch(showFrame)
	{
	case ShowFrameOnly:
	    size = cmdStruct.sizeDataRecordFrame;
	    Reson6046Commands::showRawFrame(cmdStruct.drfPtr, size);
	    break;
	case ShowFrameAndData:
	    size = cmdStruct.sizeDataRecordFrameAndData;
	    Reson6046Commands::showRawFrame(cmdStruct.drfPtr, size);
	    break;
	case ShowDataOnly:
	    size = cmdStruct.sizeDataRecordFrameAndData - cmdStruct.sizeDataRecordFrame - sizeU32;
	    Reson6046Commands::showRawFrame(&cmdStruct.drfPtr[cmdStruct.sizeDataRecordFrame],
					    size);
	case None:
	default:
	    size = 0;
	}
    }
};


// Init Payload Controller Command Frame
void Reson6046Commands::initPlcCommandFrame()
{
    cmdStruct.plcCommandFrame.sensorIndex = -1;
    cmdStruct.plcCommandFrame.commandID = 0;
    cmdStruct.plcCommandFrame.action = 0;
    cmdStruct.plcCommandFrame.commandBytesToFollow = 0;
}



// Set Payload Controller Command Frame
int Reson6046Commands::setPlcCommandFrame(U32 sensorIndex,
					  U32 commandID,
					  U32 action,
					  char *command)
{
    unsigned int size = 0, index = 0;
    //char buffer[16];

    if(command != NULL)
    {
	//sprintf(&buffer[0],command, param1);
	cmdStruct.plcCommandFrame.commandBytesToFollow = strlen(command);
    }
    else
    {
	cmdStruct.plcCommandFrame.commandBytesToFollow = 0;
    }

    // Check to see if there is alread a command pointer.
    if(cmdStruct.plccfPtr != NULL)
    {
	// Make sure we don't leak memory!
	//clean(cmdStruct.plccfPtr);
	delete[] cmdStruct.plccfPtr;
	cmdStruct.plccfPtr = NULL;
    }
    cmdStruct.sizePlcCommandFrameAndData =
	cmdStruct.sizePlcCommandFrame + cmdStruct.plcCommandFrame.commandBytesToFollow;
    cmdStruct.plccfPtr = new unsigned char[cmdStruct.sizePlcCommandFrameAndData];
    if(cmdStruct.plccfPtr == NULL)
    {
	Syslog::write("Unable to allocate memory for Payload Computer Command Frame!");
	return(-1);
    }
    memset(cmdStruct.plccfPtr, 0, cmdStruct.sizePlcCommandFrameAndData);

    // U32 fillerField
    cmdStruct.plcCommandFrame.fillerField = 0;
    size = sizeU32;
    // U32 sensorIndex
    cmdStruct.plcCommandFrame.sensorIndex = sensorIndex;
    size += sizeU32;
    // U32 hostID
    //cmdStruct.plcCommandFrame.hostID = hostID;
    //size += sizeU32;
    // U32 commandID
    cmdStruct.plcCommandFrame.commandID = commandID;
    size += sizeU32;
    // U32 action
    cmdStruct.plcCommandFrame.action = action;
    size += sizeU32;  
    // sizeU8 to follow ICD V 0.27
    //cmdStruct.plcCommandFrame.commandBytesToFollow calculated above.
    size += sizeU32;

    index = 0;
    memcpy(&cmdStruct.plccfPtr[index], &cmdStruct.plcCommandFrame.fillerField, size);
    index += size;

    // Handle dynamic data here.
    if(cmdStruct.plcCommandFrame.commandBytesToFollow > 0)
    {
	memcpy(&cmdStruct.plccfPtr[index],&command[0],cmdStruct.plcCommandFrame.commandBytesToFollow);
    }

    return(0);
}


// int Reson6046Commands::setPlcCommandFrame(U32 sensorIndex,
// 					  U32 commandID,
// 					  U32 action,
// 					  char *command,
// 					  U32 parameter)
// {
//     char buffer[32];

//     if(command != NULL)
//     {
// 	sprintf(&buffer[0],command,parameter);
// 	Reson6046Commands::setPlcCommandFrame(sensorIndex,
// 					      commandID,
// 					      action,
// 					      &buffer[0]);
//     }
//     return(0);
// }


// Display Payload Controller Command Frame
// RECORD TYPE # 1100 'Payload Controller Command' (page 34 of 44, Revision 0.27/PMO)
void Reson6046Commands::displayPlcCommandFrame(U32 showFrame)
{
    U32 size;
    Syslog::write("____Payload Computer Command Frame_____");
    Syslog::write("         sensorIndex = %d", cmdStruct.plcCommandFrame.sensorIndex);
    Syslog::write("           commandID = %d", cmdStruct.plcCommandFrame.commandID);
    Syslog::write("              action = %d", cmdStruct.plcCommandFrame.action);
    Syslog::write("commandBytesToFollow = %d", cmdStruct.plcCommandFrame.commandBytesToFollow);
    Syslog::write("...data follows...");
    if(cmdStruct.drfPtr != NULL)
    {
	switch(showFrame)
	{
	case ShowFrameOnly:
	    size = cmdStruct.sizePlcCommandFrame;
	    Reson6046Commands::showRawFrame(cmdStruct.plccfPtr, size);
	    break;
	case ShowFrameAndData:
	    size = cmdStruct.sizePlcCommandFrameAndData;
	    Reson6046Commands::showRawFrame(cmdStruct.plccfPtr, size);
	    break;
	case ShowDataOnly:
	    size = cmdStruct.sizePlcCommandFrameAndData - cmdStruct.sizePlcCommandFrame;
	    Reson6046Commands::showRawFrame(&cmdStruct.plccfPtr[cmdStruct.sizePlcCommandFrame],
					    size);
	case None:
	default:
	    size = 0;
	}
    }
}

// Set Reson 7k Time either (current time or the time in setTime).
void Reson6046Commands::set7kTime(T7K *time7k, TimeIF::TimeSpec *setTime)
{
    Time::TimeParts fullTime;
    //TimeIF::TimeSpec currentTime;

    // If setTime is NULL, splitTime returns current time (perfect).
    Time::splitTime(&fullTime, setTime);

    time7k->year    = fullTime.year;
    time7k->day     = fullTime.yday;
    time7k->seconds = fullTime.sec;
    time7k->minutes = fullTime.min;
    time7k->hours   = fullTime.hour;
}

void Reson6046Commands::showRawFrame(BYTE *framePtr, U32 size) const
{
    BYTE c;
    U32 i;

    if(framePtr != NULL)
    {
	for(i = 0; i < size; i++)
	{
	    c = framePtr[i];
	    printf("%02X:",c);
	}
	if(size > 0)
	    printf("\n");
    }
}