/*
 * I2C_UCB1.c
 *
 *  Created on: Jul 5, 2016
 *      Author: roy
 */


#include <Debug/configPkg/package/cfg/empty_min_pe430X.h>
#include <gpio.h>
#include <msp430f5528.h>
#include <my_board.h>
#include <stdbool.h>
#include <ti/drivers/i2c/I2CUSCIB.h>
#include <ti/drivers/I2C.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <usci_b_i2c.h>
#include <xdc/runtime/System.h>
#include <xdc/std.h>
#include <misc_stuff.h>
#include "com.h"
// added for uart testing
#include <ti/drivers/UART.h>
#include "ADC12.h"

// AS5510 stuff
#define SENS_REG_ADDR                                           0x0B
#define SENS_25mT                                               1

#define SPD_POL_PWR_REG_ADDR                                    0x02
#define REG_0x02_SlowMode_ReversePolarity_PowerDown             0x07
#define REG_0x02_SlowMode_ReversePolarity_PowerUp               0x06

// state constants for the Hall Effect Sensor (HES)
#define HES_IDLE                0
#define HES_INIT_DEVICE         1   // this only occurs once at power-up
// After initialization is done, this is executed every time the
// heartBeatFxn() is called.    See main.c
#define HES_READ_DATA           2

#define I2C_BUS_ERROR       0x8000


#define OFFSET_CODE_ERROR      -1

#define PRESSURE_ERROR_VALUE    -1.0

extern QUADLET read_VRS( DOUBLET offset );

// Length Of Hall Effect Sensor Averaging Data
#define LOHESAD     16
DOUBLET HES_avg_data[ LOHESAD ];
BYTE HES_avg_index = 0;

BYTE HES_state = HES_INIT_DEVICE;

// QUADLET X = 0;// temporary testing variable !!!!!!!!!!!!!!!!!!

BYTE uart_write_buffer[10] = {0,0,0,0,0,0,0,0,0,0};	// for uart testing
extern UART_Handle uart_handle;

DOUBLET I2C_transfer_failures = 0;
DOUBLET OCF_failures = 0;

I2CUSCIB_Object i2cDriver_objects[I2C_DRIVER_COUNT];
I2C_Params i2cDriver_params;
I2C_Handle i2cDriver_handle = NULL;

bool i2c_transfer_success = false;

I2C_Transaction i2c_transaction;
UChar i2c_write_buf[ LENGTH_OF_I2C_WRITE_BUF ];
UChar i2c_read_buf[ LENGTH_OF_I2C_READ_BUF ];

const I2CUSCIB_HWAttrs i2cDriver_hwattrs[ I2C_DRIVER_COUNT ] =
{
    {
         .baseAddr = USCI_B1_BASE,
		 .clockSource = USCI_B_I2C_CLOCKSOURCE_SMCLK
	}
};

/*
 *   The name of this array variable must be used as it is !
 *   If you name it something else you will get an unresolved
 *   linker error on the symbol I2C_config.
 */
const I2C_Config I2C_config[] =
{
    {
        .fxnTablePtr = &I2CUSCIB_fxnTable,
        .object      = &i2cDriver_objects[0],
		.hwAttrs     = &i2cDriver_hwattrs[0]
    },
    {NULL, NULL, NULL}
};

void my_board_init_I2C( void )
{
	UInt i;

	// see MSP430F5xx_6xx DriverLib Users Guide
	// This setting should allow the I2C module to control
	// the directions of the pins.
	// pin 43, UCB1SCL (HES_SCL), P4.2
	// pin 42, UCB1SDA (HES_SDA), P4.1
	GPIO_setAsPeripheralModuleFunctionInputPin( GPIO_PORT_P4,
			GPIO_PIN2 | GPIO_PIN1);

	// Function to initialize the I2C module
	I2C_init();

	/*
	 * Initializes i2c interface to the default settings.
	 *  transferMode = I2C_MODE_BLOCKING
	 *  transferCallbackFxn = NULL
	 *  bitRate = I2C_100kHz
	 */
	I2C_Params_init(&i2cDriver_params);

	// use 400kHz
	// for a 2 byte write, at 100KHz, the I2C_transfer can take 327uS to execute.
	// using 400kHz reduces this to 114uS.  At 38.4KBaud, a byte takes 260uS to
	// receive.  Since you are blocking, you have (260-114) = 146uS to switch
	// out of this task before losing an RX byte .
	//
	i2cDriver_params.bitRate = I2C_100kHz;

	/*
	 *	In I2C_open(), the first argument is the index into the array
	 *	I2C_config[].
	*/

	i2cDriver_handle = I2C_open( 0, &i2cDriver_params );

	if (i2cDriver_handle == NULL)
	{
		System_printf("I2C did not open");
	}

	// clear the write buffer
	for(i=0; i<LENGTH_OF_I2C_WRITE_BUF; i++)
	{
		i2c_write_buf[i] = 0;
	}

	// clear the read buffer
	for(i=0; i<LENGTH_OF_I2C_READ_BUF; i++)
	{
		i2c_read_buf[i] = 0;
	}

	// i2c_transaction.slaveAddress = 0x24;	// the slave address for the LTC2485
	i2c_transaction.slaveAddress = 0x56;	// the slave address for the AS5510
	i2c_transaction.writeBuf = i2c_write_buf;
	i2c_transaction.readBuf = i2c_read_buf;

	// clear averaging buffer for the Hall Effect Sensor
	for(i=0; i<LOHESAD; i++)
	{
	    HES_avg_data[i] = 0;
	}

}	// end of my_board_init_I2C() **********************************************



void HES_task( void )
{
	extern QUADLET X;
	extern QUADLET ram_VRS[LORVRS];
	// QDATA sensor_data, avg_sensor_offset, slope, output_pressure;
	extern bool continuous_readout_mode_flag;
	extern BYTE continuous_readout_counter;
	extern BYTE continuous_readout_packet[ LOCRP ];
	extern bool transmit_continuous_readout_packet( void );
	extern QUADLET flash_VRS_lo[ (LOVRS/2) ];
	extern QUADLET flash_VRS_hi[ (LOVRS/2) ];
	extern bool coefficient_selection_mode;
	extern void I2C_bus_error( void );
	extern float compute_pressure( DOUBLET HES_data );

	BYTE value_of_sens_reg = 0;
	BYTE value_of_spd_pol_pwr_reg = 0;
	BYTE value_read_from_spd_pol_pwr_reg = 0;
	QDATA HES_data;
	QDATA pressure;


	while( 1 )
	{
		// This semaphore is posted once, under the control of the heartbeat function
		Semaphore_pend( HES_sem, BIOS_WAIT_FOREVER );

		switch ( HES_state )
		{

		    case HES_INIT_DEVICE:

		        // program the sensitivity register
		        // only occurs once at power-up

		        // write pointer
		        i2c_write_buf[0] = SENS_REG_ADDR;

		        i2c_transaction.writeCount = 1;
		        i2c_transaction.readCount = 1;
		        i2c_transfer_success = I2C_transfer(
		                i2cDriver_handle, &i2c_transaction );

		        if( i2c_transfer_success == false )
		        {
		            // failure !
		            I2C_bus_error();
		            break;
		        }

                value_of_sens_reg = i2c_read_buf[0];
                // we want 25 mT
                // clear bits Sens1, Sens0
                value_of_sens_reg &= 0xFC;
                // set bit Sens0
                value_of_sens_reg |= 0x01;

                // write pointer, and value for the sensitvity register
                i2c_write_buf[0] = SENS_REG_ADDR;
                i2c_write_buf[1] = value_of_sens_reg;
                i2c_transaction.writeCount = 2;
                i2c_transaction.readCount = 0;
                i2c_transfer_success = I2C_transfer(
                        i2cDriver_handle, &i2c_transaction );

                if( i2c_transfer_success == false )
                {
                    // failure !
                    I2C_bus_error();
                    break;
                }

                HES_state = HES_READ_DATA;

		        break;

		    case HES_READ_DATA:

		        HES_data.quadlet = 0;

		        // configure for Slow mode, reverse polarity, and power-up

		        // read the speed, polarity, power register
		        i2c_write_buf[0] = SPD_POL_PWR_REG_ADDR;
		        i2c_transaction.writeCount = 1;
		        i2c_transaction.readCount = 1;
		        i2c_transfer_success = I2C_transfer(
		                i2cDriver_handle, &i2c_transaction );

		        if( i2c_transfer_success == false )
                {
                    // failure !
                    I2C_bus_error();
                    break;
                }

		        value_read_from_spd_pol_pwr_reg = i2c_read_buf[0];
		        value_of_spd_pol_pwr_reg = value_read_from_spd_pol_pwr_reg;

		        // clear bits: mode, polarity, power-down
		        value_of_spd_pol_pwr_reg &= 0xF8;

		        // set bits: slow mode, reverse polarity
		        value_of_spd_pol_pwr_reg |= 0x06;

                // write the speed, polarity, power register
                i2c_write_buf[0] = SPD_POL_PWR_REG_ADDR;
                i2c_write_buf[1] = value_of_spd_pol_pwr_reg;
                i2c_transaction.writeCount = 2;
                i2c_transaction.readCount = 0;
                i2c_transfer_success = I2C_transfer(
                        i2cDriver_handle, &i2c_transaction );

                if( i2c_transfer_success == false )
                {
                    // failure !
                    I2C_bus_error();
                    break;
                }

                // read HES data
                i2c_write_buf[0] = 0x00;    // pointer to first data register
                i2c_transaction.writeCount = 1;
                i2c_transaction.readCount = 2;
                i2c_transfer_success = I2C_transfer(
                        i2cDriver_handle, &i2c_transaction );

                if( i2c_transfer_success == false )
                {
                    // failure !
                    I2C_bus_error();
                    break;
                }

                // retrieve the data
                HES_data.byte[0] = i2c_read_buf[0];  // Least significant byte
                HES_data.byte[1] = i2c_read_buf[1];  // Most significant byte

                // Put the HES back into the powered-down state
                // clear bits mode, polarity, power-down
                value_of_spd_pol_pwr_reg &= 0xF8;

                // set bits: slow mode, reverse polarity, power-down
                value_of_spd_pol_pwr_reg |= 0x07;

                // write the speed, polarity, power register
                i2c_write_buf[0] = SPD_POL_PWR_REG_ADDR;
                i2c_write_buf[1] = value_of_spd_pol_pwr_reg;
                i2c_transaction.writeCount = 2;
                i2c_transaction.readCount = 0;
                i2c_transfer_success = I2C_transfer(
                        i2cDriver_handle, &i2c_transaction );

                if( i2c_transfer_success == false )
                {
                    // failure !
                    I2C_bus_error();
                    break;
                }

                // Write HES data into the VRS
                // The Virtual Register Space (VRS) is shared with other processes
                // such as read and write uart transactions
                Semaphore_pend( mutex_sem_VRS, BIOS_WAIT_FOREVER ); // lock the mutex
                ram_VRS[ (VRS_HES_OFFSET - (LOVRS-8)) ] = HES_data.quadlet;
                Semaphore_post( mutex_sem_VRS );                    // unlock the mutex

                pressure.float_num = compute_pressure( HES_data.doublet[0] );

                // Write pressure value into the VRS
                Semaphore_pend( mutex_sem_VRS, BIOS_WAIT_FOREVER ); // lock the mutex
                ram_VRS[ (VRS_PRESSURE_OFFSET - (LOVRS-8)) ] = pressure.quadlet;
                Semaphore_post( mutex_sem_VRS );                    // unlock the mutex

                // You have written the pressure into the VRS.
                // Now you need another task to drive the output device.
		        break;

		    default:
		        break;

		}   // end of switch ( HES_state )

	}	// end of while( 1 ) ***************************************************

}	// end of HES_task() *******************************************************


bool transmit_continuous_readout_packet( void )
{
	bool success = true;
	// QDATA data;
	DDATA crc;
	BYTE i;
	extern QUADLET ram_VRS[LORVRS];
	extern BYTE continuous_readout_counter;
	extern BYTE continuous_readout_packet[ LOCRP ];
	extern UART_Handle uart_handle;
	int write_size = LOCRP;	// the number of bytes being written

	// byte 0, an 8-bit counter value, incremented after each transmission
	continuous_readout_packet[0] = continuous_readout_counter++;

	// bytes 1-4, the pressure sensor value
	Semaphore_pend( mutex_sem_VRS, BIOS_WAIT_FOREVER );	// lock the mutex


	Semaphore_post( mutex_sem_VRS );				// unlock the mutex

	// bytes 11-12, crc16
	// make the crc for this packet using the crc-engine of the msp430
	CRCINIRES = 0xFFFF;					// initialize the crc
	for(i=0; i<(LOCRP-2); i++)
	{
		CRCDIRB_L = continuous_readout_packet[i];
	}
	crc.doublet = CRCINIRES;		// read the crc from the engine

	continuous_readout_packet[11] = crc.byte[1];
	continuous_readout_packet[12] = crc.byte[0];

	// transmit the completed packet
	UART_write( uart_handle, continuous_readout_packet, write_size  );

	return success;
}	// end of transmit_data_packet() *******************************************


void I2C_bus_error( void )
{
    extern QUADLET ram_VRS[LORVRS];

    // Write the error code into the VRS
    // When another process tries to use the HES data,
    // it will know that the last HES session failed
    // The Virtual Register Space (VRS) is shared with other processes
    // such as read and write uart transactions
    Semaphore_pend( mutex_sem_VRS, BIOS_WAIT_FOREVER ); // lock the mutex

    ram_VRS[ (VRS_HES_OFFSET - (LOVRS-8)) ] = I2C_BUS_ERROR;

    Semaphore_post( mutex_sem_VRS );                    // unlock the mutex

    return;
}   // end of I2C_bus_error() **************************************************


float compute_pressure( DOUBLET HES_data )
{

    extern int locate_offset( float HES_value );
    extern float calculate_pressure( int offset, float HES_value );
    extern DOUBLET HES_avg_data[ LOHESAD ];
    extern BYTE HES_avg_index;


    DOUBLET sum, i;
    float HES_avg_value;
    int offset;
    QDATA temp0;

    temp0.float_num = PRESSURE_ERROR_VALUE;

    // make sure the OCF flag from the AS5510 is set,
    // otherwise the data is no good
    if( (0x800 & HES_data) == 0 )
    {
        // do nothing, there is no data to work with
        return temp0.float_num;
    }

    // mask the OCF and parity bits
    HES_data &= 0x3FF;

    // make sure the HES value is OK
    if( HES_data > 1023 )
    {
        // The maximum output value for the AS5510 is 1023
        // do nothing
        return temp0.float_num;
    }

    // store the HES value in the averaging array
    HES_avg_data[ HES_avg_index++ ] = HES_data;
    if( HES_avg_index >= LOHESAD )
    {
        HES_avg_index = 0;
    }

    // sum the HES values
    sum = 0;
    for(i=0; i<LOHESAD; i++)
    {
        sum += HES_avg_data[i];
    }

    HES_avg_value = ( (float)sum / (float)LOHESAD );

    __no_operation();

    offset = locate_offset( HES_avg_value );

    // check for an error
    // normally the offset is 0, 1, 2 ... (LOCA-2)
    if( offset < 0 )
    {
        // this is an error !
        return ((float)PRESSURE_ERROR_VALUE);
    }

    // calculate the average pressure
    // Each device type will use this value to control
    // 4-20mA regulator, voltage output, and display of LCD
    temp0.float_num = calculate_pressure( offset, HES_avg_value );

    return temp0.float_num;
}   // End of compute_pressure() ***********************************************


int locate_offset( float HES_value )
{
    extern QUADLET read_flash_VRS( BYTE offset );
    int offset = OFFSET_CODE_ERROR;  // this is the error value

    QDATA temp0;
    float low_side, high_side;
    DOUBLET i;

    // check for an out of range HES_value !

    if( HES_value > ((float)1023.1) )
    {
        // this is an error !
        return offset;
    }

    if( HES_value < ((float)-0.1) )
    {
        // this is an error !
        return offset;
    }


    // Do all the other HES values here
    for(i=0; i<=(LOCA-2); i++)
    {
        temp0.quadlet = read_flash_VRS( VRS_START_OF_HES_CAL_DATA_OFFSET + i);

        low_side = temp0.float_num;

        temp0.quadlet = read_flash_VRS( (VRS_START_OF_HES_CAL_DATA_OFFSET + i + 1) );
        high_side = temp0.float_num;

        // special case i == 0
        // an average HES value less than low_side
        if(i == 0)
        {
            if( HES_value < low_side )
            {
                offset = 0;
                break;
            }
        }

        // special case i == 19
        // an average HES value greater than high_side
        if( i == 19 )
        {
            if( HES_value >= high_side )
            {
                offset = 19;
                break;
            }
        }

        // all other cases are handled here
        if( (HES_value>=low_side) && (HES_value<high_side) )
        {
            // the correct pair of data points has been identified !
            offset = i;
            break;
        }

    }

    return offset;

}   // end of locate_offset() **************************************************


float calculate_pressure( int offset, float HES_value )
{
    float pressure = PRESSURE_ERROR_VALUE;
    QDATA temp0, temp1;
    float x, y, m, b, delta_y, delta_x;

    extern QUADLET read_flash_VRS( BYTE offset );

    // Create the coefficients for a linear equation using the
    // selected pair of calibration data points
    // Pressure = (m * HES_value) + b
    // m = (Change of Y / Change of X)
    // Note:  Pressure data has units of PSI.
    // It is encoded as a 32-bit IEEE floating point format

    // trap a low HES value in the turn-down region
    if(offset == 0)
    {
        // Check for a very low HES_value
        // It should be clamped to zero output pressure
        temp0.quadlet = read_flash_VRS( VRS_START_OF_HES_CAL_DATA_OFFSET );
        if( HES_value < (temp0.float_num + ((float)8.0)) )
        {
            return 0.0;
        }
    }

    // Delta Y
    temp0.quadlet = read_flash_VRS( VRS_START_OF_PRESS_CAL_DATA_OFFSET + offset );
    y = temp0.float_num;
    temp1.quadlet = read_flash_VRS( VRS_START_OF_PRESS_CAL_DATA_OFFSET + offset + 1 );
    delta_y =  ( temp1.float_num - temp0.float_num );

    // Delta X
    temp0.quadlet = read_flash_VRS( VRS_START_OF_HES_CAL_DATA_OFFSET + offset );
    x = temp0.float_num;
    temp1.quadlet = read_flash_VRS( VRS_START_OF_HES_CAL_DATA_OFFSET + offset + 1 );
    delta_x =  ( temp1.float_num - temp0.float_num );

    // Slope = delta_y / delta_x
    m = ( delta_y / delta_x );

    // offset        b = Y - (m * X)
    b = ( y - (m * x) );

    // Pressure = (m * HES_value) + b
    pressure = ((m * HES_value) + b);

    // Trap a negative number, it will give strange results
    // when you go to program the DAC, 4-20mA or LCD.
    if( pressure < 0.0 )
    {
        pressure = 0.0;
    }

    // Trap a pressure above Full Scale
    temp0.quadlet = read_flash_VRS( VRS_FULL_SCALE_OFFSET );
    if( pressure > temp0.float_num )
    {
        pressure = temp0.float_num;
    }

    return pressure;

}   // end of calculate_pressure() *********************************************


