/**
 * @file sensorData.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the SensorData class
 * @version 0.1
 * @date 2023-09-02
 *
 * @copyright Copyright (c) 2023
 *
 */

#ifndef SENSOR_DATA_H
#define SENSOR_DATA_H

#include <SPI.h>
#include <I2Cdev.h>
#include <iostream>

#include "component.h"
#include "calibrateCompass.h"

#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include "calcAzimuth.h"
#include "ntripClient.h"

#include "point.h"

/**
 * @brief A class to manage all sensors
 *
 * Each sensor have separately to be enabled
 */
class SensorData : public Component
{
public:
    SensorData();
    ~SensorData();

    void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);

    /**
     * @brief Enable the gnss module over spi
     *
     * @param spiPort
     * @param csPin
     */
    void enableGnss(SPIClass *spiPort, uint8_t csPin);

    /**
     * @brief Enable the gnss module over i2c
     */
    void enableGnss();

    void enableRealCompass();
    void enableCalcCompass();
    void enableGyroscope();

    // Interface Const
    /**
     * @brief Get the azimuth measured by the compass module
     *
     * @return int16_t
     */
    int16_t getRealAzimuth() const { return this->realAzimuth; }

    /**
     * @brief Get the azimuth calculated by CalcAzimuth
     *
     * Consider to call getCalcAzimuthState() to check, if the data is valid.
     *
     * @return int16_t
     */
    int16_t getCalcAzimuth() const { return this->calcAzimuth; }

    /**
     * @brief Get the CalcAzimuth::State object
     *
     * Needed to check the quality of calculated azimuth
     *
     * @return CalcAzimuth::State
     */
    CalcAzimuth::State getCalcAzimuthState() const;

    Point getCurrentPos() const { return this->currentPosition; }
    const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
    NTRIPClientStates getNtripState() const;

    /**
     * @brief Get the data from the gyroscope
     *
     * The returned float pointer is an array of 3 floats
     * - Yaw
     * - Pitch
     * - Roll
     *
     * @return const float*
     */
    const float *getGyroData() const { return this->yawPitchRoll; }

    /**
     * @brief Get the CalcCompass object
     * @return CalcAzimuth*
     */
    CalcAzimuth *getCalcCompass() const { return this->calcCompass; }

    /**
     * @brief Get the RealCompass object
     * @return QMC5883LCompass*
     */
    QMC5883LCompass *getRealCompass() const { return this->realCompass; }

    /**
     * @brief Get the NTRIPClient object
     * @return NTRIPClient*
     */
    NTRIPClient *getNtripClient() const { return this->ntripClient; }

    /**
     * @brief Get the Gyroscope object
     * @return MPU6050*
     */
    MPU6050 *getGyroscope() const { return this->gyroscope; }

    // static
    /**
     * @brief Set the output status for PVTdata.
     *
     * If this is true, a lot of information from the gnss module will be printed in
     * the interval of navigation frequency.
     *
     * @param status
     */
    static void setOutputStatusPrintPVTdata(bool status);

private:
    void run() override;
    void runAsChild() override;
    void initGnss();
    void updateUbxData();

    QMC5883LCompass *realCompass = nullptr;
    CalcAzimuth *calcCompass = nullptr;
    SFE_UBLOX_GNSS *gnss = nullptr;
    NTRIPClient *ntripClient = nullptr;
    MPU6050 *gyroscope = nullptr;

    UBX_NAV_PVT_data_t *gnssData = nullptr;
    Point currentPosition;
    Quaternion quaternion;
    VectorFloat gravity;

    char *host = nullptr;
    char *mountPoint = nullptr;
    char *user = nullptr;
    char *password = nullptr;

    bool isNtripInit = false;

    uint8_t gyroBuffer[64];
    uint16_t port = 0;
    int16_t realAzimuth = INT16_MAX;
    int16_t calcAzimuth = INT16_MAX;
    uint32_t lastUbxUpdate = 0;

    float yawPitchRoll[3]{0, 0, 0};

    // static
    static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
    static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);

    static UBX_NAV_PVT_data_t *ubxDataStatic;
    static uint32_t ubxUpdateTimeStatic;
    static bool outputStatusPrintPVTdata;

    static constexpr uint8_t loopDelay = 50;
};

#endif // SENSOR_DATA_H