#include "sta/MS5607.hpp"

namespace sta {
    // Forward declaration
    uint16_t uint_8BufferTouint16_t(uint8_t* buffer);

    MS5607::MS5607(SpiDevice* device, OsrLevel level) {
        this->device_ = device;
        this->osr_ = level;
        this->lastPresVal = -1;
        this->adcStartTime = -1;
        this->presRead = true;

        // Reset device on start-up
        this->reset();
        // Read the PROM e.g. constants
        this->readPROM();
    }

    void MS5607::requestAdcReadout(uint32_t time) {
        // Get current state of calculation 
        if (time-adcStartTime < delayTimes(this->osr_) || !presRead) {
            // Time since last adc command is not enough
            return;
        }

        this->device_->beginTransmission();
        this->device_->transfer(MS5607::Operations::D1_CONVERSION + 2*this->osr_);
        this->device_->endTransmission();

        adcStartTime = time;
        presRead = false;
    }

    int32_t MS5607::getPressure(int32_t temp, uint32_t time) {
        if (time-adcStartTime < delayTimes(this->osr_) || presRead) {
            // Time since last adc command is not enough
            return lastPresVal;
        }

        uint8_t buffer[3] = { 0 };

        this->device_->beginTransmission();
        this->device_->transfer(MS5607::Operations::ADC_RESULT);
        this->device_->receive(buffer, 3);
        this->device_->endTransmission();
        this->presRead = true;

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

        this->requestAdcReadout(time);

        return calculatePressure(d1_val, reverseTempCalc(temp));
    }

    int32_t MS5607::reverseTempCalc(int32_t temp) {
        return this->tempsens;
    }

    // DEPRECATED
    int32_t MS5607::getPressure() {
        // Get pure ADC value from the sensor
        uint32_t d1 = readPressure();

        // Since pressure is temp-dependant, temperature needs to be polled
        // Could be too costly timewise, since 8ms is needed for the ADC
        // May need future optimisation
        uint32_t d2 = readTemp();
        int32_t dT = d2 - ( ((uint32_t)this->t_ref) << 8);

        return calculatePressure(d1, dT);
    }

    // DON'T USE
    uint32_t MS5607::readPressure() {
        // Request pressure value conversion
        this->device_->beginTransmission();
        this->device_->transfer(MS5607::Operations::D1_CONVERSION + 2*this->osr_);
        this->device_->endTransmission();

        // Wait for ADC to finish
        // Could do sth. else and schedule continuation with scheduler in RTOS
        // TODO: Find out min
        sta::delayMs(10);

        // Request readout of ADC value
        uint8_t d1Arr[3];
        this->device_->beginTransmission();
        this->device_->transfer(MS5607::Operations::ADC_RESULT);
        this->device_->receive(d1Arr, 3);
        this->device_->endTransmission();

        // Convert into best possible type
        uint32_t res = 0;
        // Shifting may not be necessary, but idk w/o testing
        res |= d1Arr[0] | (d1Arr[1] << 8) | (d1Arr[2] << 16);
        return res;
    }

    // Calculations from the Datasheet
    // Probably problems with type conversions
    // If we used Rust...
    int32_t MS5607::calculatePressure(uint32_t d1, int32_t dT) {
        int64_t offset = ( ((uint64_t)this->off) << 17) + ( ( ((uint64_t)this->tco) * dT ) >> 6);
        int64_t sensitivity = ( ((uint64_t)this->sens) << 16) + ( ( ((uint64_t)this->tcs) * dT ) >> 7);
        int32_t pres = ( (( ((uint64_t)d1) * sensitivity) >> 21) - offset ) >> 15;
        return pres;
    }

    // NOT RECOMMENDED 
    // USE TEMP FROM SCA3300 OR STH. ELSE
    int32_t MS5607::getTemperature() {
        // Get pure ADC value from the sensor
        uint32_t d2 = readTemp();

        return calculateTemperature(d2);
    }

    // OLD; DON'T USE
    uint32_t MS5607::readTemp() {
        // Request ADC conversion of temperature
        this->device_->beginTransmission();
        this->device_->transfer(MS5607::Operations::D2_CONVERSION + 2*this->osr_);
        this->device_->endTransmission();

        // Wait for ADC to finish
        // Could do sth. else and schedule continuation with scheduler in RTOS
        // TODO: Test out min
       sta::delayMs(10);


        // Request ADC readout
        uint8_t d2Arr[3];
        this->device_->beginTransmission();
        this->device_->transfer(MS5607::Operations::ADC_RESULT);
        this->device_->receive(d2Arr, 3);
        this->device_->endTransmission();   

        // Convert into best possible type
        uint32_t res = 0;
        // Shifting may be unnecessary? Don't know really w/o testing
        res |= d2Arr[0] | d2Arr[1] << 8 | d2Arr[2] << 16;
        return res;
    }

    // Calculations from the Datasheet
    // Probably problems with type conversions
    // If we used Rust...
    int32_t MS5607::calculateTemperature(uint32_t d2) {
        int32_t dT = d2 - ( ((uint32_t)this->t_ref) << 8);
        int32_t temp = 2000 + ((dT * ((uint32_t)this->tempsens)) >> 23);

        // Further calculations for low (<20) and very low (<(-15)) could be possible
        // But I don't know whether they are necessary

        return temp;
    } 

    // Reset sequence as described in datasheet
    void MS5607::reset() {
        this->device_->beginTransmission();
        this->device_->transfer(MS5607::Operations::RESET);
        this->device_->endTransmission();
        delayUs(MS5607::RESET_DELAY);
    }

    // Read all constants from the PROM
    // May be moved to be called in reset() function in future
    // Request value x -> Read value x; Then request value y etc.
    // Could be optimized; Not as important since only needed once at start-up
    void MS5607::readPROM() {
        this->device_->beginTransmission();

        this->device_->transfer(Operations::READ_PROM);
        uint8_t sensArr[2];
        this->device_->receive(sensArr, 2);
        this->sens = uint_8BufferTouint16_t(sensArr);

        pulseCS();

        this->device_->transfer(Operations::READ_PROM+2);
        uint8_t offArr[2];
        this->device_->receive(offArr, 2);
        this->off = uint_8BufferTouint16_t(offArr);

        pulseCS();

        this->device_->transfer(Operations::READ_PROM+4);
        uint8_t tcsArr[2];
        this->device_->receive(tcsArr, 2);
        this->sens = uint_8BufferTouint16_t(sensArr);

        pulseCS();

        this->device_->transfer(Operations::READ_PROM+6);
        uint8_t tcoArr[2];
        this->device_->receive(tcoArr, 2);
        this->tco = uint_8BufferTouint16_t(tcoArr);

        pulseCS();

        this->device_->transfer(Operations::READ_PROM+8);
        uint8_t t_refArr[2];
        this->device_->receive(t_refArr, 2);
        this->t_ref = uint_8BufferTouint16_t(t_refArr);

        pulseCS();

        this->device_->transfer(Operations::READ_PROM+0xA);
        uint8_t tempsensArr[2];
        this->device_->receive(tempsensArr, 2);
        this->tempsens = uint_8BufferTouint16_t(tempsensArr);

        this->device_->endTransmission();
    }

    // Helper function:
    // Take first bytes from buffer, swap them and store those in uint16_t
    // Swap may not be necessary
    uint16_t uint_8BufferTouint16_t(uint8_t* buffer) {
        return (buffer[0] << 8) | buffer[1];
    }
}