/*
 *
 * MS5607 Altimeter Module
 * @author Michael Burmeister
 * @date December 29, 2014
 * 
*/
#include "simpletools.h"
#include "simplei2c.h"
#include "lcddisplay.h"

int Send_Cmd(i2c *, int);
void Get_Prom(i2c *);
long Get_Reading(i2c *, int);
long Get_AvgTemp(i2c *);
long Get_AvgPres(i2c *);


#define DEVICE_ADDR 0xec
#define CMD_READ_ADC 0x00
#define CMD_RESET 0x1e
#define CMD_CONV_ADC 0x40
#define CMD_READ_PROM 0xa0

#define RSLT_D1 0x00
#define RSLT_D2 0x10

#define RES_256 0x00
#define RES_512 0x02
#define RES_1024 0x04
#define RES_2048 0x06
#define RES_4096 0x08

#define SDA 20
#define SCL 19
#define LCD 18
#define Baud 19200

i2c *pbus;
i2c bus;

long C[8];
long At[16];
long Ap[16];
char buffer[100];
int T;
int Ti;
int P;
int Pi;


int main()
{
  int i;
  long D1;
  long D2;
  long TOffset;
  double Pressure;
  double Mecury;
  long Celsius;
  long Fahrenheit;
  long TempDiff;
  long TempSens;
  long long Poff, Psens;
  long Ps;
  double PressureOffset;
  double PressureSens;

  memset(At, 0, sizeof(At));
  memset(Ap, 0, sizeof(Ap));
  T = 0;
  P = 0;
  Ti = 0;
  Pi = 0;
  
  Lcd_Open(Baud, 0, LCD);
  Lcd_BackLight(-1);
  Lcd_Clr();
    
  pbus = i2c_open(&bus, SCL, SDA, 0);
  Get_Prom(pbus);
  
  TempSens = 8388608/C[6];

//  for (i=0;i<7;i++)
//    print("%d ", C[i]);
//  print("\n");
    
  TOffset = C[5] * 256;
  
tg100:
  // read pressure 
  D1 = Get_Reading(pbus, RSLT_D1);
  //D1 = Get_AvgPres(pbus);

  // read temperature 
  D2 = Get_Reading(pbus, RSLT_D2);
  //D2 = Get_AvgTemp(pbus);
  print("D1:%d, D2:%d\n", D1, D2);
  
  TempDiff = D2 - TOffset;
  //print("TempDiff: %d\n", TempDiff);
  
  Celsius = (2000 + TempDiff / TempSens)/100;
  Fahrenheit = (Celsius * 18 + 320)/10;
  //sprintf(buffer, "%d --> %d", Celsius, Fahrenheit);

  //Lcd_Chrs(lcd, buffer);
  //print(" --> %5.2f, %5.2f\n", Celsius, Fahrenheit);
  pause(100);
  
  //print("Pressure Raw: %d", D1);
   
  PressureOffset = C[2] * 13107.2 + (C[4] * TempDiff) / 640;
  Poff = C[2] * 13107 + C[4] * TempDiff / 640;
  PressureSens = C[1] * 6553.6 + (C[3] * TempDiff) / 1280;
  Psens = C[1] * 6553 + (C[3] * TempDiff) / 1280;

  //printf("Pressure Offset: %5.2f %lld\n", PressureOffset, Poff);
  //printf("Pressure Sens: %5.2f %lld\n", PressureSens, Psens);
  Ps = (D1 * Psens / 2097152 - Poff) / 32768;
  //print("Pressure: %d\n", Ps);
  
  Pressure = (D1 * PressureSens / 2097152 - PressureOffset) / 32768;
  Mecury = Pressure / 338.63886;
  printf("Pressure: %5.2f %d \n", Pressure, Ps);
  
  sprintf(buffer, "%d / %2.2f  ", Fahrenheit, Mecury);
  Lcd_PosCursor(0, 1);
  Lcd_Chrs(buffer);
  //print("--> %5.2f\n", Pressure);
  
  pause(1000);
  goto tg100;
}

// Send an I2C command to the sensor
int Send_Cmd(i2c *b, int Cmd)
{
  int i = i2c_poll(b, DEVICE_ADDR);
  i = i2c_writeByte(b, Cmd);
  i2c_stop(b);
  return i;
}

// get configuration data from chip
void Get_Prom(i2c *b)
{
  int addr;
  int h;
  int l;
  Send_Cmd(b, CMD_RESET);
  pause(50);
  for (int j=0;j<8;j++)
  {
    addr = j << 1;
    l = Send_Cmd(b, CMD_READ_PROM | addr);
    pause(50);
    l = i2c_poll(b, DEVICE_ADDR | 1);
    h = i2c_readByte(b, 0) << 8;
    l = i2c_readByte(b, 0) | h;
    i2c_stop(b);
    C[j] = l;
  }
}

// Get Temperature or Pressure values
long Get_Reading(i2c *p, int t)
{
  long d;
  int i;
  
  i = Send_Cmd(p, CMD_CONV_ADC | t);
  pause(50);
  i = Send_Cmd(p, CMD_READ_ADC);
  i = i2c_poll(p, DEVICE_ADDR | 1);
  d = i2c_readByte(p, 0);
  d = (d << 8) | i2c_readByte(p, 0);
  d = (d << 8) | i2c_readByte(p, 1);
  i2c_stop(p);
  
  return d;
}


// Get Average Temperature
long Get_AvgTemp(i2c *p)
{
  long i = Get_Reading(p, RSLT_D2);
  if (Ti > 15)
      Ti = 0;
  T = T - At[Ti];
  T = T + i;
  At[Ti++] = i;
  return T/16;
}


// Get Average Pressure
long Get_AvgPres(i2c *p)
{
  long i = Get_Reading(p, RSLT_D1);
  if (Pi > 15)
      Pi = 0;
  P = P - Ap[Pi];
  P = P + i;
  Ap[Pi++] = i;
  return P/16;
}