#include <bmp280.h>
#include <bmp280_defs.h>
#include <Wire.h>
#include "MegunoLink.h"
TimePlot MyPlot1,MyPlot2,MyPlot3; 

#define Pressure
void delay_ms(uint32_t period_ms);
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
void print_rslt(const char api_name[], int8_t rslt);
#ifdef Pressure
int8_t rslt;
struct bmp280_dev bmp;
struct bmp280_config conf;
struct bmp280_uncomp_data ucomp_data;
uint32_t pres32, pres64;
double pres;
double temp;
#else
int8_t rslt;
struct bmp280_dev bmp;
struct bmp280_config conf;
struct bmp280_uncomp_data ucomp_data;
int32_t temp32;
double temp;
#endif   
float sealevel_pres = 1013;
float alti;
float calibration_A(float  sealevel_pres);
void setup() {
    Serial.begin(115200);
    Wire.pins(4,5);
    Wire.begin(0x76);

  // put your setup code here, to run once:
  /* Map the delay function pointer with the function responsible for implementing the delay */
    bmp.delay_ms = delay_ms;
    
    /* Assign device I2C address based on the status of SDO pin (GND for PRIMARY(0x76) & VDD for SECONDARY(0x77)) */
    bmp.dev_id = BMP280_I2C_ADDR_PRIM;

    /* Select the interface mode as I2C */
    bmp.intf = BMP280_I2C_INTF;

    /* Map the I2C read & write function pointer with the functions responsible for I2C bus transfer */
    bmp.read = i2c_reg_read;
    bmp.write = i2c_reg_write;

    /* To enable SPI interface: comment the above 4 lines and uncomment the below 4 lines */

    /*
     * bmp.dev_id = 0;
     * bmp.read = spi_reg_read;
     * bmp.write = spi_reg_write;
     * bmp.intf = BMP280_SPI_INTF;
     */
    rslt = bmp280_init(&bmp);
    Serial.println("init_status");
    Serial.print(rslt,HEX);


    /* Always read the current settings before writing, especially when
     * all the configuration is not modified
     */
    rslt = bmp280_get_config(&conf, &bmp);
    Serial.println("Get_conf");
    Serial.print(rslt,HEX);
#ifdef Pressure

    /* configuring the temperature oversampling, filter coefficient and output data rate */
    /* Overwrite the desired settings */
    conf.filter = BMP280_FILTER_COEFF_16;

    /* Pressure oversampling set at 4x */
    conf.os_pres = BMP280_OS_16X;

    /* Temperature oversampling set at 4x */
    conf.os_temp = BMP280_OS_2X;

    /* Setting the output data rate as 1HZ(1000ms) */
    conf.odr = BMP280_ODR_0_5_MS;
    rslt = bmp280_set_config(&conf, &bmp);
    Serial.println("data_rate_conf");
    Serial.print(rslt,HEX);


    /* Always set the power mode after setting the configuration */
    rslt = bmp280_set_power_mode(BMP280_NORMAL_MODE, &bmp);
    Serial.println("power_mode_conf");
    Serial.print(rslt,HEX);
#else
  /* Always read the current settings before writing, especially when
   * all the configuration is not modified
   */
    rslt = bmp280_get_config(&conf, &bmp);
    print_rslt(" bmp280_get_config status", rslt);

    /* configuring the temperature oversampling, filter coefficient and output data rate */
    /* Overwrite the desired settings */
    conf.filter = BMP280_FILTER_COEFF_2;

    /* Temperature oversampling set at 4x */
    conf.os_temp = BMP280_OS_4X;

    /* Pressure over sampling none (disabling pressure measurement) */
    conf.os_pres = BMP280_OS_NONE;

    /* Setting the output data rate as 1HZ(1000ms) */
    conf.odr = BMP280_ODR_1000_MS;
    rslt = bmp280_set_config(&conf, &bmp);
    print_rslt(" bmp280_set_config status", rslt);

    /* Always set the power mode after setting the configuration */
    rslt = bmp280_set_power_mode(BMP280_NORMAL_MODE, &bmp);
    print_rslt(" bmp280_set_power_mode status", rslt);
#endif
}

void loop() {

#ifdef Pressure
  // put your main code here, to run repeatedly:
   /* Reading the raw data from sensor */
    rslt = bmp280_get_uncomp_data(&ucomp_data, &bmp);

    /* Getting the compensated pressure using 32 bit precision */
    rslt = bmp280_get_comp_pres_32bit(&pres32, ucomp_data.uncomp_press, &bmp);

    /* Getting the compensated pressure using 64 bit precision */
    rslt = bmp280_get_comp_pres_64bit(&pres64, ucomp_data.uncomp_press, &bmp);
    
    /* Getting the compensated pressure as floating point value */
    rslt = bmp280_get_comp_pres_double(&pres, ucomp_data.uncomp_press, &bmp);

    rslt = bmp280_get_comp_temp_double(&temp, ucomp_data.uncomp_temp, &bmp);
    
//    printf("UP: %ld, P32: %ld, P64: %ld, P64N: %ld, P: %f\r\n",
//           ucomp_data.uncomp_press,
//           pres32,
//           pres64,
//           pres64 / 256,
//           pres);
    pres = pres/100.0;
    alti = calibration_A(sealevel_pres);
    printf("P: %f, T: %f, A = %f \r\n",pres,temp,alti);
    MyPlot1.SendData("PRESSURE", pres);  
    MyPlot2.SendData("TEMPERATURE", temp);  
    MyPlot3.SendData("ALTITUDE", alti);      
    bmp.delay_ms(1000); /* Sleep time between measurements = BMP280_ODR_1000_MS */
#else
        /* Reading the raw data from sensor */
        rslt = bmp280_get_uncomp_data(&ucomp_data, &bmp);

        /* Getting the 32 bit compensated temperature */
        rslt = bmp280_get_comp_temp_32bit(&temp32, ucomp_data.uncomp_temp, &bmp);

        /* Getting the compensated temperature as floating point value */
        rslt = bmp280_get_comp_temp_double(&temp, ucomp_data.uncomp_temp, &bmp);
        printf("UT: %ld, T32: %ld, T: %f \r\n", ucomp_data.uncomp_temp, temp32, temp);

        /* Sleep time between measurements = BMP280_ODR_1000_MS */
        bmp.delay_ms(1000);
#endif
}

void delay_ms(uint32_t period_ms)
{
    /* Implement the delay routine according to the target machine */
    delay(period_ms);
}

int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
{

    /* Implement the I2C write routine according to the target machine. */
    Wire.begin(i2c_addr); 
    Wire.beginTransmission(i2c_addr);
    Wire.write(reg_addr);
    Wire.write(*reg_data);
    Wire.endTransmission(); 
    
    return 0;
}


int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
{

    /* Implement the I2C read routine according to the target machine. */
    int i = 0;
    uint32_t data[8]={0};
  
    Wire.begin(i2c_addr); 
    Wire.beginTransmission(i2c_addr);
    Wire.write(reg_addr);
    Wire.endTransmission(); 
    
    Wire.requestFrom(i2c_addr, length);    // request 6 bytes from slave device #2
    while(Wire.available())    // slave may send less than requested
    {
      *reg_data = Wire.read();    // receive a byte as character
//      Serial.println();
//      Serial.print(*reg_data,HEX);
      
      reg_data++;
    }
    return 0;
}


float calibration_A(float  sealevel_pres)
{
  double altitude;
  

  altitude = 153.8461*((pow(( sealevel_pres/pres) , 0.1903)-1)*(temp+273.15));

  return altitude;
}


/*!
 *  @brief Prints the execution status of the APIs.
 *
 *  @param[in] api_name : name of the API whose execution status has to be printed.
 *  @param[in] rslt     : error code returned by the API whose execution status has to be printed.
 *
 *  @return void.
 */
void print_rslt(const char api_name[], int8_t rslt)
{
    if (rslt != BMP280_OK)
    {
        printf("%s\t", api_name);
        if (rslt == BMP280_E_NULL_PTR)
        {
            printf("Error [%d] : Null pointer error\r\n", rslt);
        }
        else if (rslt == BMP280_E_COMM_FAIL)
        {
            printf("Error [%d] : Bus communication failed\r\n", rslt);
        }
        else if (rslt == BMP280_E_IMPLAUS_TEMP)
        {
            printf("Error [%d] : Invalid Temperature\r\n", rslt);
        }
        else if (rslt == BMP280_E_DEV_NOT_FOUND)
        {
            printf("Error [%d] : Device not found\r\n", rslt);
        }
        else
        {
            /* For more error codes refer "*_defs.h" */
            printf("Error [%d] : Unknown error code\r\n", rslt);
        }
    }
}