#include "bmp.h"
#include "i2c_rw.h"
#include "tools.h"
#include <iostream>
#include <iomanip>
#include <exception>

#define BMP180_READ_ADDRESS 0x77
#define BMP180_WRITE_ADDRESS 0x77

#define BMP085_REG_CALIBRATION_DATA_ADDR 0xAA
#define BMP085_REG_CHIPID       0xD0
#define BMP085_REG_VERSION      0xD1
#define BMP085_CMD_CONTROL      0xf4
#define BMP085_REG_TEMPDATA     0xf6
#define BMP085_REG_PRESDATA     0xf6
#define BMP085_CMD_READTEMP     0x2e
#define BMP085_CMD_READPRES     0x34
#define BMP085_CMD_SOFTRESET    0xE0

static char chipId  = 0;
static char version = 0;
// dafault values here are from datasheet for diagnostic purposes
static int16_t  ac1 = 408,
                ac2 = -72,
                ac3 = -14383;
static uint16_t ac4 = 32741,
                ac5 = 32757,
                ac6 = 23153;
static int16_t  b1  = 6190,
                b2  = 4,
                mb  = -32768,
                mc  = -8711,
                md  = 2868;

void cleanup_bmp()
{

}

void init_bmp()
{
    try
    {
        bcm2835_i2c_setSlaveAddress(BMP180_READ_ADDRESS);
        // get id/version
        chipId  =  i2c_reg_get8(BMP085_REG_CHIPID);
        version =  i2c_reg_get8(BMP085_REG_VERSION);

#ifndef BMP_DIAGNOSTIC
        // get calibration data
        ac1 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR);
        ac2 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 2);
        ac3 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 4);
        ac4 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 6);
        ac5 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 8);
        ac6 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 10);
         b1 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 12);
         b2 = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 14);
         mb = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 16);
         mc = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 18);
         md = i2c_reg_get16(BMP085_REG_CALIBRATION_DATA_ADDR + 20);
#endif // BMP_DIAGNOSTIC

        i2c_reg_set8(BMP085_CMD_SOFTRESET, 0xB6);
    }
    catch(const std::system_error& err)
    {
        std::cout << "bmp180: " << err.what() << " : ";
        switch (err.code().value())
        {
            case BCM2835_I2C_REASON_ERROR_NACK: std::cout << "Received a NACK"; break;
            case BCM2835_I2C_REASON_ERROR_CLKT: std::cout << "Received Clock Stretch Timeout"; break;
            case BCM2835_I2C_REASON_ERROR_DATA: std::cout << "Not all data is sent / received"; break;
            default: std::cout << "Unknown error"; break;
        }
        std::cout << std::endl;
    }
}

void poll_bmp(uint8_t oversampling)
{
#ifdef BMP_DIAGNOSTIC
    oversampling = BMP085_ULTRALOWPOWER;
#endif

    try
    {
        int32_t b3, b5, b6, x1, x2, x3, p;
        uint32_t b4, b7;
        uint16_t tempRawData = 27898;
        int32_t presRawData = 23843;

        // get raw temperature
        bcm2835_i2c_setSlaveAddress(BMP180_WRITE_ADDRESS);
        i2c_reg_set8(BMP085_CMD_CONTROL, BMP085_CMD_READTEMP);

        rpi_delay(5);

#ifndef BMP_DIAGNOSTIC
//        tempRawData = i2c_reg_get16(BMP085_REG_TEMPDATA);

        char tempReadBuf[2];
        i2c_read_reg(BMP085_REG_TEMPDATA, tempReadBuf, sizeof(tempReadBuf));
        tempRawData = (int)tempReadBuf[0] << 8 | (int)tempReadBuf[1];
#endif // BMP_DIAGNOSTIC

        // get raw pressure
        i2c_reg_set8(BMP085_CMD_CONTROL, BMP085_CMD_READPRES + (oversampling << 6));
        rpi_delay(2 + (3 << oversampling));

#ifndef BMP_DIAGNOSTIC
//        presRawData =  i2c_reg_get16(BMP085_REG_PRESDATA);
//        presRawData <<= 8;
//        presRawData |= (int8_t)i2c_reg_get8(BMP085_REG_PRESDATA+2);

        char presReadBuf[3];
        i2c_read_reg(BMP085_REG_PRESDATA, presReadBuf, sizeof(presReadBuf));
        presRawData = (int)presReadBuf[0] << 16 | (int)presReadBuf[1] << 8 | (int)presReadBuf[2];

        presRawData >>= (8-oversampling);
#endif // BMP_DIAGNOSTIC

        // calculate real temp / press
        x1 = ((int)tempRawData - (int)ac6) * ((int)ac5) >> 15;
        if (x1 == -md)
            throw std::runtime_error("invalid data");
        x2 = ((int)mc << 11) / (x1+md);
        b5 = x1 + x2;

        b6 = b5 - 4000;

        x1 = (b2 *  (b6 * b6)>>12 ) >> 11;
        x2 = (ac2 * b6) >> 11;
        x3 = x1 + x2;
        b3 = ((((int)ac1*4 + x3) << oversampling) + 2) / 4;

        x1 = (ac3 * b6) >> 13;
        x2 = (b1 * ((b6 * b6) >> 12)) >> 16;
        x3 = ((x1 + x2) + 2) >> 2;
        b4 = (ac4 * (uint32_t)(x3 + 32768)) >> 15;
        if (b4 == 0)
            throw std::runtime_error ("invalid data");

        b7 = ((uint32_t)(presRawData - b3) * ( 50000UL >> oversampling ));

        if (b7 < 0x80000000)
        {
            p = (b7 * 2) / b4;
        }
        else
        {
            p = (b7 / b4) * 2;
        }
        x1 = (p >> 8) * (p >> 8);
        x1 = (x1 * 3038) >> 16;
        x2 = (-7357 * p) >> 16;

        int32_t pres = p + ((x1 + x2 + 3791)>>4);
        int16_t temp = (b5 + 8) >> 4 ;

        std::cout << "bcm085: t=" << std::fixed << std::setprecision(1) << temp/10.0
                         <<  " p=" << std::fixed << std::setprecision(1) << pres
                   << std::endl;

#ifdef DEBUG_BMP
        std::cout << "chipid: " << (int)chipId
                   << " version: " << (int)version
                   << std::hex
                   << " Calibration data: "
                   << " ac1=" << std::setw(4) << ac1 << " ac2=" << std::setw(4) << ac2
                   << " ac3=" << std::setw(4) << ac3 << " ac4=" << std::setw(4) << ac4
                   << " ac5=" << std::setw(4) << ac5 << " ac6=" << std::setw(4) << ac6
                   << "  b1=" << std::setw(4) <<  b1 << "  b2=" << std::setw(4) << b2
                   << "  mb=" << std::setw(4) <<  mb << "  mc=" << std::setw(4) << mc
                   << "  md=" << std::setw(4) << md
                   << " Raw data: "
                   << " rt=" << std::setw(4) << tempRawData
                   << " rp=" << std::setw(6) << presRawData
                   << std::dec
                   << std::endl;
#endif // DEBUG_BMP
    }
    catch(const std::system_error& err)
    {
        std::cout << "bmp180: " << err.what() << " : ";
        switch (err.code().value())
        {
            case BCM2835_I2C_REASON_ERROR_NACK: std::cout << "Received a NACK"; break;
            case BCM2835_I2C_REASON_ERROR_CLKT: std::cout << "Received Clock Stretch Timeout"; break;
            case BCM2835_I2C_REASON_ERROR_DATA: std::cout << "Not all data is sent / received"; break;
            default: std::cout << "Unknown error"; break;
        }
        std::cout << std::endl;
    }
}