/*----------------------------------------------------------------------
 *
 * I3DM-GX2 Interface Software
 *
 *----------------------------------------------------------------------
 * (c) 2008 Microstrain, Inc.
 *----------------------------------------------------------------------
 * THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING
 * CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER
 * FOR THEM TO SAVE TIME. AS A RESULT, MICROSTRAIN SHALL NOT BE HELD LIABLE
 * FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY
 * CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY
 * CUSTOMERS OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH
 * THEIR PRODUCTS.
 *---------------------------------------------------------------------*/

#include "i3dmgx2_Utils.h"
#include "i3dmgx2_Errors.h"
#include "i3dmgx2_Serial.h"
#include "i3dmgx2_readWriteDR.h"
/*----------------------------------------------------------------------
 * ReadDataRate	0xE5
 *
 * parameters   portNum      : the port number of the sensor device
 *              address      : the address location for Data-Rate
 *              
 *              Bresponse    : buffer to return contains checksum 
 *
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int ReadDataRate(int portNum, char address_A, char address_B, unsigned char *Bresponse) {
	int status = 0; 
	unsigned char DR_buff[4];
	int responseLength = 5;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;


	/* initialize send buffer to obtain Data-Rate value at address */
	DR_buff[0]    = 0xE5;        // Identifier of Read Data-Rate value
	DR_buff[1]    = 0x00;        // Required field
	DR_buff[2]    = address_A;   // 1st byte of address location all at FC + address
	DR_buff[3]    = address_B;   // 2ond byte of address localtion for the desired Data-Rate value

	
	
	   status = sendBuffData(portNum, &DR_buff[0], 4);
		
	   //if (DEBUG) printf(" Data-Rate read sendData status is %d\n", status);
       if (status != I3DMGX2_COMM_OK)
	        return status = I3DMGX2_COMM_READ_ERROR; 
	   status = receiveData(portNum, &Bresponse[0], responseLength);
	   //if (DEBUG) printf(" Data-Rate read ReceiveData status is %d\n", status);
       if (status != I3DMGX2_COMM_OK)
		   return status = I3DMGX2_COMM_READ_ERROR;
	   else {      
		  wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	      wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); 
		  if(wChecksum == wCalculatedCheckSum){
			   	return status;
		  }
		  else if(wChecksum != wCalculatedCheckSum){
			              return status = I3DMGX2_CHECKSUM_ERROR;
		  }
	  }
      return status;

}
			
/*----------------------------------------------------------------------
 * WriteDataRate  0xE4
 *
 * parameters   portNum      : the number of the sensor device (1..n)
                address_a    : first byte of the data rate address location to write
 *              address      : second byte of the data rate address location
 *              value        : the value to write to the specified 
 *                             address
 *				Bresponse 	 : return buffer
 *
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns an
 *                          error code.
 *-------------------------------------------------------------------------*/
int WriteDataRate(int portNum, char address_A, char address_B, int rwval, unsigned char *Bresponse) {
    int status = 0;
    unsigned char DR_write_buff[8];
	int responseLength = 5;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;


	// Setup the Data-Rate write buffer 	 
	DR_write_buff[0] = 0xE4;                    //Command identifier for Write to Data-Rate
	DR_write_buff[1] = 0xC1;		            //Required identifier for Data-Rate Write
	DR_write_buff[2] = 0x29;                    //Required identifier for Data-Rate Write
	DR_write_buff[3] = 0x00;                    //Required identifier for Data-RateWrite
    DR_write_buff[4] = address_A;               //1st byte of Data-Rate address location
	DR_write_buff[5] = address_B;               //2ond byte of Data-Rate address location
	if(TestByteOrder() == LITTLE_ENDIAN){
    	DR_write_buff[6] = (rwval & MSB_MASK) >> 8; //1st byte of Data-Rate value to write
	    DR_write_buff[7] =  rwval & LSB_MASK;       //2ond byte of Data-Rate value to write
	}else{ //BIG_ENDIAN format
        DR_write_buff[6] = rwval;
	}
    
	   status = sendBuffData(portNum, &DR_write_buff[0], 8);
	          
       if (status != I3DMGX2_COMM_OK) 
		    return  status; 
	   status = receiveData(portNum, &Bresponse[0], responseLength);
	   if (status == I3DMGX2_COMM_OK) {
	     wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	     wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); 
		 if(wChecksum == wCalculatedCheckSum){
		     return status;
		 }
		 else if(wChecksum != wCalculatedCheckSum){
				 return status = I3DMGX2_CHECKSUM_ERROR;
		 }
	   }	   
    return status;
}