#include "STS21.h"

//#define DEBUG

double STS21::getTemperature(boolean noHold)
{
  uint8_t data[2] = {0,0};
  double temperature(0.0);
  uint16_t alignement(0);
  
  twi->beginTransmission(address);
  if(noHold)
    twi->write(NO_HOLD_MASTER);
  else
    twi->write(HOLD_MASTER);
    
  if(twi->endTransmission() != 0)
    return temperature;//Error

  if(noHold)
  {
    switch(_stsRes)
    {
      case RES_13:
        delay(50);
        break;
      case RES_12:
        delay(35);
        break;
      case RES_11:
        delay(20);
        break;
      default:
        delay(95);
        break;
    }
  }

  //On recupère les deux octets contenant la température
  twi->requestFrom(address, (uint8_t)2);
  if(twi->available() == 2)
  {
    data[0] = twi->read();
    data[1] = twi->read();
  }

  alignement = data[0];
  alignement <<= 8;
  alignement += data[1] & 0xFC;
  
  temperature = -46.85 + 175.72 * ((double)alignement / 65536.0);

  return temperature;
}

uint8_t STS21::readUserRegister()
{
  uint8_t returned(0);
  
  twi->beginTransmission(address);
  twi->write(READ_USER_REG);
  if(twi->endTransmission() != 0)
    return 0;

  twi->requestFrom(address, (uint8_t)1);

  if(twi->available() == 1)
  {
    returned = twi->read();
  }
  
  return returned;
}

boolean STS21::writeUserRegister(uint8_t value)
{
  twi->beginTransmission(address);
  twi->write(WRITE_USER_REG);
  twi->write(value);
  
  return twi->endTransmission() == 0;
}

boolean STS21::setResolution(STS_RES res)
{
  uint8_t userReg = readUserRegister();
  boolean toBeReturned(false);
  #ifdef DEBUG
  Serial.printf("User Reg value before : %X\n",userReg);
  #endif
  
  switch(res)
  {
    case RES_13:
        userReg |= 0b10000000;
        userReg &= 0b11111110;
        break;
      case RES_12:
        userReg |= 0b00000001;
        userReg &= 0b01111111;
        break;
      case RES_11:
        userReg |= 0b10000001;
        break;
      default://Default is 14 bit resolution
        userReg &= 0b01111110;
        break;
  }

  if(writeUserRegister(userReg))
  {
    toBeReturned = true;
    _stsRes = res;
    #ifdef DEBUG
    Serial.printf("Set resolution success\n");
    #endif
  }
  
  userReg = readUserRegister();
  #ifdef DEBUG
  Serial.printf("User Reg value after : %X\n",userReg);
  #endif

  return toBeReturned;
}

boolean STS21::enableOnChipHeater(boolean enabled)
{
  uint8_t userReg = readUserRegister();
  boolean toBeReturned(false);
  
  #ifdef DEBUG
  Serial.printf("User Reg value before : %X\n",userReg);
  #endif

  if(enabled)
    userReg |= CHIP_HEATER;
  else
    userReg &= (~CHIP_HEATER);

  if(writeUserRegister(userReg))
  {
    toBeReturned = true;
    _onChipHeater = enabled;
    #ifdef DEBUG
    Serial.printf("Set chip heater success\n");
    #endif
  }
  
  userReg = readUserRegister();
  #ifdef DEBUG
  Serial.printf("User Reg value after : %X\n",userReg);
  #endif

  return toBeReturned;
}

boolean STS21::reset()
{
  boolean result;
  twi->beginTransmission(address);
  twi->write(SOFT_RST);
  result = twi->endTransmission() == 0;
  delay(30);
  return result;
}

boolean STS21::lowBattery()
{
  uint8_t userReg = readUserRegister();
  return userReg & END_OF_BAT;
}