#define THERM_WHITE 0xB0
#define THERM_BLACK 0xB1
#define THERM_FLOOR_WHITE   0xB2
#define THERM_FLOOR_BLACK   0xB3

#define MAX_MODULES 100
struct moduleStruct {
  uint16_t id = 0x0000;
  uint16_t address = 0x0000; // This represents the modbus address. This must be unique.
};
moduleStruct* modules[MAX_MODULES] = {};

/*
*   moduleSystem discovers the devices and
      it has some utility methods to work with these devices
*/
class moduleSystem{
  public:
    void init(){
      if(modules[0] != nullptr && modules[0] != NULL){
        Serial.print(modules[0]->id);
      }
      discoverDevices();
    }
    // Compares the ID with the possible thermostat ids.
    boolean isThermByID( int id ){
        if( id == THERM_FLOOR_BLACK || id == THERM_FLOOR_WHITE || id == THERM_BLACK || id == THERM_WHITE ){
            return true;
        }
        return false;
    }
    // Returns a pointer to the module if found. Returns nullptr if not.
    moduleStruct* getModuleReference(int address){
      Serial.printf("Searching for module with address %d\n",address);
      int index = findIndex(address);
      if( index == -1 ){ return nullptr; }
      Serial.printf("Found valid index: %d\n",index);
      return modules[index];
    }
  private:
    int counter = 0;

    // this method will check the bus for devices
    // We will simulate this by just creating random modules
    void discoverDevices(){
      for( int i = 0; i < 10; i++){
        modules[i] = new moduleStruct();
        modules[i]->id = random(5,30);
        modules[i]->address = random(5,30);
        counter = i; // so we have a variable for the amount of module
        Serial.printf(
          "New module created with id of %d and address %d\n",
          modules[i]->id,
          modules[i]->address
        );
      }
      Serial.printf("Found %d devices.\n",counter);
    }

    // Searches for the index of a module based on it's address
    int findIndex(uint16_t address){
      for(int i = 0; i < counter; i++){
        if( !modules[i] || modules[i] == nullptr ){ continue; }
        if( modules[i]->address == address ){
          return i;
        }
      }
      return -1;
    }
};
moduleSystem moduleSys;

class Thermostat {
  public:
    void init(){
      getModuleReference();
    }
    void run() {
      if( !isValidModule() ){ return; }
      // Do operations which requires the module object.
      Serial.print("Termostat running...");
    }
  private:
    int _moduleID = 9;
    moduleStruct* module;
    void getModuleReference(){
      // Pass some random id to the method for now.
      module = moduleSys.getModuleReference(23);
      if( !isValidModule() ){
        Serial.printf("Did not found a valid thermostat module.\n");
      }
    }

    boolean isValidModule() {
        if( !module || module == nullptr ){ return false; }
        return moduleSys.isThermByID(module->id);
    }
};
Thermostat thermostat;


void setup() {
  Serial.begin(115200);
  moduleSys.init();
  thermostat.init();
}

void loop() {
  thermostat.run();
  delay(10);
}