#include "ezo_orp.h"

namespace esphome {
namespace ezo_orp_i2c {

static const char *const TAG = "ezo_orp_i2c.sensor";

static const uint8_t COMMAND_READ[] = {0x52}; // R
static const uint8_t COMMAND_PERFERM_CALIBRATION[] = {0x43, 0x61, 0x6C}; //Cal
static const uint8_t COMMAND_IS_CALIBRATED[] = {0x43, 0x61, 0x6C, 0x2C, 0x3F}; //Cal
static const uint8_t COMMAND_EXPORT_CALIBRATION[] = {0x45, 0x78, 0x70, 0x6F, 0x72, 0x74}; // Export
static const uint8_t COMMAND_INFORMATION[] = {0x69}; // i (DON'T ASK ME WHY THAT IS ONLY A SMALL LETTER
static const uint8_t COMMAND_STATUS[] = {0x53, 0x74, 0x61, 0x74, 0x75, 0x73}; // STATUS

static const uint8_t COMMAND_FACTORY_RESET[] = {0x46, 0x61, 0x63, 0x74, 0x6F, 0x72, 0x79}; // Factory

static const uint8_t COMMMAND_TO_ARGS_SEPERATOR = {0x2C};

static const int PROCESSING_DELAY_MS = 300; // FOR SOME OPERATIONS
static const int READING_DELAY_MS = 900; // FOR BASICALLY EVERYTHING WHERE LARGE DATA IS READ/SENT


void EzoOrpSensor::setup(){
  ESP_LOGCONFIG(TAG, "Setting up EzoOrpI2C '%s'...", this->get_name().c_str());
  ESP_LOGCONFIG(TAG, "EzoOrpI2C '%s' setup finished!", this->get_name().c_str());
  scheduleNextAction(SensorAction::IS_CALIBRATED, nullptr, 0, 450);
}

void EzoOrpSensor::update() {
  scheduleNextAction(SensorAction::READ, nullptr, 0, 900);
}

void EzoOrpSensor::read_response(int maxLength, SensorAction action) {
  uint8_t data[maxLength];

  if(SensorAction::CALIBRATE == action){
    ESP_LOGD(TAG, "Device Calibrated");
    busy_ = false;
    runScheduledAction();
    return;
  }

  if(i2c::ERROR_OK != this->read(data, maxLength)){
    ESP_LOGW(TAG, "Error Occured while reading respnse!");
    return;
  }

  if(data[0] != 1 && data[0] != 255){
    ESP_LOGW(TAG, "Read status code of %d, (Operation: %d)", data[0], action);
    return;
  }

  if(SensorAction::READ == action){
    float value;
    sscanf((const char*) &data[1], "%f", &value);
    this->publish_state(value);
  }
  else if(SensorAction::IS_CALIBRATED == action){
    ESP_LOGD(TAG, "%s", data[6] == 0x31 ? "Device has been calibrated" : "Device has NOT been calibrated");
  }
  else if(SensorAction::INFORMATION == action){
    ESP_LOGD(TAG, "Device Printed the following information: %s", &data[1]);
  }
  
  busy_ = false;

  runScheduledAction();
}

void EzoOrpSensor::calibrate_to_target(int target){
  char targetArray[3];
  sprintf(targetArray, "%d", target);
  ESP_LOGD(TAG, "Calibrating to: %3s", targetArray);
  scheduleNextAction(SensorAction::CALIBRATE, (uint8_t*) &targetArray, 3, 4000);

}

float EzoOrpSensor::get_setup_priority() const { return setup_priority::DATA; }

void EzoOrpSensor::dump_config(){
  LOG_SENSOR("", "EzoOrpI2C Sensor", this);
  LOG_UPDATE_INTERVAL(this);
  LOG_I2C_DEVICE(this);
}

bool EzoOrpSensor::is_busy(){
  return this->busy_;
}

void EzoOrpSensor::runScheduledAction(){
  if(scheduledAction_ == SensorAction::NOTHING) return; 
  scheduleNextAction(scheduledAction_, scheduledActionData_, scheduledActionDataSize_, scheduledActionTime_);
  scheduledAction_ = SensorAction::NOTHING;
  scheduledActionDataSize_ = 0;
}

void EzoOrpSensor::scheduleNextAction(SensorAction action, uint8_t* data, int size, int expected_response_time){
  if(busy_){
    if(scheduledAction_ == action) return; // Queued Action cannot has no need of being repeated;
    if(scheduledAction_ != SensorAction::NOTHING) return; // Ehh... Felt better lol
    
    scheduledAction_ = action;
    if(data != nullptr){
      for(int i = 0;i < size; ++i){
        scheduledActionData_[i] = data[i];
      }
    }
    scheduledActionDataSize_ = size;
    scheduledActionTime_ = expected_response_time;
    return;
  }

  switch(action){
    case SensorAction::NOTHING:
      return;
    case SensorAction::READ:
      busy_ = true;
      send_command(COMMAND_READ, sizeof(COMMAND_READ));
      this->set_timeout(expected_response_time, [this](){ this->read_response(9, SensorAction::READ); });
      break;
    case SensorAction::CALIBRATE:
      busy_ = true;
      send_command(COMMAND_PERFERM_CALIBRATION, sizeof(COMMAND_PERFERM_CALIBRATION), scheduledActionData_, scheduledActionDataSize_);
      this->set_timeout(expected_response_time, [this](){ this->read_response(2, SensorAction::CALIBRATE); });
      break;
    case SensorAction::IS_CALIBRATED:
      busy_ = true;
      send_command(COMMAND_IS_CALIBRATED, sizeof(COMMAND_IS_CALIBRATED));
      this->set_timeout(expected_response_time, [this](){ this->read_response(8, SensorAction::IS_CALIBRATED); });
      break;
    case SensorAction::INFORMATION:
      busy_ = true;
      send_command(COMMAND_STATUS, sizeof(COMMAND_STATUS));
      this->set_timeout(expected_response_time, [this](){ this->read_response(18, SensorAction::INFORMATION); });
      break;
    default:
      break;
  }

}

bool EzoOrpSensor::send_command(const uint8_t* data, size_t dataSize){
  return send_command(data, dataSize, nullptr, 0);
}
bool EzoOrpSensor::send_command(const uint8_t* data, size_t dataSize, const uint8_t* parameterData, size_t parameterSize){

  if(parameterData == nullptr){
    if(i2c::ERROR_OK != this->write(data, dataSize)){
      ESP_LOGW(TAG, "send_command failed writing data!");
      return false;
    }
    return true;
  }
  
  uint8_t completeData[dataSize + 1 + parameterSize];
  for(int i = 0;i < dataSize; ++i){
    completeData[i] = data[i];
  }
  completeData[dataSize++] = COMMMAND_TO_ARGS_SEPERATOR;
  for(int i = 0; i < parameterSize; ++i){
    completeData[dataSize + i] = parameterData[i];
  }

  if(i2c::ERROR_OK != this->write(completeData, sizeof(completeData))){
    ESP_LOGW(TAG, "send_command failed writing data!");
    return false;
  }
  return true;
}

void EzoOrpSensor::print_info(){
  scheduleNextAction(SensorAction::INFORMATION, nullptr, 0, 300);
}

}
}