#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <fcntl.h>
#include <ctype.h>
#include <math.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <stdbool.h>

/*********************************************************************
This file was orginally meant to be used for testing the i2c bus on
LRAUVs motherboard version 3.0. This includes the temperature 
humidity, and pressure sensors. 
Note that to compile standalone for the target, the following flags 
must be used. 

arm-vfp-linux-gnu-gcc -std=c99 i2cTest.c -lm -o i2cTest

Initial revision for i2c bus testing on LPC2350
B. Kieft 05/2016
**********************************************************************/

//#include <linux/i2c-dev.h>
#include "i2c-dev.hh" // local definition of SMBus commands
  
#define FATAL do { fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", \
  __LINE__, __FILE__, errno, strerror(errno)); exit(1); } while(0)
 
#define MAP_SIZE 4096UL
#define MAP_MASK (MAP_SIZE - 1)

  //***
  bool testPressureOutput = false; // Use known test values for pressure sensor output
  //***
  
  int file;
  int adapter_nr = 0; /* probably dynamically determined */
  char filename[20];
  int addr = 0; /* The I2C address */
  float a0,b1,b2,c12; // pressure coefficients

  __u8 registerR = 0x27; // * Device register to access for Humidity
  __s32 res;
  char buf[10];
  char outVal[10];

void handleA0( void );
void handleB1( void );
void handleB2( void );
void handleC12( void );

void requestHumidityWrite()
{
	printf("Requesting Humidity/Temp\n");
	buf[0] = registerR;

  int retVal = write(file,buf,0);
  if ( retVal != 0 ) {
    /* ERROR HANDLING: i2c transaction failed */
    printf("Write failed. Expected 0, got:%d\n",retVal); 
    exit(1);
  }
  sleep(0.1);
  /* Using I2C Read, equivalent of i2c_smbus_read_byte(file) */
  if (read(file,buf,4) != 4) {
    /* ERROR HANDLING: i2c transaction failed */
        printf("Error. Nothing to read\n"); 
  } 
  else {
    /* buf contains the read bytes */
    // Humidity is contained in the last 14 bits of the first 2 bytes    
    printf("Read bytes: %x:%x:%x:%x\n", buf[0],buf[1],buf[2],buf[3]); 
    buf[0] = buf[0] & 0x3F; // Lose the first to bits
    float humidity = buf[0] << 8 | buf[1];
    printf("Humidity: %f\%\n", (humidity/(pow(2,14)-2)*100));
    
    // Temperature is contained in the first 14 bits of the last 2 bytes
    float temperature = buf[2] << 6 | buf[3];
    temperature = temperature / ( pow( 2, 14 ) - 2 ) * 165 - 40;
    printf("Temperature: %f degC\n", temperature);
    printf("Temperature: %f degF\n", temperature*1.8 + 32 );
  }
}


void requestPressureConversion()
{
	printf("Requesting Pressure/Temp Conversion\n");
	buf[0] = 0x12; // Convert pressure and temp
  buf[1] = 0x00;
  int retVal = write(file,buf,2);
  if ( retVal != 2 ) {
    /* ERROR HANDLING: i2c transaction failed */
    printf("Write failed. Expected 2, got:%d\n",retVal); 
    exit(1);
  }
}


void readPressureResults()
{
	
/*
Pressure coefficients for this sensor are given via 4 bytes and encoded as
follows:
									Padc	Tadc
Total Bits				10		10
Sign Bits					0			0
Integer Bits			10		10
Fractional Bits		0			0
dec pt zero pad		0			0

For example:
Pressure MSB = 0x66
Pressure LSB = 0x80
== 410 ADC counts

Temp MSB = 0x7E
Temp LSB = 0xC0
== 507 ADC counts

using sample pressure coefficients:
A0 = 0x3ECE = 2009.75
B1 = 0xB3F9 = -2.37585
B2 = 0xC517 = -0.92047
C12 = 33C8 = 0.000790 

PComp = 733.19051
Pressure = 96.59kPa

*/
	
	printf("Requesting Results\n");

  // Despite what the spec claims, to get the entire 4 bytes of temp/pressure
  // one must initiate 4 reads using the commands for pressure MSB/LSB and temp 
  // MSB/LSB. They are stored one by one below. 
  
  // Read temp LSB
	buf[0] = 0x03;
  buf[1] = 0xC1; // Device address plus read bit
  int retVal = write(file,buf,2);
  if ( retVal != 2 ) {
    /* ERROR HANDLING: i2c transaction failed */
    printf("Write failed. Expected 2, got:%d\n",retVal); 
    exit(1);
  }
  read(file,buf,1);
  outVal[3]=buf[0];


  // Read temp MSB
	buf[0] = 0x02; 
  buf[1] = 0xC1; // Device address plus read bit
  retVal = write(file,buf,2);
  if ( retVal != 2 ) {
    /* ERROR HANDLING: i2c transaction failed */
    printf("Write failed. Expected 2, got:%d\n",retVal); 
    exit(1);
  }
  read(file,buf,1);
  outVal[2]=buf[0];


  // Read pressure LSB
	buf[0] = 0x01; 
  buf[1] = 0xC1; // Device address plus read bit
  retVal = write(file,buf,2);
  if ( retVal != 2 ) {
    /* ERROR HANDLING: i2c transaction failed */
    printf("Write failed. Expected 2, got:%d\n",retVal); 
    exit(1);
  }
  read(file,buf,1);
  outVal[1]=buf[0];  


  // Read pressure MSB
	buf[0] = 0x00; 
  buf[1] = 0xC1; // Device address plus read bit
  retVal = write(file,buf,2);
  if ( retVal != 2 ) {
    /* ERROR HANDLING: i2c transaction failed */
    printf("Write failed. Expected 2, got:%d\n",retVal); 
    exit(1);
  }      
  if (read(file,buf,1) != 1) {
    /* ERROR HANDLING: i2c transaction failed */
        printf("Error. Nothing to read\n"); 
  } 
  else {
    /* retVal contains the read bytes */  
    outVal[0]=buf[0];
    printf("Read bytes: %x:%x:%x:%x\n", outVal[0],outVal[1],outVal[2],outVal[3]); 


    // Inject known values into the read to view expected results
    // See expected results above
    if(testPressureOutput)
      {
        outVal[0] = 0x66; // Pressure MSB
        outVal[1] = 0x80; // Pressure LSB
        outVal[2] = 0x7e; // Temperature MSB
        outVal[3] = 0xc0; // Temperature LSB
        printf("Injected bytes: %x:%x:%x:%x\n", outVal[0],outVal[1],outVal[2],outVal[3]);         
      }    
    
    unsigned short pressure = outVal[0] << 8 | outVal[1]; // Combine the two pressure bytes
    unsigned short temperature = outVal[2] << 8 | outVal[3]; // Combine the two temperature bytes    

    pressure = pressure >> 6; // Pressure only uses top 10 bits
    temperature = temperature >> 6; // temperature only uses top 10 bits

    printf("Pressure ADC counts:%d, Temp ADC counts:%d\n",pressure,temperature);    
    // And now for some math from section 3.5 of the doc
    float c12x2 = c12 * (float)temperature;
    float a1 = b1 + c12x2;
    float a1x1 = a1 * (float)pressure;
    float y1 = a0 + a1x1;
    float a2x2 = b2 * (float)temperature;
    float pComp = y1 + a2x2;
   
    printf("c12x2:%f, a1:%f, a1x1:%f, y1:%f, a2x2:%f, pComp:%f\n",c12x2,a1,a1x1,y1,a2x2,pComp);
    float finalPressure = pComp * ((115-50)/1023.0) + 50.0;
    printf("Pressure:%fkPa (%f ATM)\n",finalPressure, finalPressure*0.009869);
  }
  
}


void requestPressureCoefficients()
{

/*
Pressure coefficients for this sensor are given via 8 bytes and encoded as
follows:
									A0		B1		B2		C12
Total Bits				16		16		16		14
Sign Bits					1			1			1			1
Integer Bits			12		2			1			0
Fractional Bits		3			13		14		13
dec pt zero pad		0			0			0			9

For example:
A0 = 0x3ECE = 2009.75
B1 = 0xB3F9 = -2.37585
B2 = 0xC517 = -0.92047
C12 = 33C8 = 0.000790

*/	
	
	
	printf("Requesting Coefficients\n");
  
  // The device address plus write bit (0xC0) is sent
  // automatically during each transfer following the 
  // ioctl call to set up the port. 
  	
  buf[0] = 0x04; // Read coefficient data byte
	buf[1] = 0xC1; // Device address plus read bit
  int retVal = write(file,buf,2);
  if ( retVal != 2 ) {
    /* ERROR HANDLING: i2c transaction failed */
    printf("Write failed. Expected 2, got:%d\n",retVal); 
    exit(1);
  }
  
  // Read back the coefficients
  if (read(file,buf,8) != 8) {
        printf("Error. Nothing to read\n"); 
  } 
  else {
    /* buf contains the read bytes */
    printf("Read bytes: %x:%x:%x:%x:%x:%x:%x:%x\n", buf[0],buf[1],buf[2],buf[3],buf[4],buf[5],buf[6],buf[7]); 


    // Inject known values into the read to view expected results
    // See expected results above
    if(testPressureOutput)
      {
        buf[0] = 0x3e; // A0 MSB
        buf[1] = 0xce; // A0 LSB
        buf[2] = 0xb3; // B1 MSB
        buf[3] = 0xf9; // B1 LSB
        buf[4] = 0xc5; // B2 MSB
        buf[5] = 0x17; // B2 LSB
        buf[6] = 0x33; // C12 MSB
        //buf[6] = 0xB3; // C12 MSB. Sets the sign bit high for a negative C12 value
        buf[7] = 0xc8; // C12 LSB
        printf("Injected bytes: %x:%x:%x:%x:%x:%x:%x:%x\n", buf[0],buf[1],buf[2],buf[3],buf[4],buf[5],buf[6],buf[7]);         
      }

      handleA0();
 
      handleB1();
      
      handleB2();
      
      handleC12();
  }
}


void handleA0()
{
      short tmp = buf[0] << 8 | buf[1]; // Combine the two bytes
      int fracBits = 3; // Number of fractional bits allocated
      signed int integerPart = tmp >> fracBits; // Grab just the integer portion
      int decimalPart = tmp & 0x7; // Grab just the part after the decimal point
      float decimal = 0; // Store the floating point value here
      
      decimalPart = decimalPart << 1; // We'll shift these bits out one by one in a loop below
      
      // Walk through each bit to see if it's set
      for(int i=1;i<=fracBits;i++)
      {
       decimalPart = decimalPart >> 1;
       unsigned short j = decimalPart & 1; // Store the current bit value 
       // If the current bit is set, add it to the decimal part
       if( j > 0)
       	{
        decimal += pow(0.5,fracBits-i+1); // 2^-bitvalue == 1/2 ^ bitvalue
        }
        // printf("i:%d, Integer part:%d, Dec part:%d, decimal_bit&1:%d, power%f\n",i,integerPart, decimalPart,decimalPart & 1, decimal ); 
      }
      // Now add the decimal part to the integer part to make the full number
      decimal += integerPart;
      a0 = decimal;
      printf("A0:%f\n",a0); 
}


void handleB1()
{
      short tmp = buf[2] << 8 | buf[3]; // Combine the two bytes
      int fracBits = 13; // Number of fractional bits allocated
      signed int integerPart = tmp >> fracBits; // Grab just the integer portion
      int decimalPart = tmp & 0x1FFF; // Grab just the part after the decimal point
      float decimal = 0; // Store the floating point value here
      
      decimalPart = decimalPart << 1; // We'll shift these bits out one by one in a loop below
      
      // Walk through each bit to see if it's set
      for(int i=1;i<=fracBits;i++)
      {
       decimalPart = decimalPart >> 1;
       unsigned short j = decimalPart & 1; // Store the current bit value 
       // If the current bit is set, add it to the decimal part
       if( j > 0)
       	{
        decimal += pow(0.5,fracBits-i+1); // 2^-bitvalue == 1/2 ^ bitvalue
        }
        //printf("i:%d, Integer part:%d, Dec part:%d, decimal_bit&1:%d, power%f\n",i,integerPart, decimalPart,decimalPart & 1, decimal ); 
      }
      // Now add the decimal part to the integer part to make the full number
      decimal += integerPart;
      b1 = decimal;
      printf("B1:%f\n",b1); 
}


void handleB2()
{
      short tmp = buf[4] << 8 | buf[5]; // Combine the two bytes
      int fracBits = 14; // Number of fractional bits allocated
      signed int integerPart = tmp >> fracBits; // Grab just the integer portion
      int decimalPart = tmp & 0x3FFF; // Grab just the part after the decimal point
      float decimal = 0; // Store the floating point value here
      
      decimalPart = decimalPart << 1; // We'll shift these bits out one by one in a loop below
      
      // Walk through each bit to see if it's set
      for(int i=1;i<=fracBits;i++)
      {
       decimalPart = decimalPart >> 1;
       unsigned short j = decimalPart & 1; // Store the current bit value 
       // If the current bit is set, add it to the decimal part
       if( j > 0)
       	{
        decimal += pow(0.5,fracBits-i+1); // 2^-bitvalue == 1/2 ^ bitvalue
        }
        // printf("i:%d, Integer part:%d, Dec part:%d, decimal_bit&1:%d, power%f\n",i,integerPart, decimalPart,decimalPart & 1, decimal ); 
      }
      // Now add the decimal part to the integer part to make the full number
      decimal += integerPart;
      b2 = decimal;
      printf("B2:%f\n",b2); 
}


void handleC12()
{
      short tmp = buf[6] << 8 | buf[7]; // Combine the two bytes
      tmp = tmp >> 2; // C12 only uses the top 14 bits.
      //int fracBits = 13; // Number of fractional bits allocated. No integer bits
      int fracBits = 22; // Number of fractional bits allocated plus 9 padded zeros. No integer bits

      signed int integerPart = tmp >> fracBits; // Grab just the integer portion
      int decimalPart = tmp & 0x1FFF; // Grab just the part after the decimal point
      float decimal = 0; // Store the floating point value here
      
      decimalPart = decimalPart << 1; // We'll shift these bits out one by one in a loop below
      
      // Walk through each bit to see if it's set
      for(int i=1;i<=fracBits;i++)
      {
       decimalPart = decimalPart >> 1;
       unsigned short j = decimalPart & 1; // Store the current bit value 
       // If the current bit is set, add it to the decimal part
       if( j > 0)
       	{
        decimal += pow(0.5,fracBits-i+1); // 2^-bitvalue == 1/2 ^ bitvalue
        }
        //printf("i:%d, Integer part:%d, Dec part:%d, decimal_bit&1:%d, power%f\n",i,integerPart, decimalPart,decimalPart & 1, decimal ); 
      }
      // Now check the sign and apply as needed
      if(integerPart < 0)
      	{
      		decimal = decimal * integerPart;
      	}
      	c12 = decimal;
      printf("C12:%f\n",c12); 
}


void requestHumiditySMBus()
{
	printf("Requesting Humidity using SMBus\n");

  /* Using SMBus commands */
//  i2c_smbus_write_word_data(file,registerR,0x6543); 
//    i2c_smbus_write_quick( file, registerR);

    i2c_smbus_write_byte( file, registerR);

    res = i2c_smbus_read_byte( file );
//    res = i2c_smbus_read_byte_data( file, registerR );
	
//    res = i2c_smbus_read_word_data( file, registerR );
    
//  union i2c_smbus_data data;  
//  struct i2c_smbus_ioctl_data args;
//	args.read_write = 1;
//	args.command = 0x27;
//	args.size = 3;
//	args.data = &data;
//  
//  res = ioctl(file, I2C_SMBUS, &args);

if (res < 0) {
    /* ERROR HANDLING: i2c transaction failed */
   printf("Error. Nothing to SMBus read. Returned %d\n", res); 
  } 
  else {
    /* res contains the read byte */
   printf("Read SMBus byte: %f.\n", res);
   buf[0] = res;
//   res = i2c_smbus_read_byte( file ); 
//   if (res < 0) {
//    /* ERROR HANDLING: i2c transaction failed */
//     printf("Error. Nothing to SMBus read for second byte. Returned %d\n", res); 
//   }
//     else { 
//     	 buf[1] = res;
//       printf("Read bytes: %x:%x\n", buf[0],buf[1]); 
//       float humidity = buf[0] << 7 | buf[1];
//       printf("Humidity: %f\n", (humidity/(pow(2,14)-2)*100));
//     }
  }

}

//int main(int argc, char **argv) {
int main() {
    
//  int file;
//  int adapter_nr = 0; /* probably dynamically determined */
//  char filename[20];
  
  sprintf(filename,"/dev/i2c-%d",adapter_nr);
  printf("Opening: %s\n", filename); 
  if ((file = open(filename,O_RDWR)) < 0) {
    /* ERROR HANDLING; you can check errno to see what went wrong */
    printf("Error opening file. errno is: %d.\n", errno); 
    exit(1);
  }

  
  addr = 0x27; /* The I2C address */
  if ((ioctl(file,I2C_SLAVE,addr)) < 0) {
    /* ERROR HANDLING; you can check errno to see what went wrong */
    printf("Error setting ioctl. errno is: %d.\n", errno); 
    exit(1);
  }

  // Humidity sensor
  requestHumidityWrite();


  // Set up for pressure  
  addr = 0x60; /* The I2C address */
  if ((ioctl(file,I2C_SLAVE,addr)) < 0) {
    /* ERROR HANDLING; you can check errno to see what went wrong */
    printf("Error setting ioctl. errno is: %d.\n", errno); 
    exit(1);
  }
  
  // Pressure sensor
  printf("\n");
  requestPressureCoefficients();
  printf("\n");
  requestPressureConversion();
  //sleep(.1);
  readPressureResults();
  
return 0;


}