#include "CallChain.h"
#include "cmsis.h"

namespace mbed {

CallChain::CallChain(int size) : _chain(), _size(size), _elements(0) {
    _chain = new pFunctionPointer_t[size]();
}

CallChain::~CallChain() {
    clear();
    delete _chain;
}

pFunctionPointer_t CallChain::add(Callback<void()> func) {
    _check_size();
    _chain[_elements] = new Callback<void()>(func);
    _elements ++;
    return _chain[_elements];
}

pFunctionPointer_t CallChain::add_front(Callback<void()> func) {
    _check_size();
    memmove(_chain + 1, _chain, _elements * sizeof(pFunctionPointer_t));
    _chain[0] = new Callback<void()>(func);
    _elements ++;
    return _chain[0];
}

int CallChain::size() const {
    return _elements;
}

pFunctionPointer_t CallChain::get(int i) const {
    if (i < 0 || i >= _elements)
        return NULL;
    return _chain[i];
}

int CallChain::find(pFunctionPointer_t f) const {
    for (int i = 0; i < _elements; i++)
        if (f == _chain[i])
            return i;
    return -1;
}

void CallChain::clear() {
    for(int i = 0; i < _elements; i ++) {
        delete _chain[i];
        _chain[i] = NULL;
    }
    _elements = 0;
}

bool CallChain::remove(pFunctionPointer_t f) {
    int i;

    if ((i = find(f)) == -1)
        return false;
    if (i != _elements - 1)
        memmove(_chain + i, _chain + i + 1, (_elements - i - 1) * sizeof(pFunctionPointer_t));
    delete f;
    _elements --;
    return true;
}

void CallChain::call() {
    for(int i = 0; i < _elements; i++)
        _chain[i]->call();
}

void CallChain::_check_size() {
    if (_elements < _size)
        return;
    _size = (_size < 4) ? 4 : _size + 4;
    pFunctionPointer_t* new_chain = new pFunctionPointer_t[_size]();
    memcpy(new_chain, _chain, _elements * sizeof(pFunctionPointer_t));
    delete _chain;
    _chain = new_chain;
}

} // namespace mbed