#include "SIM_MS5525.h"

#include <SITL/SITL.h>

using namespace SITL;

void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
{
    const uint8_t Q1 = Qx_coeff[0];
    const uint8_t Q2 = Qx_coeff[1];
    const uint8_t Q3 = Qx_coeff[2];
    const uint8_t Q4 = Qx_coeff[3];
    const uint8_t Q5 = Qx_coeff[4];
    const uint8_t Q6 = Qx_coeff[5];

    // this is the forward conversion copied from the driver:
    float dT = float(D2) - int64_t(prom[5]) * (1L<<Q5);
    float TEMP = 2000 + (dT*int64_t(prom[6]))/(1L<<Q6);
    float OFF  = int64_t(prom[2])*(1L<<Q2) + (int64_t(prom[4])*dT)/(1L<<Q4);
    float SENS = int64_t(prom[1])*(1L<<Q1) + (int64_t(prom[3])*dT)/(1L<<Q3);
    float P = (float(D1)*SENS/(1L<<21)-OFF)/(1L<<15);
    const float PSI_to_Pa = 6894.757f;
    P_Pa = PSI_to_Pa * 1.0e-4 * P;
    Temp_C = TEMP * 0.01;
}

void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
{
    const uint8_t Q1 = Qx_coeff[0];
    const uint8_t Q2 = Qx_coeff[1];
    const uint8_t Q3 = Qx_coeff[2];
    const uint8_t Q4 = Qx_coeff[3];
    const uint8_t Q5 = Qx_coeff[4];
    const uint8_t Q6 = Qx_coeff[5];

    const float TEMP = Temp_C * 100.0f;
    const float dT = ((TEMP - 2000.0) * (1L<<Q6)) / int64_t(prom[6]);
    const float PSI_to_Pa = 6894.757f;
    const float P = P_Pa / (PSI_to_Pa * 1.0e-4);
    const float OFF  = int64_t(prom[2]) * (1L<<Q2) + (int64_t(prom[4]) * dT) / (1L<<Q4);
    const float SENS = int64_t(prom[1]) * (1L<<Q1) + (int64_t(prom[3]) * dT) / (1L<<Q3);

    D1 = ((P * (1L<<15) + OFF) * (1L<<21)) / SENS;
    D2 = dT + int64_t(prom[5]) * (1L<<Q5);
}

void MS5525::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2)
{
    float f_P_Pa;
    float f_Temp_C;
    convert_forward(D1, D2, f_P_Pa, f_Temp_C);

    const float p_error = fabs(f_P_Pa - P_Pa);
    const float p_error_max = 0.1;
    if (p_error > p_error_max) {
        AP_HAL::panic("Invalid pressure conversion: error %f Pa > %f Pa error max", p_error, p_error_max);
    }

    const float t_error = fabs(f_Temp_C - Temp_C);
    const float t_error_max = 0.01;
    if (t_error > t_error_max) {
        AP_HAL::panic("Invalid temperature conversion: error %f degC > %f degC error max", t_error, t_error_max);
    }
}

void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
{
    float sim_alt = AP::sitl()->state.altitude;
    sim_alt += 2 * rand_float();

    float sigma, delta, theta;
    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);

    // To Do: Add a sensor board temperature offset parameter
    Temp_C = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0;
    const uint8_t instance = 0;  // TODO: work out which sensor this is
    P_Pa = AP::sitl()->state.airspeed_raw_pressure[instance] + AP::sitl()->airspeed[instance].offset;
}