#include <math.h>

#include "i2c.h"

#include "bar.h"

static const unsigned char BAR_Addr = 0x5C; // LPS22HH

void (*BAR_DoneProc)(BAR_Data& Data);

float BAR_GetAltitude(float p0, float p1)
{
  return 44330.0f*(1.0f-powf(p1/p0, 1.0f/5.255f));
}
//------------------------------------------------------------------------------

static inline void BAR_SetReg(unsigned char Reg, unsigned char Value)
{
  unsigned char reg[2];
  reg[0]=Reg; reg[1]=Value;
  I2C2_Write(BAR_Addr, reg, 2);
  //I2C2_Stop();
}
//------------------------------------------------------------------------------

void BAR_Init()
{
  I2C2_Init();

  BAR_SetReg(0x10, 0x3E); // RESET
  
  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
}
//------------------------------------------------------------------------------

float BAR_GetData(float* Temp)
{
  static float bar=0;
  static float temp=0;
  
  unsigned char st;
  I2C2_Write(BAR_Addr, 0x27);
  I2C2_Read(BAR_Addr, &st, 1);
  I2C2_Stop();
  
  if(st & 1)
  {
    unsigned char reg[3];
    I2C2_Write(BAR_Addr, 0x28);
    I2C2_Read(BAR_Addr, reg, sizeof(reg));
    I2C2_Stop();
    

    unsigned long b;

    b = reg[2];
    b = (b * 256U) + reg[1];
    b = (b * 256U) + reg[0];
    b *= 256U;
    
    bar=((float)b)/1048576.0f;
  }
  
  if(st & 2)
  {
    unsigned char reg[2];
    I2C2_Write(BAR_Addr, 0x2B);
    I2C2_Read(BAR_Addr, reg, sizeof(reg));
   I2C2_Stop();
    

    short t;

    t = (short)reg[1];
    t = (t * 256) + (short)reg[0];
    
    temp = (float) t / 100.0f;
  }
  
  *Temp=temp;
  
  return bar;
  
}
//------------------------------------------------------------------------------