#include <sta/drivers/MS56xx.hpp>

#include <sta/debug/assert.hpp>
#include <sta/debug/debug.hpp>
#include <sta/lang.hpp>

#include <cmath>


namespace sta 
{
    MS56xx::MS56xx(SPIDevice * device, DelayUsFunc delay, OsrLevel level /* = OsrLevel::_1024 */)
        : device_{device},
          delay_{delay},
          osr_{level},
          intf_{Intf::SPI},
          C_{}
    {
        STA_ASSERT(device != nullptr);
    }

    MS56xx::MS56xx(I2CDevice * device, DelayUsFunc delay, OsrLevel osr /* = OsrLevel::_1024 */)
        : device_{device},
          delay_{delay},
          osr_{osr},
          intf_{Intf::I2C},
          C_{}
    {
        STA_ASSERT(device != nullptr);
    }

    bool MS56xx::init()
    {
        // Reset device on start-up
        this->reset();

        this->initConstants();

        // Read the PROM e.g. constants
        this->readPROM();

        return true;
    }

    void MS56xx::requestData()
    {

    }

    bool MS56xx::hasData()
    {

    }

    void MS56xx::setOsr(OsrLevel osr)
    {
        osr_ = osr;
    }

    void MS56xx::reset()
    {
        busCommand(MS56xx::Operations::RESET);
        delay_(MS56xx::RESET_DELAY);
    }

    void MS56xx::requestData(DataType type, bool blocking = false)
    {
        Operations op = type == DataType::PRESSURE ? D1_CONVERSION : D2_CONVERSION;

        // Request the ADC to read new data.
        busCommand(op + 2*this->osr_);

        if (blocking)
        {
            delay_(osrDelay());
        }
    }

    float MS56xx::getPressure(Unit unit /* = Unit::hPa */)
    {
        // Request the ADC to read pressure values.
        requestData(PRESSURE, true);

        uint8_t buffer[3] = { 0x00, 0x00, 0x00 };
        busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);
        uint32_t D1 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];

        // Request the ADC to read temperature values.
        requestData(PRESSURE, true);

        busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);
        uint32_t D2 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];

        float dT = D2 - C_[5];
        float temperature = 2000 + dT * C_[6];

        float offset =  C_[2] + dT * C_[4];
        float sens = C_[1] + dT * C_[3];

        if (temperature < 2000)
        {
            float T2 = dT * dT * 4.6566128731E-10;
            float t = (temperature - 2000) * (temperature - 2000);
            float offset2 = 2.5 * t;
            float sens2 = 1.25 * t;
            //  COMMENT OUT < -1500 CORRECTION IF NOT NEEDED
            if (temperature < -1500)
            {
                t = (temperature + 1500) * (temperature + 1500);
                offset2 += 7 * t;
                sens2 += 5.5 * t;
            }
            temperature -= T2;
            offset -= offset2;
            sens -= sens2;
        }

        // The pressure in Pa.
        float pressure = (D1 * sens * 4.76837158205E-7 - offset) * 3.051757813E-5;
        // pressure = convertPressure(pressure, unit);

        return pressure * 0.01;
    }

    int32_t MS56xx::getTemperature() {
       return 21.0f;
    }

    void MS56xx::setPressureReference(float pressRef, float altRef, Unit unit /* = Unit::hPa */)
    {
        // Taken from: https://cdn-shop.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf, page 17.
        sealevel_ = pressRef / (std::pow((1 - altRef / 44330), 5.255));
    }

    float MS56xx::getAltitudeEstimate()
    {
        float pressure = getPressure(Unit::hPa);

        // Taken from: https://cdn-shop.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf, page 16
        float altitude = 44330 * (1 - std::pow(pressure / sealevel_, 1 / 5.255));

        return altitude;
    }

    void MS56xx::readPROM() {
        uint8_t buffer[2];

        for (size_t i = 0; i < 7; i++)
        {
            busRead(Operations::READ_PROM + i*2, buffer, 2);

            C_[i] *= uint_8BufferTouint16_t(buffer);
        }
    }

    void MS56xx::initConstants(bool mathMode /* = 0 */)
    {
        C_[0] = 1;
        C_[1] = 32768L;          //  SENSt1   = C[1] * 2^15    |    * 2^16
        C_[2] = 65536L;          //  OFFt1    = C[2] * 2^16    |    * 2^17
        C_[3] = 3.90625E-3;      //  TCS      = C[3] / 2^8     |    / 2^7
        C_[4] = 7.8125E-3;       //  TCO      = C[4] / 2^7     |    / 2^6
        C_[5] = 256;             //  Tref     = C[5] * 2^8     |    * 2^8
        C_[6] = 1.1920928955E-7; //  TEMPSENS = C[6] / 2^23    |    / 2^23

        if (mathMode == 1)  //  Appnote version for pressure.
        {
            C_[1] = 65536L;          //  SENSt1
            C_[2] = 131072L;         //  OFFt1
            C_[3] = 7.8125E-3;       //  TCS
            C_[4] = 1.5625e-2;       //  TCO
        }
    }

    inline float MS56xx::convertPressure(float pressure, Unit unit)
    {
        switch (unit)
        {
        case Unit::mbar:
            return pressure / 100.0f;
        case Unit::hPa:
            return pressure / 100.0f;
        case Unit::Pa:
            return pressure;
        default:
            STA_UNREACHABLE();
            break;
        }
    }

    inline uint16_t MS56xx::uint_8BufferTouint16_t(uint8_t* buffer) {
        return (buffer[0] << 8) | buffer[1];
    }

    bool MS56xx::busCommand(uint8_t command)
    {
        this->device_->beginTransmission();
        this->device_->transfer(command);
        this->device_->endTransmission();

        return true;
    }

    uint32_t MS56xx::osrDelay() {
        // Delay times taken from:
        // https://www.amsys-sensor.com/downloads/notes/MS5XXX-C-code-example-for-MS56xx-MS57xx-MS58xx-AMSYS-an520e.pdf
        switch (osr_) {
        case _256:
            return 600;
        case _512:
            return 1200;
        case _1024:
            return 2300;
        case _2048:
            return 4600;
        case _4096:
            return 9100;
        default:
            STA_UNREACHABLE();
        }
    }

    bool MS56xx::busRead(uint8_t reg, uint8_t * buffer, size_t length)
    {
        this->device_->beginTransmission();
        this->device_->transfer(reg);
        this->device_->receive(buffer, length);
        this->device_->endTransmission();

        return true;
    }
}