#include "dataprocessor_config.h"
#include "dataprocessor_private.h"
#include "dataprocessor_register.h"
#include "metawear/processor/dataprocessor.h"

#include "metawear/core/module.h"
#include "metawear/core/status.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/platform/cpp/async_creator.h"
#include "metawear/platform/cpp/threadpool.h"

#include "metawear/processor/comparator.h"
#include "metawear/processor/delta.h"
#include "metawear/processor/math.h"
#include "metawear/processor/pulse.h"
#include "metawear/processor/threshold.h"
#include "metawear/processor/time.h"

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <functional>
#include <unordered_map>
#include <vector>

using namespace std;

#define GET_DATAPROCESSOR_STATE(board) static_pointer_cast<DataProcessorState>(board->dp_state)

const uint8_t NO_PARENT = -1;
const ResponseHeader DATAPROCESSOR_RESPONSE_HEADER(MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY));

unordered_map<DataProcessorType, uint8_t> type_to_id= {
    { DataProcessorType::ACCUMULATOR, 0x2 },
    { DataProcessorType::AVERAGE, 0x3 },
    { DataProcessorType::BUFFER, 0xf },
    { DataProcessorType::COMPARATOR, 0x6 },
    { DataProcessorType::COUNTER, 0x2 },
    { DataProcessorType::DELTA, 0xc },
    { DataProcessorType::MATH, 0x9 },
    { DataProcessorType::PASSTHROUGH, 0x1 },
    { DataProcessorType::PULSE, 0xb },
    { DataProcessorType::RMS, 0x7 },
    { DataProcessorType::RSS, 0x7 },
    { DataProcessorType::SAMPLE, 0xa },
    { DataProcessorType::THRESHOLD, 0xd },
    { DataProcessorType::TIME, 0x8 },
    { DataProcessorType::ACCOUNTER, 0x11 },
    { DataProcessorType::PACKER, 0x10 },
    { DataProcessorType::FUSER, 0x1b}
};

struct DataProcessorState : public AsyncCreator {
    void *processor_context;
    MblMwFnDataProcessor processor_callback;
    MblMwDataProcessor* next_processor;
    ProcessorEntriesHandler sync_handler;
    stack<ProcessorEntry> chain;
    uint8_t entry_id;
};

// Helper function - create processor state signal
static void create_processor_state_signal(MblMwDataProcessor* processor, DataInterpreter interpreter) {
    ResponseHeader state_header(MBL_MW_MODULE_DATA_PROCESSOR, READ_REGISTER(ORDINAL(DataProcessorRegister::STATE)));
    processor->state= new MblMwDataSignal(state_header, processor->owner, interpreter, processor->n_channels, 
            processor->channel_size, processor->is_signed, 0);
}

MblMwDataProcessor* MblMwDataProcessor::transform(const MblMwDataSignal* input, uint8_t id, const void* config) {
    auto processor = new MblMwDataProcessor(*input);
    
    switch(id) {
        case 1: {
            processor->type = DataProcessorType::PASSTHROUGH;
            processor->config_size = sizeof(PassthroughConfig);
            create_processor_state_signal(processor, DataInterpreter::UINT32);
            break;
        }
        case 2: {
            auto accumulator = (AccumulatorConfig*) config;

            processor->type = accumulator->mode == ACCUMULATOR_SUM ? DataProcessorType::ACCUMULATOR : DataProcessorType::COUNTER;
            processor->config_size = sizeof(AccumulatorConfig);

            if (processor->type == DataProcessorType::COUNTER) {
                processor->is_signed= 0;
                processor->converter = FirmwareConverter::DEFAULT;
                processor->interpreter = DataInterpreter::UINT32;
                processor->set_channel_attr(1, accumulator->output + 1);
            } else {
                processor->set_channel_attr(input->n_channels, (accumulator->output + 1) / input->n_channels);
            }
            create_processor_state_signal(processor, processor->interpreter);
            
            break;
        }
        case 3: {
            bool has_hpf = input->owner->module_info.at(MBL_MW_MODULE_DATA_PROCESSOR).revision >= HPF_REVISION;

            processor->type = DataProcessorType::AVERAGE;
            processor->config_size = sizeof(AverageConfig) - (has_hpf ? 0 : 1);
            break;
        }
        case 6: {
            if (!(input->owner->firmware_revision < MULTI_COMPARE)) {
                auto compare = (MultiComparatorConfig*) config;

                processor->config_size = sizeof(MultiComparatorConfig);
                if (compare->mode == MBL_MW_COMPARATOR_MODE_ZONE || compare->mode == MBL_MW_COMPARATOR_MODE_BINARY) {
                    processor->interpreter= DataInterpreter::UINT32;
                    processor->converter= FirmwareConverter::DEFAULT;
                    processor->is_signed= 0;
                    processor->set_channel_attr(1, 1);
                }
            } else {
                auto compare = (ComparatorConfig*) config;
                processor->config_size = sizeof(ComparatorConfig);
                processor->is_signed = compare->is_signed;
            }
            processor->type = DataProcessorType::COMPARATOR;
            break;
        }
        case 7: {
            auto combiner = (CombinerConfig*) config;

            processor->config_size = sizeof(CombinerConfig);
            processor->is_signed= 0;
            processor->offset= 0;
            processor->set_channel_attr(1, input->channel_size);
            
            switch (input->interpreter) {
            case DataInterpreter::BOSCH_ACCELERATION:
                processor->interpreter= DataInterpreter::BOSCH_ACCELERATION_UNSIGNED_SINGLE_AXIS;
                break;
            case DataInterpreter::MMA8452Q_ACCELERATION:
                processor->interpreter = DataInterpreter::MMA8452Q_ACCELERATION_UNSIGNED_SINGLE_AXIS;
                break;
            case DataInterpreter::BOSCH_ROTATION:
                processor->interpreter = DataInterpreter::BOSCH_ROTATION_UNSIGNED_SINGLE_AXIS;
                break;
            case DataInterpreter::BMM150_B_FIELD:
                processor->interpreter = DataInterpreter::BMM150_B_FIELD_UNSIGNED_SINGLE_AXIS;
                break;
            case DataInterpreter::TCS34725_COLOR_ADC:
                processor->interpreter = DataInterpreter::UINT32;
                break;
            default:
                break;
            }

            processor->type = combiner->mode == COMBINER_RMS ? DataProcessorType::RMS : DataProcessorType::RSS;
            break;
        }
        case 8: {
            auto time = (TimeDelayConfig*) config;

            processor->config_size = sizeof(TimeDelayConfig);
            if (time->mode == MBL_MW_TIME_DIFFERENTIAL) {
                processor->make_signed();
            }
            
            processor->type = DataProcessorType::TIME;
            break;
        }
        case 9: {
            auto math = (MathConfig*) config;

            processor->config_size = sizeof(MathConfig) - (input->owner->firmware_revision < MULTI_CHANNEL_MATH ? 1 : 0);

            switch (math->operation) {
            // Use the is_signed value to determine if data will be signed/unsigned
            case MBL_MW_MATH_OP_ADD:
            case MBL_MW_MATH_OP_MULTIPLY:
            case MBL_MW_MATH_OP_DIVIDE:
            case MBL_MW_MATH_OP_MODULUS:
                if (math->is_signed) {
                    processor->make_signed();
                } else {
                    processor->make_unsigned();
                }
                break;
            // data always signed for subtract
            case MBL_MW_MATH_OP_SUBTRACT:
                processor->make_signed();
                break;
            // data always unsigned for sqrt & abs
            case MBL_MW_MATH_OP_SQRT:
            case MBL_MW_MATH_OP_ABS_VALUE:
                processor->make_unsigned();
                break;
            default:
                break;
            }

            switch (math->operation) {
            case MBL_MW_MATH_OP_EXPONENT:
            case MBL_MW_MATH_OP_SQRT:
            case MBL_MW_MATH_OP_LSHIFT:
            case MBL_MW_MATH_OP_RSHIFT:
                if (processor->interpreter != DataInterpreter::INT32 && processor->interpreter != DataInterpreter::UINT32) {
                    processor->interpreter = DataInterpreter::BYTE_ARRAY;
                }
                processor->converter = FirmwareConverter::DEFAULT;
                break;
            case MBL_MW_MATH_OP_CONSTANT:
                processor->interpreter = DataInterpreter::INT32;
                processor->converter = FirmwareConverter::DEFAULT;
            default:
                break;
            }
        
            uint32_t* rhs = (uint32_t*) &(math->rhs);
            switch (math->operation) {
            case MBL_MW_MATH_OP_RSHIFT:
                processor->channel_size= max(1, (int) (input->channel_size - (*rhs / 8)));
                break;
            case MBL_MW_MATH_OP_LSHIFT:
                processor->channel_size= min(4, input->channel_size + (uint8_t) ceil(*rhs / 8));
                break;
            default:
                processor->channel_size= 4;
            }

            processor->type = DataProcessorType::MATH;
            break;
        }
        case 0xa: {
            processor->type = DataProcessorType::SAMPLE;
            processor->config_size = sizeof(SampleDelayConfig);
            break;
        }
        case 0xb: {
            auto pulse = (PulseDetectorConfig*) config;

            processor->config_size = sizeof(PulseDetectorConfig);

            switch (pulse->output_mode) {
            case MBL_MW_PULSE_OUTPUT_WIDTH:
                processor->set_channel_attr(1, 2);
                processor->interpreter = DataInterpreter::UINT32;
                processor->converter = FirmwareConverter::DEFAULT;
                break;
            case MBL_MW_PULSE_OUTPUT_AREA:
                processor->set_channel_attr(1, 4);
                break;
            case MBL_MW_PULSE_OUTPUT_ON_DETECTION:
                processor->is_signed = 0;
                processor->set_channel_attr(1, 1);
                processor->interpreter = DataInterpreter::UINT32;
                processor->converter = FirmwareConverter::DEFAULT;
                break;
            default:
                break;
            }

            processor->type = DataProcessorType::PULSE;
            break;
        }
        case 0xc: {
            auto delta = (DeltaConfig*) config;

            processor->config_size = sizeof(DeltaConfig);
            switch (delta->mode) {
            case MBL_MW_DELTA_MODE_DIFFERENTIAL:
                processor->make_signed();
                break;
            case MBL_MW_DELTA_MODE_BINARY:
                processor->is_signed = 1;
                processor->interpreter = DataInterpreter::INT32;
                processor->set_channel_attr(1, 1);
                processor->converter = FirmwareConverter::DEFAULT;
                break;
            default:
                break;
            }
            processor->type = DataProcessorType::DELTA;

            create_processor_state_signal(processor, processor->interpreter);

            break;
        }
        case 0xd: {
            auto threshold = (ThresholdConfig*) config;

            processor->config_size = sizeof(ThresholdConfig);
            if (threshold->mode == MBL_MW_THRESHOLD_MODE_BINARY) {
                processor->is_signed = 1;
                processor->interpreter = DataInterpreter::INT32;
                processor->set_channel_attr(1, 1);
                processor->converter = FirmwareConverter::DEFAULT;
            }

            processor->type = DataProcessorType::THRESHOLD;
            break;
        }
        case 0xf: {
            processor->type = DataProcessorType::BUFFER;
            processor->config_size = sizeof(BufferConfig);
            create_processor_state_signal(processor, processor->interpreter);
            break;
        }
        case 0x10: {
            processor->type = DataProcessorType::PACKER;
            processor->config_size = sizeof(PackerConfig);
            break;
        }
        case 0x11: {
            auto accounter = (AccounterConfig*) config;
            processor->config_size = sizeof(AccounterConfig);
            processor->channel_size+= accounter->length + 1;
            processor->type = DataProcessorType::ACCOUNTER;
            break;
        }
        case 0x1b: {
            auto fuse = (FuseConfig*) config;
            processor->config_size = sizeof(FuseConfig);
            processor->channel_size = input->length();
            processor->n_channels = 1;
            processor->type = DataProcessorType::FUSER;
            processor->interpreter = DataInterpreter::FUSED_DATA;
            processor->converter = FirmwareConverter::DEFAULT;

            for(uint8_t i = 0; i < fuse->count; i++) {
                auto value = lookup_processor(input->owner, fuse->references[i]);
                processor->channel_size+= value->length();
            }

            break;
        }
    }

    processor->config = malloc(processor->config_size);
    memcpy(processor->config, config, processor->config_size);
    return processor;
}

MblMwDataProcessor* MblMwDataProcessor::transform(const MblMwDataSignal* input, const std::vector<uint8_t>& config) {
    return MblMwDataProcessor::transform(input, config[0], config.data() + 1);
}

MblMwDataProcessor::MblMwDataProcessor(uint8_t** state_stream, MblMwMetaWearBoard *owner) : MblMwDataSignal(state_stream, owner) {
    parent_id = **state_stream;

    config_size = *(++(*state_stream));
    config = malloc(config_size);
    memcpy(config, ++(*state_stream), config_size);

    *state_stream += config_size;
    uint8_t n_consumers = **state_stream;
    for (uint8_t i = 0; i < n_consumers; i++) {
        consumers.push_back(*(++(*state_stream)));
    }

    type = static_cast<DataProcessorType>(*(++(*state_stream)));

    (*state_stream)++;
}

MblMwDataProcessor::MblMwDataProcessor(const MblMwDataSignal& signal) : MblMwDataSignal(signal.header, signal.owner, 
        signal.interpreter, signal.converter, signal.n_channels, signal.channel_size, signal.is_signed, signal.offset), 
        parent_id(NO_PARENT), state(nullptr), input(&signal), config(nullptr) {
    offset = 0;
    header.module_id = MBL_MW_MODULE_DATA_PROCESSOR;
    header.register_id = ORDINAL(DataProcessorRegister::NOTIFY);

    if (const MblMwDataProcessor* source= dynamic_cast<const MblMwDataProcessor*>(&signal)) {
        parent_id= source->header.data_id;
    }
}

MblMwDataProcessor::~MblMwDataProcessor() {
    free(config);
    config= nullptr;

    if (remove) {
        uint8_t command[3]= { MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::REMOVE), header.data_id };
        send_command(owner, command, sizeof(command));
    }
}

void MblMwDataProcessor::subscribe() {
    uint8_t enable_command[4] = { MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY_ENABLE), header.data_id, 1 };
    send_command(owner, enable_command, sizeof(enable_command));

    uint8_t enable_notify[3] = { MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY), 1 };
    send_command(owner, enable_notify, sizeof(enable_notify));
}

void MblMwDataProcessor::unsubscribe() {
    uint8_t disable_command[4] = { MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY_ENABLE), header.data_id, 0 };
    send_command(owner, disable_command, sizeof(disable_command));
}

MblMwDataProcessor* MblMwDataProcessor::parent() const {
    ResponseHeader parent_header(MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY), parent_id);
    return owner->module_events.count(parent_header) ?
        dynamic_cast<MblMwDataProcessor*>(owner->module_events.at(parent_header)) :
        nullptr;
}

void MblMwDataProcessor::serialize(vector<uint8_t>& state) const {
    MblMwDataSignal::serialize(state);

    state.push_back(parent_id);

    state.push_back((uint8_t) config_size);
    state.insert(state.end(), (uint8_t*) config, ((uint8_t*) config) + config_size);

    state.push_back((uint8_t) consumers.size());
    state.insert(state.end(), consumers.begin(), consumers.end());

    state.push_back(static_cast<uint8_t>(type));
}

// Name of processor
static const char* type_to_uri(DataProcessorType type, void* config) {
    switch(type) {
    case DataProcessorType::ACCUMULATOR:
        return "accumulate";
    case DataProcessorType::AVERAGE:
        return ((AverageConfig*) config)->mode == USE_HPF ? "high-pass" : "low-pass";
    case DataProcessorType::BUFFER:
        return "buffer";
    case DataProcessorType::COMPARATOR:
        return "comparison";
    case DataProcessorType::COUNTER:
        return "count";
    case DataProcessorType::DELTA:
        return "differential";
    case DataProcessorType::MATH:
        return "math";
    case DataProcessorType::PASSTHROUGH:
        return "passthrough";
    case DataProcessorType::PULSE:
        return "pulse";
    case DataProcessorType::RMS:
        return "rms";
    case DataProcessorType::RSS:
        return "rss";
    case DataProcessorType::SAMPLE:
        return "delay";
    case DataProcessorType::THRESHOLD:
        return "threshold";
    case DataProcessorType::TIME:
        return "time";
    case DataProcessorType::ACCOUNTER:
        return "account";
    case DataProcessorType::PACKER:
        return "packer";
    case DataProcessorType::FUSER:
        return "fuser";
    default:
        return nullptr;
    }
}

// Id of processor
void MblMwDataProcessor::create_uri(std::stringstream& uri) const {
    uri << type_to_uri(type, config) << "?id=" << (int) header.data_id;
}

// Helper function - name of state
void create_dataprocessor_state_uri(const MblMwDataSignal* signal, stringstream& uri) {
    ResponseHeader header(MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY), signal->header.data_id);
    auto processor = dynamic_cast<MblMwDataProcessor*>(signal->owner->module_events.at(header));

    uri << type_to_uri(processor->type, processor->config) << "-state?id=" << (int) processor->header.data_id;
}

// Helper function - processor created
static int32_t dataprocessor_created(MblMwMetaWearBoard *board, const uint8_t *response, uint8_t len) {
    auto state = GET_DATAPROCESSOR_STATE(board);

    state->timeout->cancel();
    state->next_processor->header.data_id = response[2];

    auto proc_parent = state->next_processor->parent();
    if (proc_parent != nullptr) {
        proc_parent->consumers.push_back(response[2]);
    }

    if (state->next_processor->state != nullptr) {
        state->next_processor->state->header.data_id = response[2];
        state->next_processor->owner->module_events[state->next_processor->state->header] = state->next_processor->state;
        state->next_processor->state = nullptr;
    }

    state->next_processor->owner->module_events.emplace(state->next_processor->header, state->next_processor);
    state->processor_callback(state->processor_context, state->next_processor);
    state->create_next(true);

    return MBL_MW_STATUS_OK;
}

// Helper function - processor config received
static int32_t dataprocessor_config_received(MblMwMetaWearBoard *board, const uint8_t *response, uint8_t len) {
    auto state = GET_DATAPROCESSOR_STATE(board);
    state->timeout->cancel();
    
    ProcessorEntry entry = {
        state->entry_id,
        static_cast<uint8_t>(response[5] & 0x1f),
        static_cast<uint8_t>(((response[5] >> 5) & 0x7) + 1),
        ResponseHeader(response[2], response[3], response[4])
    };
    entry.config.insert(entry.config.end(), response + 6, response + len);

    state->chain.push(entry);

    if (response[2] == MBL_MW_MODULE_DATA_PROCESSOR && response[3] == ORDINAL(DataProcessorRegister::NOTIFY)) {
        state->entry_id = response[4];
        state->pending_fns.push([=](void) -> void {
            state->timeout= ThreadPool::schedule([state, board](void) -> void {
                board->anon_signals_created(board->anon_signals_context, board, nullptr, MBL_MW_STATUS_ERROR_TIMEOUT);
                state->create_next(true);
            }, board->time_per_response);
    
            uint8_t command[3] = { MBL_MW_MODULE_DATA_PROCESSOR, READ_REGISTER(ORDINAL(DataProcessorRegister::ADD)), response[4] };
            SEND_COMMAND;
        });
    } else { 
        state->sync_handler(board, state->chain);
    }
    state->create_next(true);

    return MBL_MW_STATUS_OK;
}

// Helper function - init module
void init_dataprocessor_module(MblMwMetaWearBoard* board) {
    board->responses.emplace(piecewise_construct, forward_as_tuple(MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY)),
        forward_as_tuple(response_handler_data_with_id));
    board->responses.emplace(piecewise_construct, forward_as_tuple(MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::ADD)),
        forward_as_tuple(dataprocessor_created));
    board->responses.emplace(piecewise_construct, forward_as_tuple(MBL_MW_MODULE_DATA_PROCESSOR, READ_REGISTER(ORDINAL(DataProcessorRegister::ADD))),
        forward_as_tuple(dataprocessor_config_received));
    board->responses.emplace(piecewise_construct, forward_as_tuple(MBL_MW_MODULE_DATA_PROCESSOR, READ_REGISTER(ORDINAL(DataProcessorRegister::STATE))),
        forward_as_tuple(response_handler_data_with_id));

    if (!board->dp_state) {
        board->dp_state = make_shared<DataProcessorState>();
    }
}

// Helper function - free module
void free_dataprocessor_module(void* state) {
    delete (DataProcessorState*) state;
}

// Helper function - disconnect
void disconnect_dataprocessor(MblMwMetaWearBoard* board) {
    auto state = GET_DATAPROCESSOR_STATE(board);
    if (state != nullptr) {
        state->pending_fns.clear();
    }
}

// Dataprocessor state signal
MblMwDataSignal* mbl_mw_dataprocessor_get_state_data_signal(const MblMwDataProcessor* processor) {
    ResponseHeader state_header(MBL_MW_MODULE_DATA_PROCESSOR, READ_REGISTER(ORDINAL(DataProcessorRegister::STATE)), processor->header.data_id);
    GET_DATA_SIGNAL_BOARD(processor->owner, state_header);
}

// Helper function - processor remove
void mbl_mw_dataprocessor_remove(MblMwDataProcessor *processor) {
    if (processor->parent_id != NO_PARENT) {
        auto parent_consumers= &processor->parent()->consumers;
        parent_consumers->erase(find(parent_consumers->begin(), parent_consumers->end(), processor->header.data_id));
    }

    function<void (MblMwDataProcessor *current)> remove_inner= [&remove_inner](MblMwDataProcessor *current) -> void {
        for(auto it: current->consumers) {
            ResponseHeader consumer_header(MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY), it);
            remove_inner(dynamic_cast<MblMwDataProcessor*>(current->owner->module_events.at(consumer_header)));
        }

        auto state_signal = mbl_mw_dataprocessor_get_state_data_signal(current);
        if (state_signal != nullptr) {
            state_signal->header.disable_silent();
            current->owner->module_events.erase(state_signal->header);
            delete state_signal;
        }

        current->owner->module_events.erase(current->header);
        delete current;
    };

    remove_inner(processor);
}

// Get processor id
uint8_t mbl_mw_dataprocessor_get_id(const MblMwDataProcessor* processor) {
    return processor->header.data_id;
}

// Lookup id
MblMwDataProcessor* mbl_mw_dataprocessor_lookup_id(const MblMwMetaWearBoard* board, uint8_t id) {
    ResponseHeader map_key(MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY), id);
    return dynamic_cast<MblMwDataProcessor*>(board->module_events.at(map_key));
}

// Helper function - create processor
void create_processor(MblMwDataSignal* source, MblMwDataProcessor* processor, void *context, MblMwFnDataProcessor processor_created) {
    auto state = GET_DATAPROCESSOR_STATE(processor->owner);
    state->pending_fns.push([state, processor, context, processor_created, source](void) -> void {
        state->next_processor= processor;
        state->processor_context= context;
        state->processor_callback= processor_created;

        vector<uint8_t> command = { MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::ADD), source->header.module_id, 
                source->header.register_id, source->header.data_id, source->get_data_ubyte(), type_to_id.at(processor->type) };
        command.insert(command.end(), (uint8_t*) processor->config, ((uint8_t*) processor->config) + processor->config_size);

        state->timeout= ThreadPool::schedule([state](void) -> void {
            if (state->next_processor->state != nullptr) {
                delete state->next_processor->state;
            }
            state->next_processor->remove = false;
            delete state->next_processor;
            state->processor_callback(state->processor_context, nullptr);

            state->create_next(true);
        }, source->owner->time_per_response);
        send_command(source->owner, command.data(), (uint8_t) command.size());
    });
    state->create_next(false);
}

// Helper function - set processor state
void set_processor_state(MblMwDataProcessor *processor, void* new_state, uint8_t size) {
    vector<uint8_t> command = { MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::STATE), processor->header.data_id };
    if (new_state != nullptr) {
        command.insert(command.end(), (uint8_t*) new_state, ((uint8_t*) new_state) + size);
    }
    send_command(processor->owner, command.data(), (uint8_t) command.size());
}

// Helper function - processor config modify
void modify_processor_configuration(MblMwDataProcessor *processor, uint8_t size) {
    vector<uint8_t> command = { MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::PARAMETER), processor->header.data_id, 
            type_to_id.at(processor->type)};
    command.insert(command.end(), (uint8_t*) processor->config, ((uint8_t*) processor->config) + size);
    send_command(processor->owner, command.data(), (uint8_t) command.size());
}

// Helper function - sync proc
void sync_processor_chain(MblMwMetaWearBoard* board, uint8_t id, ProcessorEntriesHandler handler) {
    auto state = GET_DATAPROCESSOR_STATE(board);

    while(!state->chain.empty()) {
        state->chain.pop();
    }
    state->entry_id = id;
    state->sync_handler = handler;

    state->pending_fns.push([=](void) -> void {
        state->timeout= ThreadPool::schedule([state, board](void) -> void {
            board->anon_signals_created(board->anon_signals_context, board, nullptr, MBL_MW_STATUS_ERROR_TIMEOUT);
            state->create_next(true);
        }, board->time_per_response);

        uint8_t command[3] = { MBL_MW_MODULE_DATA_PROCESSOR, READ_REGISTER(ORDINAL(DataProcessorRegister::ADD)), id };
        SEND_COMMAND;
    });
    state->create_next(false);
}

// Helper function - lookup proc
MblMwDataProcessor* lookup_processor(const MblMwMetaWearBoard* board, uint8_t id) {
    ResponseHeader key = {MBL_MW_MODULE_DATA_PROCESSOR, ORDINAL(DataProcessorRegister::NOTIFY), id};
    return dynamic_cast<MblMwDataProcessor*>(board->module_events.at(key));
}

namespace std {

size_t hash<DataProcessorType>::operator()(const DataProcessorType& key) const {
    return static_cast<uint8_t>(key);
}

}