#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::setOsr(OsrLevel osr)
    {
        osr_ = osr;
    }

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

    float MS56xx::getPressure(Unit unit /* = Unit::hPa */)
    {
        // Request the ADC to read pressure values.
        busCommand(MS56xx::Operations::D1_CONVERSION + 2*this->osr_);

        // 8.22 ms conversion according to the datasheet.
        delay_(osrDelay());

        uint8_t buffer[3] = { 0x00, 0x00, 0x00 };
        busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);

        uint32_t D1 = 0x00 | buffer[0] << 16 | buffer[1] << 8 | buffer[2];

        // Request the ADC to read temperature values.
        busCommand(MS56xx::Operations::D2_CONVERSION + 2*this->osr_);

        // 8.22 ms conversion according to the datasheet.
        delay_(osrDelay());

        busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);

        uint32_t D2 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];

        float dT = D2 - C_[5];
        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()
    {
    	// Request the ADC to read temperature values.
		busCommand(MS56xx::Operations::D2_CONVERSION + 2*this->osr_);

		// 8.22 ms conversion according to the datasheet.
		delay_(osrDelay());

		uint8_t buffer[3] = { 0x00, 0x00, 0x00 };
		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];

    	return temperature * 0.01;
    }

    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()
    {
        C_[0] = 1;
        C_[1] = 65536L;
        C_[2] = 131072;
        C_[3] = 7.8125E-3;
        C_[4] = 0.015625;
        C_[5] = 256;
        C_[6] = 1.1920928955E-7;
    }

    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;
    }
}