#include "TelaireT6713.h"

TelaireT6713::TelaireT6713(uint8_t address, uint8_t sda, uint8_t scl)
  : _address(address), _sda(sda), _scl(scl) {}

void TelaireT6713::setI2CPins(uint8_t sda, uint8_t scl) {
  _sda = sda;
  _scl = scl;
}

void TelaireT6713::setAddress(uint8_t address) {
  _address = address;
}

void TelaireT6713::begin() {
  Wire.begin(_sda, _scl);
  delay(1000);
  Serial.println("Telaire T6713 Initialized.");
  printSensorStatus();
}

int TelaireT6713::readCO2(bool debug) {
  int data[4];

  Wire.beginTransmission(_address);
  Wire.write(0x04); Wire.write(0x13); Wire.write(0x8B);
  Wire.write(0x00); Wire.write(0x01);
  Wire.endTransmission();

  delay(2000);

  Wire.requestFrom(_address, (uint8_t)4);
  if (Wire.available() < 4) {
    if (debug) Serial.println("Error: Less than 4 bytes received");
    return -1;
  }

  data[0] = Wire.read();
  data[1] = Wire.read();
  data[2] = Wire.read();
  data[3] = Wire.read();

  if (debug) {
    Serial.print("FUNC: 0x"); Serial.println(data[0], HEX);
    Serial.print("BYTE COUNT: 0x"); Serial.println(data[1], HEX);
    Serial.print("MSB: 0x"); Serial.println(data[2], HEX);
    Serial.print("LSB: 0x"); Serial.println(data[3], HEX);
  }

  int ppm = ((data[2] & 0x3F) << 8) | data[3];
  return ppm;
}

void TelaireT6713::enableABC() {
  uint8_t cmd[] = { 0x05, 0x03, 0xEE, 0xFF, 0x00 };
  sendCommand(cmd, sizeof(cmd));
  Serial.println("ABC Enabled.");
}

void TelaireT6713::disableABC() {
  uint8_t cmd[] = { 0x05, 0x03, 0xEE, 0x00, 0x00 };
  sendCommand(cmd, sizeof(cmd));
  Serial.println("ABC Disabled.");
}

void TelaireT6713::printSensorStatus() {
  uint8_t statusCmd[] = { 0x04, 0x13, 0x8A, 0x00, 0x01 };
  sendCommand(statusCmd, sizeof(statusCmd));
  delay(100);

  uint8_t response[4];
  if (!readBytes(response, 4)) {
    Serial.println("Failed to read sensor status.");
    return;
  }

  uint8_t msb = response[2];
  uint8_t lsb = response[3];

  Serial.print("Sensor status: ");
  if (msb == 0x00 && lsb == 0x00) Serial.println("No error.");
  else if (msb == 0x00 && lsb == 0x01) Serial.println("Error condition.");
  else if (msb == 0x00 && lsb == 0x02) Serial.println("Flash error.");
  else if (msb == 0x00 && lsb == 0x03) Serial.println("Calibration error.");
  else if (msb == 0x04 && lsb == 0x00) Serial.println("Reboot.");
  else if (msb == 0x08 && lsb == 0x00) Serial.println("Warm-up mode.");
  else if (msb == 0x80 && lsb == 0x00) Serial.println("Single point calibration.");
  else {
    Serial.print("Unknown status (0x");
    Serial.print(msb, HEX);
    Serial.print(" 0x");
    Serial.print(lsb, HEX);
    Serial.println(")");
  }
}

void TelaireT6713::sendCommand(const uint8_t* cmd, size_t len) {
  Wire.beginTransmission(_address);
  for (size_t i = 0; i < len; ++i) {
    Wire.write(cmd[i]);
  }
  Wire.endTransmission();
}

bool TelaireT6713::readBytes(uint8_t* buffer, size_t length) {
  Wire.requestFrom(_address, (uint8_t)length);
  size_t i = 0;
  while (Wire.available() && i < length) {
    buffer[i++] = Wire.read();
  }
  return i == length;
}