#include "ConfidenceModule.h"
#include "HistoryModule.h"

#include <stdio.h>

// TODO: at the moment are only positive rates_of_changes allowed!!
void ConfidenceModule::initialize_confidence_validator(
    float lower_bound, bool flag_lower_bound_exist, float upper_bound,
    bool flag_upper_bound_exist, float rates_of_change,
    bool flag_rates_of_change_exist) {
  // plausibility
  if (lower_bound <= upper_bound) {
    this->lower_bound = lower_bound;
    this->upper_bound = upper_bound;
    this->flag_lower_bound_exist = flag_lower_bound_exist;
    this->flag_upper_bound_exist = flag_upper_bound_exist;
  } else {
    this->lower_bound = upper_bound;
    this->upper_bound = lower_bound;
    this->flag_lower_bound_exist = flag_upper_bound_exist;
    this->flag_upper_bound_exist = flag_lower_bound_exist;
  }

  // consistency
  this->rates_of_change = rates_of_change;
  this->flag_rates_of_change_exist = flag_rates_of_change_exist;
  flag_value_got_inconsistence = false;

  // TODO: Changes of Other Sensors (e.g. Environment Temperature Sensor)
}

ConfidenceModule::ConfidenceModule(float lower_bound,
                                   bool flag_lower_bound_exist,
                                   float upper_bound,
                                   bool flag_upper_bound_exist,
                                   float rates_of_change,
                                   bool flag_rates_of_change_exist) {
  set_name(NO_NAME);
  initialize_confidence_validator(lower_bound, flag_lower_bound_exist,
                                  upper_bound, flag_upper_bound_exist,
                                  rates_of_change, flag_rates_of_change_exist);
}

ConfidenceModule::ConfidenceModule(char *name, float lower_bound,
                                   bool flag_lower_bound_exist,
                                   float upper_bound,
                                   bool flag_upper_bound_exist,
                                   float rates_of_change,
                                   bool flag_rates_of_change_exist) {
  set_name(name);
  initialize_confidence_validator(lower_bound, flag_lower_bound_exist,
                                  upper_bound, flag_upper_bound_exist,
                                  rates_of_change, flag_rates_of_change_exist);
}

bool ConfidenceModule::validate_confidence(
    float input /*, HistoryModule* historyModule*/) {

  bool flag_confidence = true;

  // bounds
  if (flag_lower_bound_exist && input < lower_bound) {
    flag_confidence = false;
    // printf("lb\ninput = %f\n", input);
  }

  if (flag_upper_bound_exist && input > upper_bound) {
    flag_confidence = false;
    // printf("ub\n");
  }

  // rates_of_change
  float trend_abs = .0;
  // TODO: time_unites and time_base should be configurable and saved for the
  // object
  // unsigned int time_units = 2;
  bool got_trend =
      false; // XXX historyModule->get_history_trend_absolutely(&trend_abs,
             // time_units, TB_SECONDS);

  if (got_trend) {
    if (trend_abs > rates_of_change) {
      flag_confidence = false;
      // printf("trend\n");

      // VERSUCH
      if (flag_value_got_inconsistence == false) {
        // printf("setze flag\n");
        flag_value_got_inconsistence = true;
        value_before_value_got_inconsistence =
            0; // XXX historyModule->get_value_ago(time_units);
      }
    }
  } else {
    // printf("no trend\n");
  }

  if (flag_value_got_inconsistence) {

    // TODO: nicht hardcoded
    float seconds = 20;

    // printf("tradition\n");

    if (input == value_before_value_got_inconsistence) {
      flag_value_got_inconsistence = false;
    } else if (input < value_before_value_got_inconsistence) {
      if (input >=
          value_before_value_got_inconsistence - rates_of_change * seconds) {
        flag_value_got_inconsistence = false;
      } else {
        flag_confidence = false;
      }
    } else if (input > value_before_value_got_inconsistence) {
      if (input <=
          value_before_value_got_inconsistence + rates_of_change * seconds) {
        flag_value_got_inconsistence = false;
      } else {
        flag_confidence = false;
      }
    } else {
      flag_confidence = false;
    }
  }

  confidence = flag_confidence;
  return confidence;
}

bool ConfidenceModule::get_confidence() { return confidence; }

void ConfidenceModule::set_lower_bound(float lower_bound) {
  this->lower_bound = lower_bound;
}

float ConfidenceModule::get_lower_bound() { return lower_bound; }

void ConfidenceModule::set_flag_lower_bound_exist(bool flag) {
  this->flag_lower_bound_exist = flag;
}

bool ConfidenceModule::get_flag_lower_bound_exist() {
  return flag_lower_bound_exist;
}

void ConfidenceModule::set_upper_bound(float upper_bound) {
  this->upper_bound = upper_bound;
}

float ConfidenceModule::get_upper_bound() { return upper_bound; }

void ConfidenceModule::set_flag_upper_bound_exist(bool flag) {
  this->flag_upper_bound_exist = flag;
}

bool ConfidenceModule::get_flag_upper_bound_exist() {
  return flag_upper_bound_exist;
}

void ConfidenceModule::set_rates_of_change(float rates) {
  this->rates_of_change = rates;
}

float ConfidenceModule::get_rates_of_change() { return rates_of_change; }

void ConfidenceModule::set_flag_rates_of_change_exist(bool flag) {
  this->flag_rates_of_change_exist = flag;
}

bool ConfidenceModule::get_flag_rates_of_change_exist() {
  return flag_rates_of_change_exist;
}