#pragma once

#include <utils/math_utils.h>
#include <peripherals/sensor.hpp>
#include <peripherals/drivers/hmc5883l.h>

struct MagnetometerMsg {
    float rpy[3] {0, 0, 0};
    float heading {-1};
    bool success {false};
};

class Magnetometer : public SensorBase<MagnetometerMsg> {
  public:
    bool initialize(void* _arg) override {
        #if FT_ENABLED(USE_ICM20948)
            #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
                SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1);
                _mag = (ICM_20948_SPI*)_arg;
                #ifndef ICM20948_ALIVE
                #define ICM20948_ALIVE
                _imu->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode");
                #endif
            #else
                _mag = (ICM_20948_I2C*)_arg;
                #ifndef ICM20948_ALIVE
                #define ICM20948_ALIVE
                if (!_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
                #endif
                
            #endif
            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
            
            _mag->startupMagnetometer();
            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
            _msg.success = true;
        #elif FT_ENABLED(USE_HMC5883)
            _msg.success = _mag.begin();
        #endif
        return _msg.success;
    }

    bool update() override {
        if (!_msg.success) return false;
        #if FT_ENABLED(USE_ICM20948)
            _mag->getAGMT();
            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
            _msg.rpy[0] = _mag->magX();
            _msg.rpy[1] = _mag->magY();
            _msg.rpy[2] = _mag->magZ();

        #elif FT_ENABLED(USE_HMC5883)
            sensors_event_t event;
            bool updated = _mag.getEvent(&event);
            if (!updated) return false;
            _msg.rpy[0] = event.magnetic.x;
            _msg.rpy[1] = event.magnetic.y;
            _msg.rpy[2] = event.magnetic.z;
        #endif
        _msg.heading = atan2(_msg.rpy[1], _msg.rpy[0]); // atan2(y, x)
        _msg.heading += declinationAngle;
        if (_msg.heading < 0) _msg.heading += 2 * PI;
        if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
        _msg.heading *= 180 / M_PI;
        return true;
    }

    float getMagX() { return _msg.rpy[0]; }
    float getMagY() { return _msg.rpy[1]; }
    float getMagZ() { return _msg.rpy[2]; }
    float getHeading() { return _msg.heading; }

  private:
    
    #if FT_ENABLED(USE_ICM20948)
        #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
            #define SPI_PORT SPI // TODO in periphearals_seetings.h
            #define CS_PIN 2 
            ICM_20948_SPI* _mag;
        #else
            //#define WIRE_PORT Wire 
            ICM_20948_I2C* _mag;
        #endif
    #elif FT_ENABLED(USE_HMC5883)
        Adafruit_HMC5883_Unified _mag {12345};
    #endif
    const float declinationAngle = 0.22;
};