#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_

#include "fsfw/devicehandlers/DeviceHandlerIF.h"
#include "fsfw/datapoollocal/StaticLocalDataSet.h"

#include <cstddef>

namespace ADIS1650X {

enum class Type {
    ADIS16505,
    ADIS16507
};

static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
static constexpr uint8_t WRITE_MASK = 0b1000'0000;

static constexpr uint32_t GYRO_RANGE = 125;
static constexpr uint32_t ACCELEROMETER_RANGE_16507 = 392;
static constexpr float ACCELEROMETER_RANGE_16505 = 78.4;

static constexpr uint32_t STALL_TIME_MICROSECONDS = 16;

static constexpr uint16_t PROD_ID_16507 = 16507;
static constexpr uint16_t PROD_ID_16505 = 16505;

static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};

static constexpr dur_millis_t START_UP_TIME = 310;
static constexpr dur_millis_t SW_RESET_BREAK = 255;
static constexpr dur_millis_t FACTORY_CALIBRATION_BREAK = 136;
static constexpr dur_millis_t FLASH_MEMORY_UPDATE_BREAK = 70;
static constexpr dur_millis_t FLASH_MEMORY_TEST_BREAK = 30;
static constexpr dur_millis_t SELF_TEST_BREAK = 24;

static constexpr uint8_t DIAG_STAT_REG = 0x02;
static constexpr uint8_t FILTER_CTRL_REG = 0x5c;
static constexpr uint8_t MSC_CTRL_REG = 0x60;
static constexpr uint8_t DEC_RATE_REG = 0x64;
static constexpr uint8_t GLOB_CMD = 0x68;
static constexpr uint8_t PROD_ID_REG = 0x72;

static constexpr DeviceCommandId_t READ_SENSOR_DATA = 0;
static constexpr DeviceCommandId_t READ_OUT_CONFIG = 1;
static constexpr DeviceCommandId_t SELF_TEST_SENSORS = 2;
static constexpr DeviceCommandId_t SELF_TEST_MEMORY = 3;
static constexpr DeviceCommandId_t UPDATE_NV_CONFIGURATION = 4;
static constexpr DeviceCommandId_t SELECT_BURST_READ_MODE = 5;

static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30;
static constexpr DeviceCommandId_t SW_RESET = 31;
static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;

static constexpr uint16_t BURST_32_BIT = 1 << 9;
static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
static constexpr uint16_t POINT_PERCUSSION_COMPENSATION_BIT = 1 << 6;

static constexpr size_t CONFIG_READOUT_SIZE = 10 + 2;
static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;

static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;

enum GlobCmds: uint8_t {
    FACTORY_CALIBRATION = 0b0000'0010,
    SENSOR_SELF_TEST = 0b0000'0100,
    FLASH_MEMORY_UPDATE = 0b0000'1000,
    FLASH_MEMORY_TEST = 0b0001'0000,
    SOFTWARE_RESET = 0b1000'0000
};

enum PrimaryPoolIds: lp_id_t {
    ANG_VELOC_X,
    ANG_VELOC_Y,
    ANG_VELOC_Z,
    ACCELERATION_X,
    ACCELERATION_Y,
    ACCELERATION_Z,
    TEMPERATURE,
    DIAG_STAT_REGISTER,
    FILTER_SETTINGS,
    MSC_CTRL_REGISTER,
    DEC_RATE_REGISTER,
};

enum FilterSettings: uint8_t {
    NO_FILTER = 0,
    TWO_TAPS = 1,
    FOUR_TAPS = 2,
    EIGHT_TAPS = 3,
    SIXTEEN_TAPS = 4,
    THIRTYTWO_TAPS = 5,
    SIXTYFOUR_TAPS = 6
};

}

class AdisGyroPrimaryDataset: public StaticLocalDataSet<8> {
public:

    /** Constructor  for data users like controllers */
    AdisGyroPrimaryDataset(object_id_t adisId):
        StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) {
        setAllVariablesReadOnly();
    }

    /* Angular velocities in degrees per second (DPS) */
    lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId,
            ADIS1650X::ANG_VELOC_X, this);
    lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId,
            ADIS1650X::ANG_VELOC_Y, this);
    lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId,
            ADIS1650X::ANG_VELOC_Z, this);
    lp_var_t<double> accelX = lp_var_t<double>(sid.objectId,
            ADIS1650X::ACCELERATION_X, this);
    lp_var_t<double> accelY = lp_var_t<double>(sid.objectId,
            ADIS1650X::ACCELERATION_Y, this);
    lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId,
            ADIS1650X::ACCELERATION_Z, this);
    lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
            ADIS1650X::TEMPERATURE, this);
private:

    friend class GyroADIS1650XHandler;
    /** Constructor for the data creator */
    AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
            StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {}
};

class AdisGyroConfigDataset: public StaticLocalDataSet<5> {
public:

    /** Constructor  for data users like controllers */
    AdisGyroConfigDataset(object_id_t adisId):
        StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) {
        setAllVariablesReadOnly();
    }

    lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId,
            ADIS1650X::DIAG_STAT_REGISTER);
    lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS);
    lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER);
    lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER);
private:
    friend class GyroADIS1650XHandler;
    /** Constructor for the data creator */
    AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner):
            StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {}
};

#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */