#include "PMS.h"
#include "../Main/BoardDef.h"

/**
 * @brief Initializes the sensor and attempts to read data.
 *
 * @param HardwareSerial UART stream
 * @return true Sucecss
 * @return false Failure
 */
bool PMSBase::begin(HardwareSerial &serial) {
  failCount = 0;
  _connected = false;

  // Run and check sensor data for 4sec
  int bytesCleared = serial.available();
  serial.flush();
  if (bytesCleared) {
    Serial.printf("cleared %d byte(s)\n", bytesCleared);
  }

  // explicitly put the sensor into active mode, this seems to be be needed for
  // the Cubic PM2009X
  Serial.printf("Setting active mode\n");
  uint8_t activeModeCommand[] = {0x42, 0x4D, 0xE1, 0x00, 0x01, 0x01, 0x71};
  size_t bytesWritten =
      serial.write(activeModeCommand, sizeof(activeModeCommand));
  Serial.printf("%d byte(s) written\n", bytesWritten);

  unsigned long lastInit = millis();
  while (true) {
    readPackage(serial);
    if (_connected) {
      break;
    }

    delay(1);
    unsigned long ms = (unsigned long)(millis() - lastInit);
    if (ms >= 4000) {
      Serial.println("Initialize PMS sensor: Timeout");
      break;
    }
  }

  if (_connected) {
    Serial.println("Initialize PMS sensor");
  }

  return _connected;
}

/**
 * @brief Read PMS package send to device each 1sec
 *
 * @param serial
 */
void PMSBase::readPackage(HardwareSerial &serial) {
  /** If readPackage has process as period larger than READ_PACKAGE_TIMEOUT,
   * should be clear the lastPackage and readBufferIndex */
  if (lastReadPackage) {
    unsigned long ms = (unsigned long)(millis() - lastReadPackage);
    if (ms >= READ_PACKGE_TIMEOUT) {
      /** Clear buffer */
      readBufferIndex = 0;

      /** Disable check read package timeout */
      lastPackage = 0;

      Serial.println("Last process timeout, clear buffer and last handle package");
    }

    lastReadPackage = millis();
    if (!lastReadPackage) {
      lastReadPackage = 1;
    }
  } else {
    lastReadPackage = millis();
    if (!lastReadPackage) {
      lastReadPackage = 1;
    }
  }

  /** Count to call delay() to release the while loop MCU resource for avoid the
   * watchdog time reset */
  uint8_t delayCount = 0;
  while (serial.available()) {
    /** Get value */
    uint8_t value = (uint8_t)serial.read();

    /** Process receiving package... */
    switch (readBufferIndex) {
    case 0: /** Start byte 1 */
      if (value == 0x42) {
        readBuffer[readBufferIndex++] = value;
      }
      break;
    case 1: /** Start byte 2 */
      if (value == 0x4d) {
        readBuffer[readBufferIndex++] = value;
      } else {
        readBufferIndex = 0;
      }
      break;
    case 2: /** Frame length */
      if (value == 0x00) {
        readBuffer[readBufferIndex++] = value;
      } else {
        readBufferIndex = 0;
      }
      break;
    case 3: /** Frame length */
      if (value == 0x1C) {
        readBuffer[readBufferIndex++] = value;
      } else {
        readBufferIndex = 0;
      }
      break;
    default: /** Data */
    {
      readBuffer[readBufferIndex++] = value;

      /** Check that received full bufer */
      if (readBufferIndex >= sizeof(readBuffer)) {
        /** validata package */
        if (validate(readBuffer)) {
          _connected = true; /** Set connected status */

          /** Parse data */
          parse(readBuffer);

          /** Set last received package */
          lastPackage = millis();
          if (lastPackage == 0) {
            lastPackage = 1;
          }
        }

        /** Clear buffer index */
        readBufferIndex = 0;
      }
      break;
    }
    }

    /** Avoid task watchdog timer reset... */
    delayCount++;
    if (delayCount >= 32) {
      delayCount = 0;
      delay(1);
    }
  }

  /** Check that sensor removed */
  if (lastPackage) {
    unsigned long ms = (unsigned long)(millis() - lastPackage);
    if (ms >= READ_PACKGE_TIMEOUT) {
      lastPackage = 0;
      _connected = false;
    }
  }
}

/**
 * @brief Increate number of fail
 *
 */
void PMSBase::updateFailCount(void) {
  if (failCount < failCountMax) {
    failCount++;
  }
}

void PMSBase::resetFailCount(void) { failCount = 0; }

/**
 * @brief Get number of fail
 *
 * @return int
 */
int PMSBase::getFailCount(void) { return failCount; }

int PMSBase::getFailCountMax(void) { return failCountMax; }

/**
 * @brief Read PMS 0.1 ug/m3 with CF = 1 PM estimates
 *
 * @return uint16_t
 */
uint16_t PMSBase::getRaw0_1(void) { return pms_raw0_1; }

/**
 * @brief Read PMS 2.5 ug/m3 with CF = 1 PM estimates
 *
 * @return uint16_t
 */
uint16_t PMSBase::getRaw2_5(void) { return pms_raw2_5; }

/**
 * @brief Read PMS 10 ug/m3 with CF = 1 PM estimates
 *
 * @return uint16_t
 */
uint16_t PMSBase::getRaw10(void) { return pms_raw10; }

/**
 * @brief Read PMS 0.1 ug/m3
 *
 * @return uint16_t
 */
uint16_t PMSBase::getPM0_1(void) { return pms_pm0_1; }

/**
 * @brief Read PMS 2.5 ug/m3
 *
 * @return uint16_t
 */
uint16_t PMSBase::getPM2_5(void) { return pms_pm2_5; }

/**
 * @brief Read PMS 10 ug/m3
 *
 * @return uint16_t
 */
uint16_t PMSBase::getPM10(void) { return pms_pm10; }

/**
 * @brief Get numnber concentrations over 0.3 um/0.1L
 *
 * @return uint16_t
 */
uint16_t PMSBase::getCount0_3(void) { return pms_count0_3; }

/**
 * @brief Get numnber concentrations over 0.5 um/0.1L
 *
 * @return uint16_t
 */
uint16_t PMSBase::getCount0_5(void) { return pms_count0_5; }

/**
 * @brief Get numnber concentrations over 1.0 um/0.1L
 *
 * @return uint16_t
 */
uint16_t PMSBase::getCount1_0(void) { return pms_count1_0; }

/**
 * @brief Get numnber concentrations over 2.5 um/0.1L
 *
 * @return uint16_t
 */
uint16_t PMSBase::getCount2_5(void) { return pms_count2_5; }

bool PMSBase::connected(void) { return _connected; }

/**
 * @brief Get numnber concentrations over 5.0 um/0.1L (only PMS5003)
 *
 * @return uint16_t
 */
uint16_t PMSBase::getCount5_0(void) { return pms_count5_0; }

/**
 * @brief Get numnber concentrations over 10.0 um/0.1L (only PMS5003)
 *
 * @return uint16_t
 */
uint16_t PMSBase::getCount10(void) { return pms_count10; }

/**
 * @brief Get temperature (only PMS5003T)
 *
 * @return uint16_t
 */
int16_t PMSBase::getTemp(void) { return pms_temp; }

/**
 * @brief Get humidity (only PMS5003T)
 *
 * @return uint16_t
 */
uint16_t PMSBase::getHum(void) { return pms_hum; }

/**
 * @brief Get firmware version code
 *
 * @return uint8_t
 */
uint8_t PMSBase::getFirmwareVersion(void) { return pms_firmwareVersion; }

/**
 * @brief Ge PMS5003 error code
 *
 * @return uint8_t
 */
uint8_t PMSBase::getErrorCode(void) { return pms_errorCode; }

/**
 * @brief Convert PMS2.5 to US AQI unit
 *
 * @param pm02
 * @return int
 */
int PMSBase::pm25ToAQI(int pm02) {
  if (pm02 <= 12.0)
    return ((50 - 0) / (12.0 - .0) * (pm02 - .0) + 0);
  else if (pm02 <= 35.4)
    return ((100 - 50) / (35.4 - 12.0) * (pm02 - 12.0) + 50);
  else if (pm02 <= 55.4)
    return ((150 - 100) / (55.4 - 35.4) * (pm02 - 35.4) + 100);
  else if (pm02 <= 150.4)
    return ((200 - 150) / (150.4 - 55.4) * (pm02 - 55.4) + 150);
  else if (pm02 <= 250.4)
    return ((300 - 200) / (250.4 - 150.4) * (pm02 - 150.4) + 200);
  else if (pm02 <= 350.4)
    return ((400 - 300) / (350.4 - 250.4) * (pm02 - 250.4) + 300);
  else if (pm02 <= 500.4)
    return ((500 - 400) / (500.4 - 350.4) * (pm02 - 350.4) + 400);
  else
    return 500;
}

/**
 * @brief Correction PM2.5
 *
 * Formula: https://www.airgradient.com/documentation/correction-algorithms/
 *
 * @param pm25 Raw PM2.5 value
 * @param humidity Humidity value (%)
 * @return int
 */
int PMSBase::compensate(int pm25, float humidity) {
  float value;
  float fpm25 = pm25;
  if (humidity < 0) {
    humidity = 0;
  }
  if (humidity > 100) {
    humidity = 100.0f;
  }

  if(pm25 < 30) { /** pm2.5 < 30 */
    value = (fpm25 * 0.524f) - (humidity * 0.0862f) + 5.75f;
  } else if(pm25 < 50) { /** 30 <= pm2.5 < 50 */
    value = (0.786f * (fpm25 * 0.05f - 1.5f) + 0.524f * (1.0f - (fpm25 * 0.05f - 1.5f))) * fpm25 - (0.0862f * humidity) + 5.75f;
  } else if(pm25 < 210) { /** 50 <= pm2.5 < 210 */
    value = (0.786f * fpm25) - (0.0862f * humidity) + 5.75f;
  } else if(pm25 < 260) { /** 210 <= pm2.5 < 260 */
    value = (0.69f * (fpm25 * 0.02f - 4.2f) + 0.786f * (1.0f - (fpm25 * 0.02f - 4.2f))) * fpm25 - (0.0862f * humidity * (1.0f - (fpm25 * 0.02f - 4.2f))) + (2.966f * (fpm25 * 0.02f - 4.2f)) + (5.75f * (1.0f - (fpm25 * 0.02f - 4.2f))) + (8.84f * (1.e-4) * fpm25 * fpm25 * (fpm25 * 0.02f - 4.2f));
  } else { /** 260 <= pm2.5 */
    value = 2.966f + (0.69f * fpm25) + (8.84f * (1.e-4) * fpm25 * fpm25);
  }

  if (value < 0) {
    value = 0;
  }

  return (int)value;
}

/**
 * @brief   Convert two byte value to uint16_t value
 *
 * @param buf bytes array (must be >= 2)
 * @return int16_t
 */
int16_t PMSBase::toI16(const uint8_t *buf) {
  int16_t value = buf[0];
  value = (value << 8) | buf[1];
  return value;
}

uint16_t PMSBase::toU16(const uint8_t *buf) {
  uint16_t value = buf[0];
  value = (value << 8) | buf[1];
  return value;
}

/**
 * @brief Validate package data
 *
 * @param buf Package buffer
 * @return true Success
 * @return false Failed
 */
bool PMSBase::validate(const uint8_t *buf) {
  uint16_t sum = 0;
  for (int i = 0; i < 30; i++) {
    sum += buf[i];
  }
  if (sum == toU16(&buf[30])) {
    return true;
  }
  return false;
}

void PMSBase::parse(const uint8_t *buf) {
  pms_raw0_1 = toU16(&buf[4]);
  pms_raw2_5 = toU16(&buf[6]);
  pms_raw10 = toU16(&buf[8]);
  pms_pm0_1 = toU16(&buf[10]);
  pms_pm2_5 = toU16(&buf[12]);
  pms_pm10 = toU16(&buf[14]);
  pms_count0_3 = toU16(&buf[16]);
  pms_count0_5 = toU16(&buf[18]);
  pms_count1_0 = toU16(&buf[20]);
  pms_count2_5 = toU16(&buf[22]);
  pms_count5_0 = toU16(&buf[24]);
  pms_count10 = toU16(&buf[26]);
  pms_temp = toU16(&buf[24]);
  pms_hum = toU16(&buf[26]);
  pms_firmwareVersion = buf[28];
  pms_errorCode = buf[29];
}