#include "metawear/core/module.h"
#include "metawear/core/status.h"
#include "metawear/core/cpp/datasignal_private.h"
#include "metawear/core/cpp/metawearboard_def.h"
#include "metawear/core/cpp/metawearboard_macro.h"
#include "metawear/core/cpp/register.h"
#include "metawear/core/cpp/responseheader.h"

#include "metawear/sensor/gyro_bosch.h"
#include "gyro_bosch_private.h"
#include "gyro_bosch_register.h"
#include "utils.h"

#include <cstdlib>
#include <cstring>
#include <unordered_map>

using std::forward_as_tuple;
using std::malloc;
using std::memcpy;
using std::memset;
using std::piecewise_construct;
using std::stringstream;
using std::unordered_map;
using std::vector;

#define CREATE_BMI160_ROT_SIGNAL_SINGLE(offset) CREATE_BMI160_ROT_SIGNAL(DataInterpreter::BOSCH_ROTATION_SINGLE_AXIS, 1, offset)
#define CREATE_BMI160_ROT_SIGNAL(interpreter, channels, offset) new MblMwDataSignal(GYRO_BMI160_ROT_RESPONSE_HEADER, board, interpreter, \
        FirmwareConverter::BOSCH_ROTATION, channels, 2, 1, offset)
#define CREATE_BMI270_ROT_SIGNAL_SINGLE(offset) CREATE_BMI270_ROT_SIGNAL(DataInterpreter::BOSCH_ROTATION_SINGLE_AXIS, 1, offset)        
#define CREATE_BMI270_ROT_SIGNAL(interpreter, channels, offset) new MblMwDataSignal(GYRO_BMI270_ROT_RESPONSE_HEADER, board, interpreter, \
        FirmwareConverter::BOSCH_ROTATION, channels, 2, 1, offset)

const uint8_t MBL_MW_MODULE_GYRO_TYPE_BMI160 = 0;            ///< Constant identifying the BMI160 accelerometer module type
const uint8_t MBL_MW_MODULE_GYRO_TYPE_BMI270 = 1;            ///< Constant identifying the BMI270 accelerometer module type

const float FSR_SCALE[5]= {16.4f, 32.8f, 65.6f, 131.2f, 262.4f};
const uint8_t PACKED_ROT_REVISION= 1;
const ResponseHeader    GYRO_BMI160_ROT_RESPONSE_HEADER(MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::DATA)),
                        GYRO_BMI160_PACKED_ROT_RESPONSE_HEADER(MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::PACKED_GYRO_DATA)),
                        GYRO_BMI270_ROT_RESPONSE_HEADER(MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::DATA)),
                        GYRO_BMI270_PACKED_ROT_RESPONSE_HEADER(MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::PACKED_GYRO_DATA));

struct GyroBoschConfig {
    uint8_t gyr_odr : 4;
    uint8_t gyr_bwp : 2;
    uint8_t:2;
    uint8_t gyr_range : 3;
    uint8_t:5;
};

struct GyroBoschState {
    MblMwFnBoardPtrInt read_config_completed;
    void *read_config_context;
};

static unordered_map<const MblMwMetaWearBoard*, GyroBoschState> states;

// Helper function - receive a config response
static int32_t received_config_response(MblMwMetaWearBoard *board, const uint8_t *response, uint8_t len) {
    memcpy(board->module_config.at(MBL_MW_MODULE_GYRO), response + 2, sizeof(GyroBoschConfig));

    auto callback = states[board].read_config_completed;
    auto context = states[board].read_config_context;
    states[board].read_config_completed = nullptr;
    states[board].read_config_context = nullptr;
    callback(context, board, MBL_MW_STATUS_OK);

    return MBL_MW_STATUS_OK;
}

// Helper function - get gyro scale
float bosch_gyro_get_data_scale(const MblMwMetaWearBoard *board) {
    return FSR_SCALE[((GyroBoschConfig*) board->module_config.at(MBL_MW_MODULE_GYRO))->gyr_range];
}

// Helpfer function - init module
void init_gyro_module(MblMwMetaWearBoard *board) {
    switch(board->module_info.at(MBL_MW_MODULE_GYRO).implementation) {
    case MBL_MW_MODULE_GYRO_TYPE_BMI160:
        if (board->module_info.count(MBL_MW_MODULE_GYRO) && board->module_info.at(MBL_MW_MODULE_GYRO).present) {
            if (!board->module_config.count(MBL_MW_MODULE_GYRO)) {
                GyroBoschConfig *new_config = (GyroBoschConfig*)malloc(sizeof(GyroBoschConfig));

                memset(new_config, 0, sizeof(GyroBoschConfig));
                new_config->gyr_bwp = 2;
                new_config->gyr_odr = MBL_MW_GYRO_BOSCH_ODR_100Hz;
                new_config->gyr_range = MBL_MW_GYRO_BOSCH_RANGE_2000dps;
                board->module_config.emplace(MBL_MW_MODULE_GYRO, new_config);
            }

            MblMwDataSignal* rotation;
            if (board->module_events.count(GYRO_BMI160_ROT_RESPONSE_HEADER)) {
                rotation = dynamic_cast<MblMwDataSignal*>(board->module_events[GYRO_BMI160_ROT_RESPONSE_HEADER]);
            } else {
                rotation = CREATE_BMI160_ROT_SIGNAL(DataInterpreter::BOSCH_ROTATION, 3, 0);
                board->module_events[GYRO_BMI160_ROT_RESPONSE_HEADER] = rotation;
            }
            if (!rotation->components.size()) {
                rotation->components.push_back(CREATE_BMI160_ROT_SIGNAL_SINGLE(0));
                rotation->components.push_back(CREATE_BMI160_ROT_SIGNAL_SINGLE(2));
                rotation->components.push_back(CREATE_BMI160_ROT_SIGNAL_SINGLE(4));
            }

            board->responses[GYRO_BMI160_ROT_RESPONSE_HEADER]= response_handler_data_no_id;

            if (board->module_info.at(MBL_MW_MODULE_GYRO).revision >= PACKED_ROT_REVISION) {
                if (!board->module_events.count(GYRO_BMI160_PACKED_ROT_RESPONSE_HEADER)) {
                    board->module_events[GYRO_BMI160_PACKED_ROT_RESPONSE_HEADER]= new MblMwDataSignal(GYRO_BMI160_PACKED_ROT_RESPONSE_HEADER, board, 
                        DataInterpreter::BOSCH_ROTATION, FirmwareConverter::BOSCH_ROTATION, 3, 2, 1, 0);
                }
                board->responses[GYRO_BMI160_PACKED_ROT_RESPONSE_HEADER]= response_handler_packed_data;
            }

            board->responses.emplace(piecewise_construct, forward_as_tuple(MBL_MW_MODULE_GYRO, READ_REGISTER(ORDINAL(GyroBmi160Register::CONFIG))),
                    forward_as_tuple(received_config_response));

            GyroBoschState newState = {nullptr};
            states.insert({board, newState});
        }
        break;
    case MBL_MW_MODULE_GYRO_TYPE_BMI270:
        if (board->module_info.count(MBL_MW_MODULE_GYRO) && board->module_info.at(MBL_MW_MODULE_GYRO).present) {
            if (!board->module_config.count(MBL_MW_MODULE_GYRO)) {
                GyroBoschConfig *new_config = (GyroBoschConfig*)malloc(sizeof(GyroBoschConfig));

                memset(new_config, 0, sizeof(GyroBoschConfig));
                new_config->gyr_bwp = 2;
                new_config->gyr_odr = MBL_MW_GYRO_BOSCH_ODR_100Hz;
                new_config->gyr_range = MBL_MW_GYRO_BOSCH_RANGE_2000dps;
                board->module_config.emplace(MBL_MW_MODULE_GYRO, new_config);
            }

            MblMwDataSignal* rotation;
            if (board->module_events.count(GYRO_BMI270_ROT_RESPONSE_HEADER)) {
                rotation = dynamic_cast<MblMwDataSignal*>(board->module_events[GYRO_BMI270_ROT_RESPONSE_HEADER]);
            } else {
                rotation = CREATE_BMI270_ROT_SIGNAL(DataInterpreter::BOSCH_ROTATION, 3, 0);
                board->module_events[GYRO_BMI270_ROT_RESPONSE_HEADER] = rotation;
            }
            if (!rotation->components.size()) {
                rotation->components.push_back(CREATE_BMI270_ROT_SIGNAL_SINGLE(0));
                rotation->components.push_back(CREATE_BMI270_ROT_SIGNAL_SINGLE(2));
                rotation->components.push_back(CREATE_BMI270_ROT_SIGNAL_SINGLE(4));
            }

            board->responses[GYRO_BMI270_ROT_RESPONSE_HEADER]= response_handler_data_no_id;

            if (!board->module_events.count(GYRO_BMI270_PACKED_ROT_RESPONSE_HEADER)) {
                board->module_events[GYRO_BMI270_PACKED_ROT_RESPONSE_HEADER]= new MblMwDataSignal(GYRO_BMI270_PACKED_ROT_RESPONSE_HEADER, board, 
                    DataInterpreter::BOSCH_ROTATION, FirmwareConverter::BOSCH_ROTATION, 3, 2, 1, 0);
            }
            board->responses[GYRO_BMI270_PACKED_ROT_RESPONSE_HEADER]= response_handler_packed_data;

            board->responses.emplace(piecewise_construct, forward_as_tuple(MBL_MW_MODULE_GYRO, READ_REGISTER(ORDINAL(GyroBmi270Register::CONFIG))),
                    forward_as_tuple(received_config_response));

            GyroBoschState newState = {nullptr};
            states.insert({board, newState});
        }
        break;    
    default:
        return;
    }
}

// Helper function - delete module
void free_gyro_module(MblMwMetaWearBoard *board) {
    states.erase(board);
}

// Get the bmi160 gyro signal
MblMwDataSignal* mbl_mw_gyro_bmi160_get_rotation_data_signal(const MblMwMetaWearBoard *board) {
    GET_DATA_SIGNAL(GYRO_BMI160_ROT_RESPONSE_HEADER);
}

// Get the bmi160 gyro signal
MblMwDataSignal* mbl_mw_gyro_bmi160_get_high_freq_rotation_data_signal(const MblMwMetaWearBoard *board) {
    return mbl_mw_gyro_bmi160_get_packed_rotation_data_signal(board);
}

// Get the bmi270 gyro signal
MblMwDataSignal* mbl_mw_gyro_bmi270_get_rotation_data_signal(const MblMwMetaWearBoard *board) {
    GET_DATA_SIGNAL(GYRO_BMI270_ROT_RESPONSE_HEADER);
}

// Get the packed bmi160 gyro signal
MblMwDataSignal* mbl_mw_gyro_bmi160_get_packed_rotation_data_signal(const MblMwMetaWearBoard *board) {
    GET_DATA_SIGNAL(GYRO_BMI160_PACKED_ROT_RESPONSE_HEADER);
}

// Get the packed bmi270 gyro signal
MblMwDataSignal* mbl_mw_gyro_bmi270_get_packed_rotation_data_signal(const MblMwMetaWearBoard *board) {
    GET_DATA_SIGNAL(GYRO_BMI270_PACKED_ROT_RESPONSE_HEADER);
}

// Helper function - serialize
void serialize_gyro_config(const MblMwMetaWearBoard *board, vector<uint8_t>& state) {
    SERIALIZE_MODULE_CONFIG(GyroBoschConfig, MBL_MW_MODULE_GYRO);
}

// Helper function - deserialize
void deserialize_gyro_config(MblMwMetaWearBoard *board, uint8_t** state_stream) {
    DESERIALIZE_MODULE_CONFIG(GyroBoschConfig, MBL_MW_MODULE_GYRO);
}

// Set the bmi160 odr
void mbl_mw_gyro_bmi160_set_odr(MblMwMetaWearBoard *board, MblMwGyroBoschOdr odr) {
    ((GyroBoschConfig*) board->module_config.at(MBL_MW_MODULE_GYRO))->gyr_odr= odr;
}

// Set the bmi160 range
void mbl_mw_gyro_bmi160_set_range(MblMwMetaWearBoard *board, MblMwGyroBoschRange range) {
    ((GyroBoschConfig*) board->module_config.at(MBL_MW_MODULE_GYRO))->gyr_range= range;
}

// Write the bmi160 config
void mbl_mw_gyro_bmi160_write_config(const MblMwMetaWearBoard *board) {
    uint8_t command[4]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::CONFIG)};
    memcpy(command + 2, board->module_config.at(MBL_MW_MODULE_GYRO), sizeof(GyroBoschConfig));
    SEND_COMMAND;
}

// Set the bmi270 odr
void mbl_mw_gyro_bmi270_set_odr(MblMwMetaWearBoard *board, MblMwGyroBoschOdr odr) {
    ((GyroBoschConfig*) board->module_config.at(MBL_MW_MODULE_GYRO))->gyr_odr= odr;
}

// Set the bmi270 range
void mbl_mw_gyro_bmi270_set_range(MblMwMetaWearBoard *board, MblMwGyroBoschRange range) {
    ((GyroBoschConfig*) board->module_config.at(MBL_MW_MODULE_GYRO))->gyr_range= range;
}

// Write the bmi270 config
void mbl_mw_gyro_bmi270_write_config(const MblMwMetaWearBoard *board) {
    uint8_t command[4]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::CONFIG)};
    memcpy(command + 2, board->module_config.at(MBL_MW_MODULE_GYRO), sizeof(GyroBoschConfig));
    SEND_COMMAND;
}

// Read the bmi160 config
void mbl_mw_gyro_bmi160_read_config(const MblMwMetaWearBoard* board, void *context, MblMwFnBoardPtrInt completed) {
    states[board].read_config_context = context;
    states[board].read_config_completed = completed;

    uint8_t command[2]= {MBL_MW_MODULE_GYRO, READ_REGISTER(ORDINAL(GyroBmi160Register::CONFIG))};
    SEND_COMMAND;
}

// Read the bmi270 config
void mbl_mw_gyro_bmi270_read_config(const MblMwMetaWearBoard* board, void *context, MblMwFnBoardPtrInt completed) {
    states[board].read_config_context = context;
    states[board].read_config_completed = completed;

    uint8_t command[2]= {MBL_MW_MODULE_GYRO, READ_REGISTER(ORDINAL(GyroBmi270Register::CONFIG))};
    SEND_COMMAND;
}

// Start the bmi160 gyro
void mbl_mw_gyro_bmi160_start(const MblMwMetaWearBoard *board) {
    uint8_t command[3]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::POWER_MODE), 1};
    SEND_COMMAND;
}

// Stop the bmi160 gyro
void mbl_mw_gyro_bmi160_stop(const MblMwMetaWearBoard *board) {
    uint8_t command[3]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::POWER_MODE), 0};
    SEND_COMMAND;
}

// Start the bmi270 gyro
void mbl_mw_gyro_bmi270_start(const MblMwMetaWearBoard *board) {
    uint8_t command[3]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::POWER_MODE), 1};
    SEND_COMMAND;
}

// Stop the bmi270 gyro
void mbl_mw_gyro_bmi270_stop(const MblMwMetaWearBoard *board) {
    uint8_t command[3]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::POWER_MODE), 0};
    SEND_COMMAND;
}

// Start sampling the bmi160 gyro
void mbl_mw_gyro_bmi160_enable_rotation_sampling(const MblMwMetaWearBoard *board) {
    uint8_t command[4]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::DATA_INTERRUPT_ENABLE), 0x1, 0x0};
    SEND_COMMAND;
}

// Stop sampling the bmi160 gyro
void mbl_mw_gyro_bmi160_disable_rotation_sampling(const MblMwMetaWearBoard *board) {
    uint8_t command[4]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::DATA_INTERRUPT_ENABLE), 0x0, 0x1};
    SEND_COMMAND;
}

// Start sampling the bmi270 gyro
void mbl_mw_gyro_bmi270_enable_rotation_sampling(const MblMwMetaWearBoard *board) {
    uint8_t command[4]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::DATA_INTERRUPT_ENABLE), 0x1, 0x0};
    SEND_COMMAND;
}

// Stop sampling the bmi270 gyro
void mbl_mw_gyro_bmi270_disable_rotation_sampling(const MblMwMetaWearBoard *board) {
    uint8_t command[4]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::DATA_INTERRUPT_ENABLE), 0x0, 0x1};
    SEND_COMMAND;
}

// Manual gyroscope offsets
void mbl_mw_gyro_bmi270_offsets(const MblMwMetaWearBoard* board, uint8_t x_offset, uint8_t y_offset, uint8_t z_offset) {
    uint8_t gyr_usr_off_x_7_0 = x_offset;
    uint8_t gyr_usr_off_y_7_0 = y_offset;
    uint8_t gyr_usr_off_z_7_0 = z_offset;

    uint8_t command[5]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi270Register::OFFSET), gyr_usr_off_x_7_0, gyr_usr_off_y_7_0, gyr_usr_off_z_7_0};
    SEND_COMMAND;
}

// Name for the loggers
void create_gyro_uri(const MblMwDataSignal* signal, std::stringstream& uri) {
    switch(signal->owner->module_info.at(MBL_MW_MODULE_GYRO).implementation) {
    case MBL_MW_MODULE_GYRO_TYPE_BMI160:
        switch(CLEAR_READ(signal->header.register_id)) {
        case ORDINAL(GyroBmi160Register::DATA):
            uri << "angular-velocity";
            if (signal->length() <= 2) {
                uri << "[" << (int) (signal->offset >> 1) << "]";
            }
        }
        break;
    case MBL_MW_MODULE_GYRO_TYPE_BMI270:
        switch(CLEAR_READ(signal->header.register_id)) {
        case ORDINAL(GyroBmi270Register::DATA):
            uri << "angular-velocity";
            if (signal->length() <= 2) {
                uri << "[" << (int) (signal->offset >> 1) << "]";
            }
        }
        break;
    
    }
}