#include "mgos_barometer_mpl115.h"
#include "mgos_i2c.h"

bool mgos_barometer_mpl115_create(struct mgos_barometer *dev) {
  struct mgos_barometer_mpl115_data *mpl115_data;

  if (!dev) return false;
  uint8_t data[8];
  if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MPL115_REG_COEFF_BASE, 8, data))
    return false;

  mpl115_data=calloc(1, sizeof(struct mgos_barometer_mpl115_data));
  if (!mpl115_data) return false;

  // TODO(pim): fix this -- math is wrong.
  mpl115_data->a0=(float)(((uint16_t) data[0] << 8) | data[1]) / 8.0;
  mpl115_data->b1=(float)(((uint16_t) data[2] << 8) | data[3]) / 8192.0;
  mpl115_data->b2=(float)(((uint16_t) data[4] << 8) | data[5]) / 16384.0;
  mpl115_data->c12=(float)((((uint16_t) (data[6] << 8) | data[7])) >> 2) / 4194304.0;
  dev->user_data=mpl115_data;
  LOG(LL_DEBUG, ("Calibration data: a0=%.4f b1=%.4f b2=%.4f c12=%.4f", mpl115_data->a0, mpl115_data->b1, mpl115_data->b2, mpl115_data->c12));

  dev->has_barometer=true;
  dev->has_thermometer=true;
  return true;
}

bool mgos_barometer_mpl115_destroy(struct mgos_barometer *dev) {
  if (!dev) return false;
  if (dev->user_data) {
    free(dev->user_data);
    dev->user_data=NULL;
  }
  return true;
}

bool mgos_barometer_mpl115_read(struct mgos_barometer *dev) {
  struct mgos_barometer_mpl115_data *mpl115_data;
  uint16_t Padc, Tadc;
  float Pcomp;

  if (!dev) return false;
  mpl115_data=(struct mgos_barometer_mpl115_data *) dev->user_data;
  if (!mpl115_data) return false;

  if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MPL115_REG_START, 0x00))
    return false;

  mgos_usleep(4000);
  uint8_t data[4];
  if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MPL115_REG_PRESSURE, 4, data))
    return false;
  Padc=(((uint16_t) data[0] << 8) | data[1]) >> 6;
  Tadc=(((uint16_t) data[2] << 8) | data[3]) >> 6;

  Pcomp= mpl115_data->a0 + (mpl115_data->b1 + mpl115_data->c12 * Tadc ) * Padc + mpl115_data->b2 * Tadc;

  dev->pressure= 1000*Pcomp*(65.0/1023) + 50.0;        // Pascals
  dev->temperature=((float) Tadc - 498.0F) / -5.35F +25.0F;           // Celsius

  return true;
}