#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_

#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <cstdint>

namespace L3GD20H {

static constexpr uint8_t READ_MASK = 0b1000'0000;

static constexpr uint8_t AUTO_INCREMENT_MASK = 0b0100'0000;

static constexpr uint8_t WHO_AM_I_REG = 0b0000'1111;
static constexpr uint8_t WHO_AM_I_VAL = 0b1101'0111;

/*------------------------------------------------------------------------*/
/* Control registers
/*------------------------------------------------------------------------*/
static constexpr uint8_t CTRL_REG_1 = 0b0010'0000;
static constexpr uint8_t CTRL_REG_2 = 0b0010'0001;
static constexpr uint8_t CTRL_REG_3 = 0b0010'0010;
static constexpr uint8_t CTRL_REG_4 = 0b0010'0011;
static constexpr uint8_t CTRL_REG_5 = 0b0010'0100;

// Register 1
static constexpr uint8_t SET_DR_1 = 1 << 7;
static constexpr uint8_t SET_DR_0 = 1 << 6;
static constexpr uint8_t SET_BW_1 = 1 << 5;
static constexpr uint8_t SET_BW_0 = 1 << 4;
static constexpr uint8_t SET_POWER_NORMAL_MODE = 1 << 3;
static constexpr uint8_t SET_Z_ENABLE = 1 << 2;
static constexpr uint8_t SET_X_ENABLE = 1 << 1;
static constexpr uint8_t SET_Y_ENABLE = 1;

static constexpr uint8_t CTRL_REG_1_VAL = SET_POWER_NORMAL_MODE | SET_Z_ENABLE |
		SET_Y_ENABLE | SET_X_ENABLE;

// Register 2
static constexpr uint8_t EXTERNAL_EDGE_ENB = 1 << 7;
static constexpr uint8_t LEVEL_SENSITIVE_TRIGGER = 1 << 6;
static constexpr uint8_t SET_HPM_1 = 1 << 5;
static constexpr uint8_t SET_HPM_0 = 1 << 4;
static constexpr uint8_t SET_HPCF_3 = 1 << 3;
static constexpr uint8_t SET_HPCF_2 = 1 << 2;
static constexpr uint8_t SET_HPCF_1 = 1 << 1;
static constexpr uint8_t SET_HPCF_0 = 1;

static constexpr uint8_t CTRL_REG_2_VAL = 0b0000'0000;

// Register 3
static constexpr uint8_t CTRL_REG_3_VAL = 0b0000'0000;

// Register 4
static constexpr uint8_t SET_BNU = 1 << 7;
static constexpr uint8_t SET_BLE = 1 << 6;
static constexpr uint8_t SET_FS_1 = 1 << 5;
static constexpr uint8_t SET_FS_0 = 1 << 4;
static constexpr uint8_t SET_IMP_ENB = 1 << 3;
static constexpr uint8_t SET_SELF_TEST_ENB_1 = 1 << 2;
static constexpr uint8_t SET_SELF_TEST_ENB_0 = 1 << 1;
static constexpr uint8_t SET_SPI_IF_SELECT = 1;

static constexpr uint8_t CTRL_REG_4_VAL = 0b0000'0000;

// Register 5
static constexpr uint8_t SET_REBOOT_MEM = 1 << 7;
static constexpr uint8_t SET_FIFO_ENB = 1 << 6;

static constexpr uint8_t CTRL_REG_5_VAL = 0b0000'0000;

// In degrees per second (DPS) for now.
static constexpr uint16_t RANGE_DPS_00 = 245;
static constexpr uint16_t RANGE_DPS_01 = 500;
static constexpr uint16_t RANGE_DPS_11 = 2000;

static constexpr uint8_t READ_START = CTRL_REG_1;
static constexpr size_t READ_LEN = 14;

// Indexing
static constexpr uint8_t REFERENCE_IDX = 6;
static constexpr uint8_t TEMPERATURE_IDX = 7;
static constexpr uint8_t STATUS_IDX = 8;
static constexpr uint8_t OUT_X_L = 9;
static constexpr uint8_t OUT_X_H = 10;
static constexpr uint8_t OUT_Y_L = 11;
static constexpr uint8_t OUT_Y_H = 12;
static constexpr uint8_t OUT_Z_L = 13;
static constexpr uint8_t OUT_Z_H = 14;

/*------------------------------------------------------------------------*/
/* Device Handler specific
/*------------------------------------------------------------------------*/
static constexpr DeviceCommandId_t READ_REGS = 0;
static constexpr DeviceCommandId_t CONFIGURE_CTRL_REGS = 1;
static constexpr DeviceCommandId_t READ_CTRL_REGS = 2;

static constexpr uint32_t GYRO_DATASET_ID = READ_REGS;

enum GyroPoolIds: lp_id_t {
    ANG_VELOC_X,
	ANG_VELOC_Y,
	ANG_VELOC_Z,
	TEMPERATURE
};

class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
public:
	GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
        StaticLocalDataSet(hkOwner, GYRO_DATASET_ID) {}

	GyroPrimaryDataset(object_id_t mgmId):
        StaticLocalDataSet(sid_t(mgmId, GYRO_DATASET_ID)) {}

	// Angular velocities in degrees per second (DPS)
    lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId,
    		ANG_VELOC_X, this);
    lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId,
    		ANG_VELOC_Y, this);
    lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId,
    		ANG_VELOC_Z, this);
    lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
    		TEMPERATURE, this);
};

}



#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */