KIAPBCI/CereConn/src/sbpe/SpikeBandPowerEstimator.cpp

#include "sbpe/SpikeBandPowerEstimator.h"

#include <algorithm>
#include <iostream>

namespace cc
{
namespace sbpe
{


SpikeBandPowerEstimator::SpikeBandPowerEstimator()
: m_ch_list{}
, mp_cont_data(std::make_unique<cont_sample_rb_type>(1))
, m_iir_filt_coeff_b{0.190616600097498, 0, -0.381233200194995, 0, 0.190616600097498}
, m_iir_filt_coeff_a{1, -2.33508240207651, 1.9509646897792, -0.819263685312402, 0.206671985166565}
, mp_iir_filt_rms(std::make_unique<iir_filt_RMS_type>(1, 600, m_iir_filt_coeff_b, m_iir_filt_coeff_a))
, m_t_sbp_loop(10ms)
, m_keep_going(false)
, m_thread_loop_over_time_flag(false)
, m_sbp_rb_needs_reset(true)
, m_iir_filt_need_reset(true)
, m_accepted_sample_group(6)
{
    initLogger();
    fillChannelList(constants::MaxFEChannels);

}

SpikeBandPowerEstimator::SpikeBandPowerEstimator(size_type n_channels,
    sbp_interval_type t_sbp_loop)
: m_ch_list{}
, mp_cont_data(std::make_unique<cont_sample_rb_type>(1))
, m_iir_filt_coeff_b{0.190616600097498, 0, -0.381233200194995, 0, 0.190616600097498}
, m_iir_filt_coeff_a{1, -2.33508240207651, 1.9509646897792, -0.819263685312402, 0.206671985166565}
, mp_iir_filt_rms(std::make_unique<iir_filt_RMS_type>(n_channels, 600, m_iir_filt_coeff_b, m_iir_filt_coeff_a))
, m_t_sbp_loop(t_sbp_loop)
, m_keep_going(false)
, m_thread_loop_over_time_flag(false)
, m_sbp_rb_needs_reset(true)
, m_iir_filt_need_reset(true)
, m_accepted_sample_group(6)
{
    initLogger();
    fillChannelList(n_channels);
}
SpikeBandPowerEstimator::SpikeBandPowerEstimator(ch_list_type ch_list,
    sbp_interval_type t_sbp_loop)
: m_ch_list(ch_list)
, mp_cont_data(std::make_unique<cont_sample_rb_type>(1))
, m_iir_filt_coeff_b{0.190616600097498, 0, -0.381233200194995, 0, 0.190616600097498}
, m_iir_filt_coeff_a{1, -2.33508240207651, 1.9509646897792, -0.819263685312402, 0.206671985166565}
, mp_iir_filt_rms(std::make_unique<iir_filt_RMS_type>(ch_list.size(), 600, m_iir_filt_coeff_b, m_iir_filt_coeff_a))
, m_t_sbp_loop(t_sbp_loop)
, m_keep_going(false)
, m_thread_loop_over_time_flag(false)
, m_sbp_rb_needs_reset(true)
, m_iir_filt_need_reset(true)
, m_accepted_sample_group(6)
{
    initLogger();
    applyChannelList();
}


SpikeBandPowerEstimator::~SpikeBandPowerEstimator ()
{
    stopProcessThread();
}

void SpikeBandPowerEstimator::attachSpikeBandPowerRB(std::shared_ptr<sbp_ring_buffer_type> sbp_rb)
{
    mp_logger->debug("attachSpikeBandPowerRB");
    mp_sbp_rb = sbp_rb;
}


void SpikeBandPowerEstimator::setChannelList(const ch_list_type & ch_list)
{
    m_ch_list.clear();
    std::copy(ch_list.cbegin(), ch_list.cend(), std::back_inserter(m_ch_list));
    applyChannelList();
}

void SpikeBandPowerEstimator::applyChannelList()
{
    m_ch_map.clear();
    size_type n_ch = m_ch_list.size();
    
    for (size_type i = 0; i < n_ch; ++ i)
    {
        m_ch_map[m_ch_list[i]] = i;
    }

    updateChannelPairList(mp_cont_data->ch_list_cbegin(), mp_cont_data->ch_list_cend());
    
    m_sbp_rb_needs_reset = true;
    m_iir_filt_need_reset = true;
    mp_logger->debug("Set new channel list, with {} items", m_ch_list.size());
}


void SpikeBandPowerEstimator::fillChannelList(const size_type n_channels)
{
    ch_list_type ch_list(n_channels);
    std::iota (std::begin(ch_list), std::end(ch_list), 1);
    setChannelList(ch_list);
}

std::pair<SpikeBandPowerEstimator::ch_list_type, SpikeBandPowerEstimator::ch_map_type> SpikeBandPowerEstimator::getChannelMapping() const
{
    return std::make_pair(m_ch_list, m_ch_map);
}

SpikeBandPowerEstimator::ch_list_type SpikeBandPowerEstimator::getChannelList() const
{
    return m_ch_list;
}

void SpikeBandPowerEstimator::updateChannelPairList(const ChannelNumList_t & channel_ids)
{
    updateChannelPairList(channel_ids.cbegin(), channel_ids.cend());
}

void SpikeBandPowerEstimator::setLoopInterval(const SpikeBandPowerEstimator::sbp_interval_type & new_interval)
{
    if (m_t_sbp_loop != new_interval)
    {
        m_t_sbp_loop = new_interval;
        m_sbp_rb_needs_reset = true;
    }
}

SpikeBandPowerEstimator::sbp_interval_type SpikeBandPowerEstimator::getLoopInterval() const
{
    return m_t_sbp_loop;
}

bool SpikeBandPowerEstimator::ringBufferInvalid() const
{
    return !mp_sbp_rb || m_sbp_rb_needs_reset || m_iir_filt_need_reset;
}

void SpikeBandPowerEstimator::setAcceptedSampleGroup(const size_t group)
{
    if (group < constants::NSampleGroups)
    {
        m_accepted_sample_group = group;
    }
}

size_t SpikeBandPowerEstimator::getAcceptedSampleGroup() const
{
    return m_accepted_sample_group;
}

void SpikeBandPowerEstimator::setIIRFilterCoeffs(const iir_filt_RMS_type::iir_filter_coeff_container_type &b, const iir_filt_RMS_type::iir_filter_coeff_container_type &a)
{
    if (m_iir_filt_coeff_b.n_rows != b.n_rows || m_iir_filt_coeff_b.n_cols != b.n_cols || any(m_iir_filt_coeff_b != b))
    {
        m_iir_filt_coeff_b = b;
        m_iir_filt_need_reset = true;
    }
    if (m_iir_filt_coeff_a.n_rows != a.n_rows || m_iir_filt_coeff_a.n_cols != a.n_cols ||any(m_iir_filt_coeff_a != a))
    {
        m_iir_filt_coeff_a = a;
        m_iir_filt_need_reset = true;
    }
}

std::pair<SpikeBandPowerEstimator::iir_filt_RMS_type::iir_filter_coeff_container_type, SpikeBandPowerEstimator::iir_filt_RMS_type::iir_filter_coeff_container_type> SpikeBandPowerEstimator::getIIRFilterCoeffs() const
{
    return std::make_pair<>(m_iir_filt_coeff_b, m_iir_filt_coeff_a);
}



void SpikeBandPowerEstimator::setIIRFilterNSamples(const size_t n_samples)
{
    mp_iir_filt_rms->setNSamples(n_samples);
}

size_t SpikeBandPowerEstimator::getIIRFilterNSamples() const
{
    return mp_iir_filt_rms->getNSamples();
}

void SpikeBandPowerEstimator::setRMSNBins(const size_t n_bins)
{
    mp_iir_filt_rms->setNAvgBins(n_bins);
}

size_t SpikeBandPowerEstimator::getRMSNBins() const
{
    return mp_iir_filt_rms->getNAvgBins();
}


bool SpikeBandPowerEstimator::reset(const size_t group, const ChannelNumList_t &channel_list)
{
    if (group != m_accepted_sample_group)
    {
        return false;
    }
    
    mp_cont_data->reset(channel_list);
    return true;
}

bool SpikeBandPowerEstimator::reset(const ChannelNumList_t &channel_list)
{
    mp_cont_data->reset(channel_list);
    return true;
}

bool SpikeBandPowerEstimator::addSamplesFromPacket(const Timestamp_t timestamp, const SampleGroupRingBuffer::ContinuousDataSampleT * data, const unsigned int maxdatalen)
{
    if (!m_keep_going)
    {
        return false;
    }
    return mp_cont_data->addSamplesFromPacket(timestamp, data, maxdatalen);
}

bool SpikeBandPowerEstimator::addSamplesFromPacket(const size_t group, const Timestamp_t timestamp, const SampleGroupRingBuffer::ContinuousDataSampleT * data, const unsigned int maxdatalen)
{
    if (!m_keep_going || group != m_accepted_sample_group)
    {
        return false;
    }
    return mp_cont_data->addSamplesFromPacket(timestamp, data, maxdatalen);
}

bool SpikeBandPowerEstimator::addSamplesFromPacket(const cbPKT_GROUP *pkt_grp)
{
    if (!m_keep_going || pkt_grp->type != m_accepted_sample_group)
    {
        return false;
    }
    return mp_cont_data->addSamplesFromPacket(pkt_grp);
}

bool SpikeBandPowerEstimator::setCARChannels(const size_t group, const ChannelNumList_t &carChannelList)
{
    if (group != m_accepted_sample_group)
    {
        return false;
    }
    return mp_cont_data->setCARChannels(carChannelList);
}

bool SpikeBandPowerEstimator::setCARChannels(const size_t group, ChannelNumList_t &&carChannelList)
{
    if (group != m_accepted_sample_group)
    {
        return false;
    }
    return mp_cont_data->setCARChannels(carChannelList);
}

void SpikeBandPowerEstimator::disableCAR(const size_t group)
{
    if (group != m_accepted_sample_group)
    {
        return;
    }
    mp_cont_data->disableCAR();
}


bool SpikeBandPowerEstimator::applyFilters()
{
    if (mp_sbp_rb)
    {
        if (m_iir_filt_need_reset)
        {
            mp_iir_filt_rms->resize(m_ch_list.size());
            mp_iir_filt_rms->setIIRFilterCoeffs(m_iir_filt_coeff_b, m_iir_filt_coeff_a);
            m_iir_filt_need_reset = false;
        }
        if (m_sbp_rb_needs_reset)
        {
            mp_sbp_rb->reset();
            m_sbp_rb_needs_reset = false;
        }

        SampleGroupChunk v;

        mp_cont_data->consume(v);
        if (v.channel_ids_changed)
        {
            mp_logger->debug("channel ids changed (in applyFilters())");
            updateChannelPairList(v.channel_ids);
        }

        const size_t num_channels = v.channel_ids.size();
        if (num_channels == 0)
        {
            mp_logger->debug("No channels");
            return false;
        }
        const size_t num_samples = v.buffer.size() / num_channels;

        std::vector<sbp_type> datav(m_ch_list.size(), 0);
        bool rms_complete;
        for (size_t i_sample = 0; i_sample < num_samples; ++ i_sample)
        {
            for (auto &p: m_ch_pair_v)
            {
                datav[p.second] = v.buffer[p.first + i_sample * num_channels];
            }
            rms_complete = (*mp_iir_filt_rms)(datav.cbegin(), datav.cend());
            if (rms_complete)
            {
                sbp_ring_buffer_item_type time_and_rates{v.last_timestamp - constants::SampleGroupPeriods[m_accepted_sample_group - 1] * (num_samples - i_sample - 1), sbp_item_type(mp_iir_filt_rms->cbegin(), mp_iir_filt_rms->cend())};
                mp_sbp_rb->push(time_and_rates);
            }
        }
        return true;
        
    }
    return false;
}

void SpikeBandPowerEstimator::signalStart()
{
    stopProcessThread();
    // mp_logger->debug("Launching spike processing thread");
    m_keep_going = true;
    m_sbp_rb_needs_reset = true;
    m_iir_filt_need_reset = true;
    SampleGroupChunk v;
    mp_cont_data->consume(v);
    if (v.channel_ids_changed)
    {
        mp_logger->debug("channel ids changed (in signalStart())");
        updateChannelPairList(v.channel_ids);
    }
    
    m_process_thread = std::thread(&SpikeBandPowerEstimator::processThreadFun, this);
}

void SpikeBandPowerEstimator::signalStop()
{
    m_keep_going = false;
}


void SpikeBandPowerEstimator::stopProcessThread()
{
    m_keep_going = false;
    if (m_process_thread.joinable())
    {
        mp_logger->info("Stopping spike band power processing thread");
        m_process_thread.join();
    }
}

void SpikeBandPowerEstimator::processThreadFun()
{
    m_thread_loop_over_time_flag = false;

    clock::time_point currentStartTime{clock::now()};
    clock::time_point nextStartTime{clock::now()};
    clock::duration taskDuration;
    
    size_t loop_counter = 0;
    while (m_keep_going)
    {
        ++ loop_counter;
        currentStartTime = clock::now();
        applyFilters();
        taskDuration = clock::now() - currentStartTime;
        if (taskDuration > m_t_sbp_loop)
        {
            m_thread_loop_over_time_flag = true;
            m_thread_loop_over_time = taskDuration;
            mp_logger->warn("Spike band power processing loop went over time! {}μs vs. {}ms. loop {}", duration_cast<microseconds>(taskDuration).count(), m_t_sbp_loop.count(), loop_counter);
        }
        nextStartTime += m_t_sbp_loop;
        std::this_thread::sleep_until(nextStartTime);
    }
}

void SpikeBandPowerEstimator::initLogger()
{
    auto l = spdlog::get("SpikeBandPowerEstimator");
    if (l)
    {
        mp_logger = l;
    }
    else
    {
        mp_logger = spdlog::stdout_color_mt<spdlog::async_factory>("SpikeBandPowerEstimator");
    }
}

void SpikeBandPowerEstimator::setLogger(std::shared_ptr<spdlog::logger> logger)
{
    mp_logger = logger->clone("SpikeBandPowerEstimator");
}


void SpikeBandPowerEstimator::bindTimestampCallback(const SpikeBandPowerEstimator::timestamp_callback_type & tscb)
{
    m_timestamp_callback = tscb;
}

} // namespace sbpe
} // namespace cc