#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include <MechaQMC5883.h>

#ifndef SensorMagnetometroModel
#define SensorMagnetometroModel

class SensorMagnetometro {
  public:

    SensorMagnetometro(String _modID, String _id, int _taxaAmostragem = 2000) {
      Mod_ID = _modID;
      _ID = _id;
    }

    // Variáveis
    MechaQMC5883* qmc = new MechaQMC5883();

    // Definições
    String Mod_ID;
    String _ID;
    S_Code _componente = sMAG;
    byte _Endereco;

    bool Iniciado = false;

    void Inicializar() {
      if (Iniciado) {
        //EnviarDadosSerial(_ID + " ja inicializado");
        //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      qmc.init();
      
      Iniciado = true;

      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void Desligar() {
      if (!Iniciado) {
        //EnviarDadosSerial(_ID + " nao esta inicializado");
        //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      //EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void AferirOrientacao() {
      int x, y, z;
      float orientacao;
      qmc.read(&x, &y, &z,&orientacao);
    }

    void RequisitarDados() {
      AferirOrientacao();

      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sMAG, _ID, MontarDadosProtocoloMagnetometro(x, y, z, orientacao)));
    }


};

#endif