#define BOOST_HANA_CONFIG_ENABLE_STRING_UDL
#define USCXML_VERBOSE

#include "state_machine.h"
#include <cstring>
#include <cstdio>
#include <functional>
#include <boost/hana/string.hpp>
#include "MPDEF_DIRECTIVE.hpp"

namespace mountel {
    MPDEF_DIRECTIVE(State)
    MPDEF_DIRECTIVE(ParallelState)
    MPDEF_DIRECTIVE(Transition)
    MPDEF_DIRECTIVE_LEAF(Name)
    MPDEF_DIRECTIVE_LEAF(Action)
    MPDEF_DIRECTIVE_LEAF(OnEnter)
    MPDEF_DIRECTIVE_LEAF(OnExit)
    MPDEF_DIRECTIVE_LEAF(Guard)
    MPDEF_DIRECTIVE_LEAF(Initial)
    MPDEF_DIRECTIVE_LEAF(Target)
    MPDEF_DIRECTIVE_LIST(States)
    MPDEF_DIRECTIVE_TYPE(Event)

    constexpr auto statechart_elements = boost::hana::make_tuple(
        tag::State,
        tag::ParallelState,
        tag::Transition,
        tag::Name,
        tag::Action,
        tag::OnEnter,
        tag::OnExit,
        tag::Guard,
        tag::Initial,
        tag::Target,
        tag::States,
        tag::Event
    );

    template <typename Context>
    struct StateMachineLocals {
        Context& context;
        const double time;

        template <typename Event>
        void send(const Event& event);

        template <typename State>
        bool active(const State& state) const;

        bool after(double time) const;
    };

    // Num states

    struct count_tags_impl {
        template <typename T, typename Tags>
        constexpr auto operator()(T& t, Tags tags) const;

        constexpr auto operator()(...) const { return 0; }

        template <typename First, typename Second, typename Tags>
        constexpr auto operator()(mpdef::tree_node<First, Second> const node, Tags tags) const {
            using namespace boost::hana;
            using namespace boost::hana::literals;
            const auto states = find(node.second, tag::States);

            return if_(first(node) ^in^ tags, size_c<1>, size_c<0>)
            + count_if(keys(node.second), [&] (auto elem) { return elem ^in^ tags; })
            + maybe(
                size_c<0>,
                [&] (auto& states) {
                    return fold_right(states, size_c<0>, [&] (auto& elem, auto sum) { return sum + this->operator()(elem, tags); });
                },
                states
            );
        }
    };

    constexpr count_tags_impl count_tags{};

    template <typename T>
    constexpr auto num_states(T& t) {
        using namespace boost::hana;
        return count_tags(t, make_tuple(tag::State, tag::ParallelState));
    }

    template <typename T>
    constexpr auto num_transitions(T& t) {
        using namespace boost::hana;
        return count_tags(t, make_tuple(tag::Transition));
    }

    struct num_states_bytes_impl {
        template <typename T>
        constexpr auto operator()(T& t) const {
            using namespace boost::hana;
            return (num_states(t) + size_c<7>) / size_c<8>;
        }
    };

    constexpr num_states_bytes_impl num_states_bytes{};

    struct num_transitions_bytes_impl {
        template <typename T>
        constexpr auto operator()(T& t) const {
            using namespace boost::hana;
            return (num_transitions(t) + size_c<7>) / size_c<8>;
        }
    };

    constexpr num_transitions_bytes_impl num_transitions_bytes{};

    // Validate state chart

    struct validate_state_chart_impl {
        template <typename T>
        constexpr auto operator()(T& t) const {
            using namespace boost::hana;
            using namespace boost::hana::literals;
            const auto less_than_256_states = num_states(t) < size_c<256>;
            static_assert(less_than_256_states, "State chart must have less than 256 states");

            return true_c;
        }
    };

    constexpr validate_state_chart_impl validate_state_chart{};
}

namespace example {
    using namespace mountel;
    using namespace boost::hana::literals;
    // Events

    struct a {
        int data = 0;
    };
    struct b { };
    struct c { };

    // State chart
    BOOST_HANA_CONSTEXPR_STATELESS_LAMBDA auto state_chart = State(
        Name("root"_s),
        Initial("A"_s),
        States(
            State(
                Name("A"_s),
                Transition(
                    Event<b>,
                    Target("B"_s),
                    Action([] { std::puts("A -> B"); })
                )
            ),
            State(
                Name("B"_s),
                OnEnter(
                    [] (auto&& locals ) {
                        std::puts("Entered B");
                        locals.send(c{});
                    }
                ),
                Transition(
                    Event<c>,
                    Target("C"_s)
                ),
                OnExit([] { std::puts("Leaving C"); })
            ),
            ParallelState(
                Name("C"_s),
                States(
                    State(
                        Name("C1"_s),
                        Transition(
                            Event<a>,
                            Target("A"_s),
                            Guard(
                                [] (const auto&& locals, a a_) {
                                    return locals.active("C2") && a_.data;
                                }
                            )
                        )
                    ),
                    State(
                        Name("C2"_s)
                    )
                )
            )
        )
    );

    BOOST_HANA_CONSTEXPR_STATELESS_LAMBDA auto statechart_simple = State(
        Name("root"_s),
        Transition(
            Event<a>
        )
    );
}

struct event_t {
    const char* name;
};

static event_t event[1] = {{"hello"}};

int main() {
    using namespace mountel;
    using namespace example;
    using namespace boost;

    BOOST_HANA_CONSTEXPR_LAMBDA auto name = hana::find(state_chart, tag::Name);
    static_assert(name != hana::nothing, "Root state machine must have a name");

    BOOST_HANA_CONSTEXPR_LAMBDA auto states = hana::find(state_chart, tag::States);
    static_assert(states != hana::nothing, "Couldn't find states");

    BOOST_HANA_CONSTEXPR_LAMBDA auto first_state = hana::find_if(states.value(), [] (auto x) {
        return hana::first(x) == tag::State;
    });
    static_assert(first_state != hana::nothing, "Couldn't find first state");

    BOOST_HANA_CONSTEXPR_LAMBDA auto transition = hana::find(first_state.value(), tag::Transition);
    static_assert(transition != hana::nothing, "No transition found");

    BOOST_HANA_CONSTEXPR_LAMBDA auto action = hana::find(transition.value(), tag::Action);
    static_assert(action != hana::nothing, "No action found");

    action.value()();

    BOOST_HANA_CONSTANT_ASSERT(validate_state_chart(state_chart));

    std::printf("Num states: %lu\n", hana::value(num_states(state_chart)));
    std::printf("Num transitions: %lu\n", hana::value(num_transitions(state_chart)));
    std::printf("sizeof(state_chart): %lu\n", sizeof(state_chart));

    std::printf("Num names: %lu\n", hana::value(count_tags(state_chart, boost::hana::make_tuple(tag::Name))));

    using uscxml = USCXML<1, 1, std::deque>;

    uscxml::uscxml_ctx ctx;
    // memset(&ctx, 0, sizeof(ctx));
    ctx.machine = &USCXML_MACHINE;

    ctx.is_matched = [] (const uscxml::uscxml_ctx* ctx, const uscxml::uscxml_transition* transition, const void* event) -> int {
        return strcmp(transition->event, ((const struct event_t*)event)->name) == 0;
    };

    int err = USCXML_ERR_OK;

    while((err = uscxml_step(&ctx)) != USCXML_ERR_IDLE) {}
    std::puts("idle");

    event[0] = {"b"};
    ctx.external_queue.push_back((void*)event);
    while((err = uscxml_step(&ctx)) != USCXML_ERR_IDLE) {}

    using f = std::function<void(int)>;
    f a = [] (int i) { std::printf("hello %d\n", i); };
    f b = [] (auto a) {
        return [a] (int i) {
            return a(i + 1);
        };
    }(a);

    b(1);
}


#include <iostream>
#include <boost/python.hpp>
#include <Python.h>
#include <vector>

#include "common.h"
#include "opencv2/cudaarithm.hpp"
#include "opencv2/cudaoptflow.hpp"
#include "opencv2/cudacodec.hpp"

#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/xfeatures2d.hpp"

#include "warp_flow.h"

using namespace cv::cuda;
using namespace cv;

namespace bp = boost::python;

class TVL1FlowExtractor{
public:

    TVL1FlowExtractor(int bound){
        alg_tvl1 = cuda::OpticalFlowDual_TVL1::create();
        bound_ = bound;
    }

    static void set_device(int dev_id){
        setDevice(dev_id);
    }

    bp::list extract_flow(bp::list frames, int img_width, int img_height){
        bp::list output;
        Mat input_frame, prev_frame, next_frame, prev_gray, next_gray;
        Mat flow_x, flow_y;

        // initialize the first frame
        const char* first_data = ((const char*)bp::extract<const char*>(frames[0]));
        input_frame = Mat(img_height, img_width, CV_8UC3);
        initializeMats(input_frame, prev_frame, prev_gray, next_frame, next_gray);

        memcpy(prev_frame.data, first_data, bp::len(frames[0]));
        cvtColor(prev_frame, prev_gray, CV_BGR2GRAY);
        for (int idx = 1; idx < bp::len(frames); idx++){
            const char* this_data = ((const char*)bp::extract<const char*>(frames[idx]));
            memcpy(next_frame.data, this_data, bp::len(frames[0]));
            cvtColor(next_frame, next_gray, CV_BGR2GRAY);

            d_frame_0.upload(prev_gray);
            d_frame_1.upload(next_gray);

            alg_tvl1->calc(d_frame_0, d_frame_1, d_flow);

            GpuMat planes[2];
            cuda::split(d_flow, planes);
            planes[0].download(flow_x);
            planes[1].download(flow_y);

            std::vector<uchar> str_x, str_y;

            encodeFlowMap(flow_x, flow_y, str_x, str_y, bound_, false);
            output.append(
                bp::make_tuple(
                    bp::str((const char*) str_x.data(), str_x.size()),
                    bp::str((const char*) str_y.data(), str_y.size())
                    )
            );

            std::swap(prev_gray, next_gray);
        }
        return output;
    };
private:
    int bound_;
    GpuMat d_frame_0, d_frame_1;
    GpuMat d_flow;
    cv::Ptr<cuda::OpticalFlowDual_TVL1> alg_tvl1;
};

class TVL1WarpFlowExtractor {
public:

    TVL1WarpFlowExtractor(int bound) {
        alg_tvl1 = cuda::OpticalFlowDual_TVL1::create();
        detector_surf = xfeatures2d::SurfFeatureDetector::create(200);
        extractor_surf = xfeatures2d::SurfDescriptorExtractor::create(true, true);
        bound_ = bound;
    }

    static void set_device(int dev_id){
        setDevice(dev_id);
    }

    bp::list extract_warp_flow(bp::list frames, int img_width, int img_height){
        bp::list output;
        Mat input_frame, prev_frame, next_frame, prev_gray, next_gray, human_mask;
        Mat flow_x, flow_y;

        // initialize the first frame
        const char* first_data = ((const char*)bp::extract<const char*>(frames[0]));
        input_frame = Mat(img_height, img_width, CV_8UC3);
        initializeMats(input_frame, prev_frame, prev_gray, next_frame, next_gray);
        human_mask = Mat::ones(input_frame.size(), CV_8UC1);

        memcpy(prev_frame.data, first_data, bp::len(frames[0]));
        cvtColor(prev_frame, prev_gray, CV_BGR2GRAY);
        for (int idx = 1; idx < bp::len(frames); idx++){
            const char* this_data = ((const char*)bp::extract<const char*>(frames[idx]));
            memcpy(next_frame.data, this_data, bp::len(frames[0]));
            cvtColor(next_frame, next_gray, CV_BGR2GRAY);

            d_frame_0.upload(prev_gray);
            d_frame_1.upload(next_gray);

            alg_tvl1->calc(d_frame_0, d_frame_1, d_flow);

            GpuMat planes[2];
            cuda::split(d_flow, planes);
            planes[0].download(flow_x);
            planes[1].download(flow_y);

            // warp to reduce holistic motion
            detector_surf->detect(next_gray, kpts_surf, human_mask);
            extractor_surf->compute(next_gray, kpts_surf, desc_surf);
            ComputeMatch(prev_kpts_surf, kpts_surf, prev_desc_surf, desc_surf, prev_pts_surf, pts_surf);
            MatchFromFlow_copy(next_gray, flow_x, flow_y, prev_pts_flow, pts_flow, human_mask);
            MergeMatch(prev_pts_flow, pts_flow, prev_pts_surf, pts_surf, prev_pts_all, pts_all);
            Mat H = Mat::eye(3, 3, CV_64FC1);
            if(pts_all.size() > 50) {
                std::vector<unsigned char> match_mask;
                Mat temp = findHomography(prev_pts_all, pts_all, RANSAC, 1, match_mask);
                if(cv::countNonZero(Mat(match_mask)) > 25)
                    H = temp;
            }

            Mat H_inv = H.inv();
            Mat gray_warp = Mat::zeros(next_gray.size(), CV_8UC1);
            MyWarpPerspective(prev_gray, next_gray, gray_warp, H_inv);

            d_frame_0.upload(prev_gray);
            d_frame_1.upload(gray_warp);

            alg_tvl1->calc(d_frame_0, d_frame_1, d_flow);

            cuda::split(d_flow, planes);
            planes[0].download(flow_x);
            planes[1].download(flow_y);

            std::vector<uchar> str_x, str_y;

            encodeFlowMap(flow_x, flow_y, str_x, str_y, bound_, false);
            output.append(
                    bp::make_tuple(
                            bp::str((const char*) str_x.data(), str_x.size()),
                            bp::str((const char*) str_y.data(), str_y.size())
                    )
            );

            std::swap(prev_gray, next_gray);
        }
        return output;
    }
private:
    cv::Ptr<Feature2D> detector_surf;
    cv::Ptr<Feature2D> extractor_surf;
    std::vector<Point2f> prev_pts_flow, pts_flow;
    std::vector<Point2f> prev_pts_surf, pts_surf;
    std::vector<Point2f> prev_pts_all, pts_all;
    std::vector<KeyPoint> prev_kpts_surf, kpts_surf;
    Mat prev_desc_surf, desc_surf;

    GpuMat d_frame_0, d_frame_1;
    GpuMat d_flow;

    cv::Ptr<cuda::OpticalFlowDual_TVL1> alg_tvl1;
    int bound_;
};

//// Boost Python Related Decl
BOOST_PYTHON_MODULE(libpydenseflow){
    bp::class_<TVL1FlowExtractor>("TVL1FlowExtractor", bp::init<int>())
            .def("extract_flow", &TVL1FlowExtractor::extract_flow)
            .def("set_device", &TVL1FlowExtractor::set_device)
            .staticmethod("set_device");
    bp::class_<TVL1WarpFlowExtractor>("TVL1WarpFlowExtractor", bp::init<int>())
            .def("extract_warp_flow", &TVL1WarpFlowExtractor::extract_warp_flow)
            .def("set_device", &TVL1WarpFlowExtractor::set_device)
            .staticmethod("set_device");
}


#include <boost/icl/closed_interval.hpp>
#include <boost/icl/continuous_interval.hpp>
#include <boost/icl/discrete_interval.hpp>
#include <boost/icl/dynamic_interval_traits.hpp>
#include <boost/icl/functors.hpp>
#include <boost/icl/gregorian.hpp>
#include <boost/icl/impl_config.hpp>
#include <boost/icl/interval_base_set.hpp>
#include <boost/icl/interval_bounds.hpp>
#include <boost/icl/interval_combining_style.hpp>
#include <boost/icl/interval.hpp>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/interval_set.hpp>
#include <boost/icl/interval_traits.hpp>
#include <boost/icl/iterator.hpp>
#include <boost/icl/left_open_interval.hpp>
#include <boost/icl/map.hpp>
#include <boost/icl/open_interval.hpp>
#include <boost/icl/ptime.hpp>
#include <boost/icl/rational.hpp>
#include <boost/icl/right_open_interval.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/set.hpp>
#include <boost/icl/split_interval_map.hpp>
#include <boost/icl/split_interval_set.hpp>

int
main ()
{
  return 0;
}


/* Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/defs.h"

#include "bio/bifa_algorithm.h"
#include "bio/bifa_test_case.h"
#include "bio/run_match.h"
#include "bio/adjust_hits.h"
#include "bio/biobase_binding_model.h"
#include "bio/match_binding_model.h"
#include "bio/biobase_score.h"

#include <boost/iterator/filter_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>

#include <gsl/gsl_sf_pow_int.h>

#include <iostream>

BIO_NS_START

BiFaInput::BiFaInput( const seq_t & centre_sequence )
: centre_sequence( centre_sequence )
{
}

BiFaAlgorithm::~BiFaAlgorithm() { }

BiFaAlgorithm::ptr_t
BiFaAlgorithm::get_default_algorithm()
{
	static BiFaAlgorithm::ptr_t default_algorithm;

	if( ! default_algorithm )
	{
		default_algorithm.reset( new TransfacBiFaAlgorithm( PssmMatchArgs(), true ) );
	}

	return default_algorithm;
}

TransfacBiFaAlgorithm::TransfacBiFaAlgorithm(
	const PssmMatchArgs & args,
	bool adjust_phylo,
	bool verbose)
: verbose( verbose )
, adjust_phylo( adjust_phylo )
, args( args )
{
}

TransfacBiFaAlgorithm::~TransfacBiFaAlgorithm() { }

std::string
TransfacBiFaAlgorithm::get_name() const
{
	return
		BIO_MAKE_STRING(
			"BiFa-"
			<< ( adjust_phylo ? "phylo" : "no phylo" )
			<< "-"
			<< ( args.use_bayesian ? "bayesian" : "not bayesian" )
			<< "-"
			<< ( args.use_or_better ? "or better" : "equal" )
			<< "-"
			<< args.threshold
			);
}

BindingModel *
TransfacBiFaAlgorithm::get_model_for( const boost::any & key )
{
	return Link2BiobaseBindingModel( BioEnvironment::singleton().get_tf_binding_prior(), args.use_or_better )( boost::any_cast< TableLink >( key ) );
}

void
TransfacBiFaAlgorithm::fill_model_universe( BindingModel::set_t & universe )
{
	transform_biobase_sites_and_matrices(
		args.get_filter(),
		Link2BiobaseBindingModel( BioEnvironment::singleton().get_tf_binding_prior(), args.use_or_better ),
		std::inserter( universe, universe.begin() ) );
}

BiFaOutput::ptr_t
TransfacBiFaAlgorithm::operator()(const BiFaInput & input)
{
	BiFaOutput::ptr_t result(new BiFaOutput);

	//score the sites and matrices
	score_all_biobase_pssms(
		make_sequence_scorer(
			input.centre_sequence.begin(),
			input.centre_sequence.end(),
			args.threshold,
			std::inserter( result->hits, result->hits.begin() )
		),
		args.get_filter(),
		Link2BiobaseBindingModel( BioEnvironment::singleton().get_tf_binding_prior(), args.use_or_better )
	);

	if ( verbose )
	{
		std::cout << result->hits.size() << " matches over threshold\n";
	}

	//are we making adjustments for the phylogenetic sequences
	if ( adjust_phylo )
	{
		//raise the threshold to the power of the number of sequences
		const float_t phylo_threshold = float_t( gsl_sf_pow_int( args.threshold, input.conserved_sequences.size() + 1 ) );

		adjust_hits(
			result->hits,
			input.conserved_sequences,
			phylo_threshold);

		if ( verbose )
		{
			std::cout
				<< "Adjusted for " << input.conserved_sequences.size()
				<< " sequences, still have "
				<< std::count_if(
					result->hits.begin(),
					result->hits.end(),
					boost::bind(
						std::greater< double >(),
						boost::bind(
							&BindingModel::hit_t::get_p_binding,
							_1 ),
						phylo_threshold ) )
				<< " hits above the threshold\n";
		}
	}

	return result;
}

std::string
MatchBiFaAlgorithm::get_name() const
{
	return "Match";
}

struct MatchMapFilter
{
	bool operator()( const TableLink & link ) const
	{
		return get_min_fp_match_map().find( link ) != get_min_fp_match_map().end();
	}

	bool operator()( Matrix::map_t::value_type matrix ) const
	{
		return ( *this )( matrix.first );
	}

	bool operator()( Site::map_t::value_type site ) const
	{
		return false;
	}
};

BindingModel *
MatchBiFaAlgorithm::get_model_for( const boost::any & key )
{
	const TableLink & link = boost::any_cast< const TableLink & >( key );

	return
		MatchMapFilter()( link )
			? Link2MatchBindingModel()( link )
			: 0
			;
}

void
MatchBiFaAlgorithm::fill_model_universe( BindingModel::set_t & universe )
{
	transform_biobase_sites_and_matrices(
		MatchMapFilter(),
		Link2MatchBindingModel(),
		std::inserter( universe, universe.begin() ) );
}

BiFaOutput::ptr_t
MatchBiFaAlgorithm::operator()(const BiFaInput & input)
{
	BiFaOutput::ptr_t result(new BiFaOutput);

	//score the sites and matrices
	score_all_biobase_pssms(
		make_sequence_scorer(
			input.centre_sequence.begin(),
			input.centre_sequence.end(),
			0.5,					// our hits for match algorithm are 0 or 1 so 0.5 is a good enough threshold
			std::inserter( result->hits, result->hits.begin() )
		),
		MatchMapFilter(),
		Link2MatchBindingModel()
	);

	return result;
}

ROCPoint::ROCPoint( double specificity, double sensitivity )
	: specificity( specificity )
	, sensitivity( sensitivity )
{
}

std::ostream &
operator<<( std::ostream & os, const ROCPoint & roc_point )
{
	os << "(" << roc_point.specificity << "," << roc_point.sensitivity << ")";
	return os;
}

BinaryTestResults::BinaryTestResults(
	unsigned true_positives,
	unsigned false_positives,
	unsigned true_negatives,
	unsigned false_negatives)
	: true_positives( true_positives )
	, false_positives( false_positives )
	, true_negatives( true_negatives )
	, false_negatives( false_negatives )
{
}

BinaryTestResults &
BinaryTestResults::operator+=( const BinaryTestResults & rhs )
{
	true_positives += rhs.true_positives;
	false_positives += rhs.false_positives;
	true_negatives += rhs.true_negatives;
	false_negatives += rhs.false_negatives;

	return *this;
}

BinaryTestResults &
BinaryTestResults::operator()(bool tested_positive, bool should_be_positive)
{
	if (tested_positive)
	{
		if (should_be_positive)
		{
			++true_positives;
		}
		else
		{
			++false_positives;
		}
	}
	else
	{
		if (should_be_positive)
		{
			++false_negatives;
		}
		else
		{
			++true_negatives;
		}
	}

	return *this;
}

ROCPoint
BinaryTestResults::get_roc_point() const
{
	return
		ROCPoint(
			double( true_negatives ) / double( true_negatives + false_positives ),
			double( true_positives ) / double( true_positives + false_negatives ) );
}

double
BiFaTestCase::calculate_next_threshold(
	BiFaTestCase::threshold_result_map_t::const_iterator begin,
	BiFaTestCase::threshold_result_map_t::const_iterator end,
	double min_threshold,
	double max_threshold)
{
	//either we use the specificity or sensitivity to choose the next threshold
	static bool use_specificity = ! use_specificity;

	//check args
	if (min_threshold > max_threshold)
	{
		throw std::invalid_argument( "Min must be <= max threshold" );
	}

	//if we have no results try the average
	if ( end == begin )
	{
		return (min_threshold + max_threshold) / 2.0;
	}

	//look for the largest gap between successive iterators
	double largest_gap = 0.0;
	threshold_result_map_t::const_iterator max_gap = end;
	for (threshold_result_map_t::const_iterator left = begin;
		end != left;
		++left)
	{
		threshold_result_map_t::const_iterator right = left;
		++right;
		if (end == right)
		{
			break;
		}

		//is there a bigger gap between these two than any two before?
		const double left_value = use_specificity ? left->second.specificity : left->second.sensitivity;
		const double right_value = use_specificity ? right->second.specificity : right->second.sensitivity;
		const double gap = fabs( left_value - right_value );
		if ( gap > largest_gap )
		{
			largest_gap = gap;
			max_gap = left;
		}
	}

	//check the gaps between the ends of the sequence and the min/max thresholds

	return (min_threshold + max_threshold) / 2.0;
}

BIO_NS_END


/*
 * Copyright 2020-2021 Telecom Paris

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include "phy.h"
#include "../../lib/phy/synchronization/synchronization.h"
#include "../../lib/phy/libphy/libphy.h"
#include "../../lib/variables/common_variables/common_variables.h"
#include "../../lib/utils/sequence_generator/sequence_generator.h"
#include "../../lib/phy/transport_channel/transport_channel.h"
#include <iostream>
#include <vector>
#include <fftw3.h>
#include <fstream>
#include <boost/log/core.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/utility/setup/file.hpp>
#include <boost/log/utility/setup/common_attributes.hpp>
#include "../../lib/asn1c/nr_rrc/BCCH-DL-SCH-Message.h"
#include "../../lib/variables/common_structures/common_structures.h"
#include "../../lib/phy/physical_channel/physical_channel.h"
#include "../../lib/utils/common_utils/common_utils.h"
#include "../../lib/asn1c/nr_rrc/BCCH-DL-SCH-Message.h"
#include "../../lib/asn1c/nr_rrc/FrequencyInfoUL-SIB.h"

using namespace std;

phy::phy(rf *rf_dev, double ssb_period, int fft_size, int scs, free5GRAN::band band_obj) {
    /**
     * \fn phy
     * \param[in] rf_dev: RF device. (Only USRP B210 is currently supported)
     * \param[in] ssb_period: SSB periodicity. Default value is 0.02 (20 ms)
     * \param[in] fft_size: FFT/iFFT size. Represents the total number of os subcarriers to be decoded
     * \param[in] scs: Subcarrier spacing
     * \param[in] band_obj: Band object for getting Lmax value
    */
    this->rf_device = rf_dev;
    this->ssb_period = ssb_period;
    this->fft_size = fft_size;
    this->scs = scs;
    this->band_object = band_obj;
    l_max = band_obj.l_max;
    this->is_extended_cp = 0;
    common_cp_length = 0;
}

int phy::cell_synchronization(float &received_power) {
    /**
     * \fn cell_synchronization
     * \brief Perform time synchronization
     * \details
     * - PSS cross-correlation to retrieve N_ID_2
     * - SSS correlation to retrieve N_ID_1
     * - PCI computation based on N_ID_1 and N_ID_2
     *
     * \param[in] received_power: PSS received power. Used for power ramping.
    */
    BOOST_LOG_TRIVIAL(trace) << "PSS synchronization";

    int n_id_2,synchronisation_index;
    float peak_value;
    received_power = 0;

    size_t num_samples = 2 * ssb_period * rf_device->getSampleRate();

    // Create buffer
    vector<complex<float>> buff_2_ssb_periods(num_samples);
    buff.clear();
    buff.resize(num_samples / 2);

    complex<float> j(0, 1);
    // Get samples from RF layer and put them in buff variable
    time_first_pss = chrono::high_resolution_clock::now();
    try {
        double time_first_sample;
        rf_device->get_samples(&buff_2_ssb_periods, time_first_sample);
    }catch (const exception& e) {
        return 1;
    }
    int num_symbols_per_subframe_pbch = free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP * scs/15e3;
    int cp_lengths_pbch[num_symbols_per_subframe_pbch];
    int cum_sum_pbch[num_symbols_per_subframe_pbch];

    free5GRAN::phy::signal_processing::compute_cp_lengths((int) scs/1e3, fft_size, 0, num_symbols_per_subframe_pbch, cp_lengths_pbch, cum_sum_pbch);
    /*
     * Take second symbol CP as common CP as SSB is never transmitted at long CP symbols (long CP are transmitted every 0.5ms)
     */
    common_cp_length = cp_lengths_pbch[1];
    /*
     * Extract first half of buffer (= 1 SSB period)
     */
    for (int i = 0; i < num_samples / 2; i ++){
        buff[i] = buff_2_ssb_periods[i];
    }

    /*
     * Get PSS correlation result
     */
    free5GRAN::phy::synchronization::search_pss(n_id_2,synchronisation_index,peak_value, common_cp_length, buff, fft_size);
    BOOST_LOG_TRIVIAL(trace) << "Peak value: "+ to_string(peak_value/common_cp_length);
    /*
     * Computing symbol length and first sample index of PSS in buff
     */
    int symbol_duration = fft_size + common_cp_length;
    int pss_start_index = synchronisation_index - symbol_duration + 1;
    int sss_init_index = pss_start_index + 2 * symbol_duration + common_cp_length; // = (synchronisation_index - symbol_duration + 1) + 2 * symbol_duration;
    /*
     * If highest correlation peak is not fully in buffer, cell is not found
     */
    if (pss_start_index < 0){
        return 1;
    }

    index_first_pss = pss_start_index;

    vector<complex<float>> sss_signal(fft_size);
    /*
     * Extracting the SSS signal based on sss_init_index and cp_length
     */
    for (int i = 0; i < fft_size; i++){
        sss_signal[i] = buff[i + sss_init_index];
    }
    /*
     * Computing received power
     */
    for (int i = 0; i < 4 * symbol_duration; i ++){
        received_power += pow(abs(buff[pss_start_index + i]),2);
    }
    received_power /= 4 * symbol_duration;
    received_power = 10 * log10(received_power);
    int n_id_1;
    float peak_value_sss;

    /*
     * Get SSS correlation result
     */
    BOOST_LOG_TRIVIAL(trace) << "SSS synchronization";
    free5GRAN::phy::synchronization::get_sss(n_id_1, peak_value_sss, sss_signal, fft_size, n_id_2);
    BOOST_LOG_TRIVIAL(trace) << "Peak value: "+ to_string(peak_value_sss);
    pci = 3 * n_id_1 + n_id_2;
    BOOST_LOG_TRIVIAL(trace) << "PCI : "+ to_string(pci);

    /*
     * Retreive first SSB symbol of second SSB period
     */
    vector<complex<float>> second_pss(fft_size + common_cp_length), second_sss(fft_size);
    int second_pss_index = pss_start_index + num_samples / 2;
    int n_id_1_2, n_id_2_2, sync_index_pss_2;
    float peak_value_pss_2;
    for (int i = 0; i < fft_size + common_cp_length; i++){
        second_pss[i] = buff_2_ssb_periods[i + second_pss_index];
    }
    /*
     * Retrieve N ID 2 value from second SSB
     */
    free5GRAN::phy::synchronization::search_pss(n_id_2_2,sync_index_pss_2,peak_value_pss_2, common_cp_length, second_pss, fft_size);

    /*
     * Extract SSS symbol from second SSB
     */
    for (int i = 0; i < fft_size; i++){
        second_sss[i] = buff_2_ssb_periods[i + second_pss_index + 2 * symbol_duration + common_cp_length];
    }

    free5GRAN::phy::synchronization::get_sss(n_id_1_2, peak_value_sss, second_sss, fft_size, n_id_2_2);

    if (3 * n_id_1_2 + n_id_2_2 == pci){
        return 0;
    }else {
        return 1;
    }
}

int phy::extract_pbch() {
    /**
     * \fn extract_pbch
     * \brief Time resynchronization, frequency synchronization, PBCH extraction and decoding.
     * \details
     * - Getting 3ms signal from RF device
     * - PSS cross-correlation to retrieve N_ID_2
     * - SSS correlation to retrieve N_ID_1
     * - PCI computation based on N_ID_1 and N_ID_2
     * - Function ends if recomputed PCI differs from to initially computed one
     * - Fine frequency synchronization by correlating cyclic prefixes and corresponding symbol part
     * - Signal extraction and FFT
     * - Resource element de-mapper
     * - Channel estimation based on different values of i_ssb
     * - Channel equalization based on best SNR value
     * - PBCH decoding
     * - BCH decoding
     * - MIB parsing
    */
    BOOST_LOG_TRIVIAL(trace) << "Extracting PBCH";
    // Get at least 30ms of signal (=3 frames, at least 2 complete ones)
    size_t num_samples = max(0.03, ssb_period) * rf_device->getSampleRate();
    //vector<complex<float>> buff(num_samples);
    buff.clear();
    buff.resize(num_samples);

    // Getting samples
    auto now = chrono::high_resolution_clock::now();
    try {
        double second_frame_time;
        rf_device->get_samples(&buff, second_frame_time);
    }catch (const exception& e) {
        return 1;
    }

    /*
     * SYNCHRONIZING IN THE NEW RECEIVED FRAME
     * Computing approximate PSS index inside the received buffer using the time reference of the first PSS index SSB initial search
     */
    auto time_window = chrono::duration_cast<chrono::microseconds>(now - time_first_pss);
    int offset_to_ssb_period = (int)(time_window.count() - index_first_pss / (128*scs *1e-6)) % ((int) (ssb_period * 1e6));
    int index_second_pss = (ssb_period * 1e6 - offset_to_ssb_period) * rf_device->getSampleRate() * 1e-6;

    ofstream data;
    data.open("data.txt");
    for (int i = 0; i < num_samples; i ++){
        data << buff[i];
        data << "\n";
    }
    data.close();

    /*
     * Compute PBCH CP length
     */
    int num_symbols_per_subframe_pbch = free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP * scs/15e3;
    int cp_lengths_pbch[num_symbols_per_subframe_pbch];
    int cum_sum_pbch[num_symbols_per_subframe_pbch];

    free5GRAN::phy::signal_processing::compute_cp_lengths((int) scs/1e3, fft_size, 0, num_symbols_per_subframe_pbch, cp_lengths_pbch, cum_sum_pbch);
    common_cp_length = cp_lengths_pbch[1];
    int symbol_duration = fft_size + common_cp_length;

    vector<complex<float>> pss_signal(48 * symbol_duration);

    /*
     * Isolate PSS signal around calculated new PSS occurence (based on timestamp of first synchronization step)
     */
    int begin_offset = 0;
    int end_offset = 0;
    if (pss_signal.size()/2 <= index_second_pss && pss_signal.size()/2  <= num_samples - index_second_pss){
        begin_offset = pss_signal.size()/2;
        end_offset = pss_signal.size()/2;
    } else if (pss_signal.size()/2 > index_second_pss){
        begin_offset = index_second_pss;
        end_offset = pss_signal.size() - index_second_pss;
    } else if (pss_signal.size()/2  > num_samples - index_second_pss) {
        end_offset = num_samples - index_second_pss;
        begin_offset = pss_signal.size() - (num_samples - index_second_pss);
    }

    // Extracting the signal around the PSS approximation
    int count = 0;
    for (int i = -begin_offset; i < end_offset; i ++){
        pss_signal[count] = buff[(int) index_second_pss + i];
        count ++;
    }

    int synchronisation_index;
    float peak_value;

    /*
     * Downsample signal for better performance
     */
    int downsampling_factor = fft_size / free5GRAN::PSS_SSS_FFT_SIZE;
    BOOST_LOG_TRIVIAL(trace) << "PSS synchronization downsampling factor: " << downsampling_factor;
    int symbol_duration_downsampled = symbol_duration / downsampling_factor;

    vector<complex<float>> pss_signal_downsampled(48 * symbol_duration_downsampled);
    for (int i = 0; i < 48 * symbol_duration_downsampled; i ++){
        pss_signal_downsampled[i] = pss_signal[(size_t) downsampling_factor * i];
    }

    free5GRAN::phy::synchronization::search_pss(this->n_id_2,synchronisation_index,peak_value, common_cp_length / downsampling_factor, pss_signal_downsampled,fft_size / downsampling_factor);

    int pss_start_index = downsampling_factor * (synchronisation_index - symbol_duration_downsampled + 1);

    /*
     * Once synchronization is made on downsampled signal, it can be performed on full signal for finer results
     */
    vector<complex<float>> fine_pss_signal(symbol_duration + (2 * downsampling_factor + 1));
    count = 0;
    for (int i = pss_start_index - downsampling_factor; i < pss_start_index + symbol_duration + (downsampling_factor + 1); i ++){
        fine_pss_signal[count] = pss_signal[i];
        count ++;
    }
    free5GRAN::phy::synchronization::search_pss(this->n_id_2,synchronisation_index,peak_value, common_cp_length, fine_pss_signal,fft_size);

    pss_start_index = pss_start_index + (synchronisation_index - symbol_duration + 1  - downsampling_factor);
    int buffer_pss_index = pss_start_index + index_second_pss - begin_offset;
    index_first_pss = buffer_pss_index;
    int sss_init_index = buffer_pss_index + 2 * symbol_duration + common_cp_length;

    vector<complex<float>> sss_signal(fft_size);
    /*
     * Extracting the SSS signal based on sss_init_index and common_cp_length
     */
    for (int i = 0; i < fft_size; i++){
        sss_signal[i] = buff[i + sss_init_index];
    }

    float peak_value_sss;
    /*
     * Get SSS correlation result
     */
    free5GRAN::phy::synchronization::get_sss(this->n_id_1, peak_value_sss, sss_signal,fft_size,this->n_id_2);
    if (pci == 3 * n_id_1 + n_id_2){
        BOOST_LOG_TRIVIAL(trace) << "PCI confirmed";
        cell_confirmed = true;
    }else{
        BOOST_LOG_TRIVIAL(trace) << "PCI not confirmed";
        cell_confirmed = false;
        return 1;
    }
    /*
     * WE ARE NOW SYNCHONIZED IN OUR NEW FRAME
     * Trying to extract DMRS AND PBCH
     */

    vector<complex<float>> ssb_signal(4 * symbol_duration);

    // Extract SSB signal
    for (int i = 0; i < free5GRAN::NUM_SYMBOLS_SSB * symbol_duration; i ++){
        ssb_signal[i] = buff[i + buffer_pss_index];
    }

    /*
     * Fine frequency correlation
     * Getting phase offset between CP and corresponding part of the OFDM symbol for each of the 4 symbols.
     * phase_offset is the mean phase offset
     */
    free5GRAN::phy::signal_processing::compute_fine_frequency_offset(ssb_signal, symbol_duration, fft_size, common_cp_length, scs, freq_offset, free5GRAN::NUM_SYMBOLS_SSB);

    // Correcting signal based on frequency offset
    free5GRAN::phy::signal_processing::transpose_signal(&buff, freq_offset, rf_device->getSampleRate(), buff.size());

    vector<complex<float>> final_pbch_modulation_symbols(free5GRAN::SIZE_SSB_PBCH_SYMBOLS);

    /*
     * Extracting DMRS AND PBCH modulation symbols
     * ref is the reference grid for resource element demapper
     */
    vector<complex<float>> temp_mod_symbols, temp_mod_symbols2,temp_mod_symbols_dmrs;

    complex<float> pbch_symbols[free5GRAN::SIZE_SSB_PBCH_SYMBOLS];
    complex<float> dmrs_symbols[free5GRAN::SIZE_SSB_DMRS_SYMBOLS];
    complex<float> sss_symbols[free5GRAN::SIZE_PSS_SSS_SIGNAL];
    /*
     * ref[0] -> indexes of PBCH resource elements
     * ref[1] -> indexes of DMRS resource elements
     */
    vector<vector<vector<int>>> ref(3, vector<vector<int>>(free5GRAN::SIZE_SSB_DMRS_SYMBOLS, vector<int>(free5GRAN::NUM_SC_SSB)));
    /*
     * channel_indexes[0] contains PBCH samples indexes
     * channel_indexes[1] contains DMRS samples indexes
     * channel_indexes[2] contains SSS samples indexes
     */
    vector<vector<vector<int>>> channel_indexes = {vector<vector<int>>(2, vector<int>(free5GRAN::SIZE_SSB_PBCH_SYMBOLS)), vector<vector<int>>(2, vector<int>(free5GRAN::SIZE_SSB_DMRS_SYMBOLS)), vector<vector<int>>(2, vector<int>(free5GRAN::SIZE_PSS_SSS_SIGNAL))};

    vector<vector<complex<float>>> ssb_symbols(free5GRAN::NUM_SYMBOLS_SSB - 1, vector<complex<float>>(free5GRAN::NUM_SC_SSB));

    int cum_sum_fft[free5GRAN::NUM_SYMBOLS_SSB];
    for (int symbol = 0; symbol < free5GRAN::NUM_SYMBOLS_SSB; symbol ++){
        cum_sum_fft[symbol] = symbol * symbol_duration;
    }

    /*
     * Recover RE grid from time domain signal
     */
    free5GRAN::phy::signal_processing::fft(ssb_signal, ssb_symbols,fft_size,cp_lengths_pbch,&cum_sum_fft[0],free5GRAN::NUM_SYMBOLS_SSB - 1,free5GRAN::NUM_SC_SSB,1,0);

    free5GRAN::phy::physical_channel::compute_pbch_indexes(ref, pci);
    /*
     * Channel demapping using computed ref grid
     */
    complex<float>* output_channels[] = {pbch_symbols, dmrs_symbols, sss_symbols};
    free5GRAN::phy::signal_processing::channel_demapper(ssb_symbols, ref, output_channels, channel_indexes, 3, free5GRAN::NUM_SYMBOL_PBCH_SSB, free5GRAN::NUM_SC_SSB);

    /*
     * Channel estimation and equalization
     * Creating coefficients arrays
     */
    vector<vector<vector<complex<float>>>> coefficients(free5GRAN::MAX_I_BAR_SSB, vector<vector<complex<float>>>(free5GRAN::NUM_SYMBOL_PBCH_SSB, vector<complex<float>>(free5GRAN::NUM_SC_SSB)));

    complex<float> dmrs_sequence[free5GRAN::SIZE_SSB_DMRS_SYMBOLS];
    float snr[free5GRAN::MAX_I_BAR_SSB];

    /*
     * For each possible iBarSSB value, estimate the corresponding transport_channel
     */
    for (int i = 0; i < free5GRAN::MAX_I_BAR_SSB; i ++){
        free5GRAN::utils::sequence_generator::generate_pbch_dmrs_sequence(pci,i,dmrs_sequence);
        free5GRAN::phy::signal_processing::channelEstimation(dmrs_symbols, dmrs_sequence, channel_indexes[1],coefficients[i], snr[i], free5GRAN::NUM_SC_SSB, free5GRAN::NUM_SYMBOL_PBCH_SSB , free5GRAN::SIZE_SSB_DMRS_SYMBOLS);
    }
    /*
     * Choose the iBarSSB value that maximizes the SNR
     */
    max_snr = snr[0];
    int i_b_ssb = 0;
    for (int i = 1; i < free5GRAN::MAX_I_BAR_SSB ; i ++){
        if (snr[i] > max_snr){
            max_snr = snr[i];
            i_b_ssb = i;
        }
    }

    // Equalize transport_channel
    for (int i = 0; i < free5GRAN::SIZE_SSB_PBCH_SYMBOLS; i ++){
        final_pbch_modulation_symbols[i] = (pbch_symbols[i]) * conj(coefficients[i_b_ssb][channel_indexes[0][0][i]][channel_indexes[0][1][i]]) / (float) pow(abs(coefficients[i_b_ssb][channel_indexes[0][0][i]][channel_indexes[0][1][i]]),2);
    }

    ss_pwr.ss_rsrp = 0;
    for (int i = 0; i < free5GRAN::SIZE_PSS_SSS_SIGNAL; i ++){
        ss_pwr.ss_rsrp +=pow(abs(sss_symbols[i]),2);
    }
    for (int i = 0; i < free5GRAN::SIZE_SSB_DMRS_SYMBOLS; i ++){
        ss_pwr.ss_rsrp +=pow(abs(dmrs_symbols[i]),2);
    }
    ss_pwr.ss_rsrp /= (free5GRAN::SIZE_PSS_SSS_SIGNAL + free5GRAN::SIZE_SSB_DMRS_SYMBOLS);

    ss_pwr.ss_rssi = 0;
    for (int symb = 0 ; symb < free5GRAN::NUM_SYMBOLS_SSB - 1; symb++){
        for (int sc = 0; sc < free5GRAN::NUM_SC_SSB; sc ++){
            ss_pwr.ss_rssi += pow(abs(ssb_symbols[symb][sc]),2);
        }
    }
    // 20 is the number of RB in SSB block
    int n_rb = 20;
    ss_pwr.ss_rssi /= n_rb;
    ss_pwr.ss_rsrq = 10 * log(n_rb * ss_pwr.ss_rsrp / ss_pwr.ss_rssi);
    // Converting RSRP and RSSI from W to dBm
    ss_pwr.ss_rsrp  = 10 * log10(ss_pwr.ss_rsrp) + 30;
    ss_pwr.ss_rssi = 10 * log10(ss_pwr.ss_rssi) + 30;
    ss_pwr.ss_sinr = max_snr;

    this->i_b_ssb = i_b_ssb;
    if (l_max == 4){
        this-> i_ssb = i_b_ssb % 4;
    }else {
        this-> i_ssb = i_b_ssb;
    }

    /*
     * Physical and transport channel decoding
     * MIB parsing
     */
    int bch_bits[free5GRAN::SIZE_SSB_PBCH_SYMBOLS * 2];
    free5GRAN::phy::physical_channel::decode_pbch(final_pbch_modulation_symbols, i_ssb, pci, bch_bits);
    int mib_bits[free5GRAN::BCH_PAYLOAD_SIZE];
    free5GRAN::phy::transport_channel::decode_bch(bch_bits, crc_validated, mib_bits, pci);
    free5GRAN::utils::common_utils::parse_mib(mib_bits, mib_object);
    return 0;
}

phy::phy() {

}

void phy::print_cell_info() {
    /**
     * \fn print_cell_info
     * \brief Print cells global informations and MIB.
    */
    cout << "\n";
    cout << "###### RADIO" << endl;
    cout << "# SS-RSRP: " + to_string(ss_pwr.ss_rsrp) + " dbm" << endl;
    cout << "# SS-RSSI: " + to_string(ss_pwr.ss_rssi) + " dbm" << endl;
    cout << "# SS-RSRQ: " + to_string(ss_pwr.ss_rsrq) + " db" << endl;
    cout << "# SS-SNR: " + to_string(ss_pwr.ss_sinr) + " db" << endl;
    cout << "# Frequency offset: " + to_string(freq_offset) + " Hz" << endl;
    cout << "\n";
    cout << "###### CELL" << endl;
    cout << "## PCI: " + to_string(pci) + ((cell_confirmed) ? " (confirmed)" :  " (not confirmed)") << endl;
    cout << "## CP: ";
    cout << ((is_extended_cp == 0 ) ? "Normal" :  "Extended") << endl;
    cout << "## I_B_SSB: " + to_string(i_b_ssb) << endl;
    cout << "## I_SSB: " + to_string(i_ssb) << endl;
    cout << "\n";
    cout << "###### MIB" << endl;
    cout << "## Frame number: " + to_string(mib_object.sfn) << endl;
    cout << "## PDCCH configuration: " + to_string(mib_object.pdcch_config) << endl;
    cout << "## Subcarrier spacing common: " + to_string(mib_object.scs) << endl;
    cout << "## Cell barred: " + to_string(mib_object.cell_barred) << endl;
    cout << "## DMRS type A position : " + to_string(mib_object.dmrs_type_a_position) << endl;
    cout << "## k SSB: " + to_string(mib_object.k_ssb) << endl;
    cout << "## Intra freq reselection: " + to_string(mib_object.intra_freq_reselection) << endl;
    cout << "## CRC ";
    cout << ((crc_validated) ? "validated" :  "not validated") << endl;
    cout << "\n";
    cout << "#######################################################################" << endl;
    cout << "\n";
}

void phy::reconfigure(int fft_size) {
    this->fft_size = fft_size;
}

void phy::search_pdcch(bool &dci_found) {
    /**
     * \fn search_pdcch
     * \brief PDCCH config extraction, PDCCH blind search and DCI decoding
     * \standard TS 38.213 13
     * \details
     * - Read PDCCH config from MIB
     * - Detect frame beginning
     * - Select frame containing PDCCH and PDSCH based of SFN
     * - Frequency calibration to retrieve center on CORESET0
     * - Compute CCE to REG mapping
     * - Blind search DCI decoding over different candidates:
     *  -# Select a candidate
     *  -# Perform resource element de-mapping and FFT
     *  -# Channel estimation & equalization
     *  -# PDCCH decoding
     *  -# DCI decoding
     *  -# If CRC is validated, candidate is selected and function ends
     *  -# Otherwise, function continues with another candidates
     *
     * \param[out] dci_found: returns true if blind decode succeeds.
    */

    /*
     * If SSB offset is greater than 23, PDCCH is not present in the current BWP
     */
    if(mib_object.k_ssb > 23){
        dci_found = false;
        return;
    }
    mu = log2(mib_object.scs/15);
    int symbol_in_frame = band_object.ssb_symbols[this->i_ssb];
    frame_size = 0.01 * rf_device->getSampleRate();
    num_slots_per_frame = 10 * mib_object.scs/15;

    /*
     * Computing CP lengths of SSB/PBCH and recovering SSB position in frame
     */
    int num_symbols_per_subframe_pbch = free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP * (int) (scs/15e3);
    int cp_lengths_pbch[num_symbols_per_subframe_pbch];
    int cum_sum_pbch[num_symbols_per_subframe_pbch];

    free5GRAN::phy::signal_processing::compute_cp_lengths((int) scs/1e3, fft_size, is_extended_cp, num_symbols_per_subframe_pbch, &cp_lengths_pbch[0], &cum_sum_pbch[0]);

    int num_samples_before_pss = (symbol_in_frame / free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP) * (15e3/scs * frame_size / 10.0) + cum_sum_pbch[symbol_in_frame % free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP];
    int num_samples_after_pss = frame_size - num_samples_before_pss;

    /*
     * Computing new FFT size, based on MIB common SCS
     */
    fft_size = (int) (rf_device->getSampleRate() / (1e3 * mib_object.scs));

    BOOST_LOG_TRIVIAL(trace) << "###### PDCCH Search & decode";
    BOOST_LOG_TRIVIAL(trace) << "## INDEX_1: " + to_string(mib_object.pdcch_config/16);
    BOOST_LOG_TRIVIAL(trace) << "## INDEX_2: " + to_string(mib_object.pdcch_config%16);
    BOOST_LOG_TRIVIAL(trace) << "## SYMBOL IN FRAME: " + to_string(symbol_in_frame);
    BOOST_LOG_TRIVIAL(trace) << "## FRAME SIZE: " + to_string(frame_size);
    BOOST_LOG_TRIVIAL(trace) << "## INDEX PSS: " + to_string(index_first_pss);
    BOOST_LOG_TRIVIAL(trace) << "## SLOTS PER FRAME: " + to_string(num_slots_per_frame);
    BOOST_LOG_TRIVIAL(trace) << "## SAMPLES AFTER PSS: " + to_string(num_samples_after_pss);
    BOOST_LOG_TRIVIAL(trace) << "## BUFFER SIZE: " + to_string(buff.size());
    BOOST_LOG_TRIVIAL(trace) << "## FFT SIZE: " + to_string(fft_size);

    /*
     * Getting two candidate frames in received signal.
     * frame_indexes are the beginning and ending indexes of the two candidate frames
     * frame_numbers stores SFN for each candidate frame
     */
    vector<vector<int>> frame_indexes(2, vector<int>(2));
    int frame_numbers[2];
    free5GRAN::phy::signal_processing::get_candidates_frames_indexes(frame_indexes,frame_numbers,mib_object.sfn, index_first_pss,num_samples_before_pss, frame_size);

    BOOST_LOG_TRIVIAL(trace) << "## FRAME 1 FROM: " + to_string(1e3 * frame_indexes[0][0]/rf_device->getSampleRate()) + " TO: " + to_string(1e3 * frame_indexes[0][1]/rf_device->getSampleRate()) + " ms";
    BOOST_LOG_TRIVIAL(trace) << "## FRAME 2 FROM: " + to_string(1e3 * frame_indexes[1][0]/rf_device->getSampleRate()) + " TO: " + to_string(1e3 * frame_indexes[1][1]/rf_device->getSampleRate()) + " ms";

    /*
     * Computing PDCCH Search Space information
     */
    pdcch_ss_mon_occ = free5GRAN::phy::signal_processing::compute_pdcch_t0_ss_monitoring_occasions(mib_object.pdcch_config, scs, mib_object.scs * 1e3, i_ssb);
    pdcch_ss_mon_occ.n0 = (int)(pdcch_ss_mon_occ.O * pow(2, mu) + floor(i_ssb * pdcch_ss_mon_occ.M)) % num_slots_per_frame;
    pdcch_ss_mon_occ.sfn_parity = (int)((pdcch_ss_mon_occ.O * pow(2, mu) + floor(i_ssb * pdcch_ss_mon_occ.M)) / num_slots_per_frame) % 2;

    BOOST_LOG_TRIVIAL(trace) << "## n0: " + to_string(pdcch_ss_mon_occ.n0) ;
    BOOST_LOG_TRIVIAL(trace) << "## ODD/EVEN ?: " + to_string(pdcch_ss_mon_occ.sfn_parity);

    /*
     * Getting candidate frame which satisfies Search Space SFN parity
     */
    int frame;
    if (frame_numbers[0] % 2 == pdcch_ss_mon_occ.sfn_parity){
        frame = 0;
    }else {
        frame = 1;
    }

    BOOST_LOG_TRIVIAL(trace) << "## FRAME: " + to_string(frame);

    /*
     * Normalizing signal
     */
    complex<float> rms = 0;
    frame_data.resize(frame_size);
    for (int i = 0; i < frame_size; i ++){
        frame_data[i] = buff[i + frame_indexes[frame][0]];
        rms += abs(pow(frame_data[i],2));
    }
    rms = sqrt(rms/complex<float>(frame_size,0));
    for (int i = 0; i < frame_size; i ++){
        frame_data[i] = frame_data[i] / rms;
    }

    /*
     * Computing phase offset from SSB, based on RB offset and k_ssb and transposing signal to center on current BWP (which is here CORESET0)
     */
    float freq_diff = 12 * 1e3 * mib_object.scs * (pdcch_ss_mon_occ.n_rb_coreset / 2 - (10 * ((float) scs / (1e3*mib_object.scs)) + pdcch_ss_mon_occ.offset));
    float freq_diff2 = - 15e3 * mib_object.k_ssb;
    free5GRAN::phy::signal_processing::transpose_signal(&frame_data, freq_diff + freq_diff2 , rf_device->getSampleRate(), frame_size);

    BOOST_LOG_TRIVIAL(trace) << "## FREQ DIFF 1: " + to_string(freq_diff);
    BOOST_LOG_TRIVIAL(trace) << "## FREQ DIFF 2: " + to_string(freq_diff2);

    /*
     * Logging frame data to text file for plotting
     */
    ofstream data;
    data.open("output_files/studied_frame.txt");
    for (int i = 0; i < frame_size; i ++){
        data << frame_data[i];
        data << "\n";
    }
    data.close();

    /*
     * Plotting 4 slots around PDCCH monitoring slots
     */
    int begin_index = (pdcch_ss_mon_occ.n0-1) * frame_size / num_slots_per_frame;
    begin_index = max(begin_index,0);
    ofstream data2;
    data2.open("output_files/moniroting_slots.txt");
    for (int i = 0; i < 4 * frame_size / num_slots_per_frame; i ++){
        data2 << frame_data[i + begin_index];
        data2 << "\n";
    }
    data2.close();

    /*
     * Initialize arrays
     */
    int num_sc_coreset_0 = 12 * pdcch_ss_mon_occ.n_rb_coreset;

    /*
     * Compute CCE-to-REG mapping From TS38.211 7.3.2.2
     */
    int height_reg_rb = free5GRAN::NUMBER_REG_PER_CCE / pdcch_ss_mon_occ.n_symb_coreset;
    int R = 2;
    int C = pdcch_ss_mon_occ.n_rb_coreset / (height_reg_rb * R);
    int j;
    int reg_index[C * R];
    for (int c = 0; c < C; c ++){
        for (int r = 0; r < R; r ++){
            j = c * R + r;
            reg_index[j] = (r * C + c + this->pci) % (pdcch_ss_mon_occ.n_rb_coreset/height_reg_rb);
        }
    }
    for (int i = 0 ; i < C * R ; i ++){
        BOOST_LOG_TRIVIAL(trace) << "## CCE"+ to_string(i) + ": REG" + to_string(reg_index[i]);
    }

    /*
     * Computing current BWP CP lengths
     */
    int num_symbols_per_subframe_pdcch = free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP * mib_object.scs/15;
    int cp_lengths_pdcch[num_symbols_per_subframe_pdcch];
    int cum_sum_pdcch[num_symbols_per_subframe_pdcch];
    free5GRAN::phy::signal_processing::compute_cp_lengths(mib_object.scs, fft_size, is_extended_cp, num_symbols_per_subframe_pdcch, &cp_lengths_pdcch[0], &cum_sum_pdcch[0]);

    ofstream data_pdcch;

    int K, freq_domain_ra_size;
    /*
     * Number of bits for Frequency domain allocation in DCI
     */
    freq_domain_ra_size = ceil(log2(pdcch_ss_mon_occ.n_rb_coreset*(pdcch_ss_mon_occ.n_rb_coreset+1) / 2));
    /*
     * K is the DCI payload size including CRC
     */
    K = freq_domain_ra_size + 4 + 1 + 5 + 2 + 1 + 15 + 24;

    float snr;
    bool validated = false;
    int agg_level, num_candidates, dci_decoded_bits[K-24];
    vector<vector<complex<float>>> global_sequence(pdcch_ss_mon_occ.n_symb_coreset, vector<complex<float>>(pdcch_ss_mon_occ.n_rb_coreset * 3));
    vector<vector<vector<int>>> ref(2, vector<vector<int>>(pdcch_ss_mon_occ.n_symb_coreset, vector<int>(12 * pdcch_ss_mon_occ.n_rb_coreset)));
    vector<vector<complex<float>>> coreset_0_samples(pdcch_ss_mon_occ.n_symb_coreset, vector<complex<float>>(num_sc_coreset_0));
    vector<vector<complex<float>>> coefficients(pdcch_ss_mon_occ.n_symb_coreset, vector<complex<float>>(num_sc_coreset_0));

    /*
     * PDCCH blind search. First, loop over every monitoring slot
     */
    BOOST_LOG_TRIVIAL(trace) << "### PDCCH BLIND SEARCH";
    for (int monitoring_slot = 0; monitoring_slot < 2; monitoring_slot ++){
        pdcch_ss_mon_occ.monitoring_slot = monitoring_slot;
        BOOST_LOG_TRIVIAL(trace) << "## MONITORING SLOT: "+ to_string(monitoring_slot);

        /*
         * Extract corresponding CORESET0 samples. CORESET0 number of symbols is given by PDCCH config in MIB
         * Recover RE grid from time domain signal
         */
        free5GRAN::phy::signal_processing::fft(frame_data, coreset_0_samples,fft_size,cp_lengths_pdcch,cum_sum_pdcch,pdcch_ss_mon_occ.n_symb_coreset,num_sc_coreset_0,pdcch_ss_mon_occ.first_symb_index, (pdcch_ss_mon_occ.n0 + monitoring_slot) * frame_size / num_slots_per_frame);
        for (int symb = 0; symb < pdcch_ss_mon_occ.n_symb_coreset; symb ++){
            /*
             * Generate DMRS sequence for corresponding symbols
             */
            free5GRAN::utils::sequence_generator::generate_pdcch_dmrs_sequence(pci, pdcch_ss_mon_occ.n0 + monitoring_slot, pdcch_ss_mon_occ.first_symb_index + symb, global_sequence[symb], pdcch_ss_mon_occ.n_rb_coreset * 3);
        }
        /*
         * Loop over possible aggregation level (from 2 to 4 included) and candidates
         */
        for (int i = 2; i < 5; i ++){
            agg_level = pow(2, i);
            if (agg_level <= pdcch_ss_mon_occ.n_rb_coreset / height_reg_rb){
                vector<vector<vector<int>>> channel_indexes = {vector<vector<int>>(2, vector<int>((size_t) agg_level * free5GRAN::NUMBER_REG_PER_CCE * 9)), vector<vector<int>>(2, vector<int>((size_t) agg_level * free5GRAN::NUMBER_REG_PER_CCE * 3))};
                vector<complex<float>> pdcch_symbols((size_t) agg_level * free5GRAN::NUMBER_REG_PER_CCE * 9);
                complex<float> temp_pdcch_symbols[agg_level * free5GRAN::NUMBER_REG_PER_CCE * 9];
                complex<float> dmrs_symbols[agg_level * free5GRAN::NUMBER_REG_PER_CCE * 3];
                complex<float> dmrs_sequence[agg_level * free5GRAN::NUMBER_REG_PER_CCE * 3];
                int reg_bundles[agg_level];
                int reg_bundles_ns[agg_level];
                int dci_bits[agg_level * free5GRAN::NUMBER_REG_PER_CCE * 9 * 2];

                BOOST_LOG_TRIVIAL(trace) << "## AGGREGATION LEVEL"+ to_string(agg_level);
                num_candidates = pdcch_ss_mon_occ.n_rb_coreset / (agg_level * height_reg_rb);
                /*
                 * Loop over candidates of current aggregation level
                 */
                for (int p = 0; p < num_candidates; p ++){
                    BOOST_LOG_TRIVIAL(trace) << "## CANDIDATE "+ to_string(p);
                    /*
                     * Extract REG bundles for current candidate and aggregation level
                     */
                    for (int l = 0; l < agg_level; l ++){
                        reg_bundles[l] = reg_index[l + p * agg_level];
                        reg_bundles_ns[l] = reg_index[l + p * agg_level];
                    }
                    sort(reg_bundles, reg_bundles+agg_level);
                    /*
                     * PDCCH samples extraction
                     */
                    for (int symbol = 0; symbol < pdcch_ss_mon_occ.n_symb_coreset; symbol ++) {
                        for (int sc = 0; sc < 12 * pdcch_ss_mon_occ.n_rb_coreset; sc ++){
                            ref[1][symbol][sc] = 0;
                            ref[0][symbol][sc] = 0;
                        }
                    }
                    /*
                     * Computing PDCCH candidate position in RE grid
                     */
                    free5GRAN::phy::physical_channel::compute_pdcch_indexes(ref, pdcch_ss_mon_occ, agg_level, reg_bundles, height_reg_rb);
                    /*
                     * Channel de-mapping
                     */
                    complex<float>* output_channels[] = {temp_pdcch_symbols,dmrs_symbols};
                    free5GRAN::phy::signal_processing::channel_demapper(coreset_0_samples, ref, output_channels, channel_indexes, 2, pdcch_ss_mon_occ.n_symb_coreset, 12 * pdcch_ss_mon_occ.n_rb_coreset);
                    /*
                     * DMRS CCE-to-REG de-mapping/de-interleaving
                     */
                    for (int k = 0 ; k < agg_level; k ++){
                        for (int reg = 0; reg < free5GRAN::NUMBER_REG_PER_CCE; reg ++){
                            dmrs_sequence[((agg_level * free5GRAN::NUMBER_REG_PER_CCE * 3) / pdcch_ss_mon_occ.n_symb_coreset) * (reg%pdcch_ss_mon_occ.n_symb_coreset) +  k * height_reg_rb * 3 + (reg/pdcch_ss_mon_occ.n_symb_coreset) * 3] = global_sequence[reg%pdcch_ss_mon_occ.n_symb_coreset][reg_bundles[k] * height_reg_rb * 3 + (reg/pdcch_ss_mon_occ.n_symb_coreset) * 3];
                            dmrs_sequence[((agg_level * free5GRAN::NUMBER_REG_PER_CCE * 3) / pdcch_ss_mon_occ.n_symb_coreset) * (reg%pdcch_ss_mon_occ.n_symb_coreset) +  k * height_reg_rb * 3 + (reg/pdcch_ss_mon_occ.n_symb_coreset) * 3 + 1] = global_sequence[reg%pdcch_ss_mon_occ.n_symb_coreset][reg_bundles[k] * height_reg_rb * 3 + (reg/pdcch_ss_mon_occ.n_symb_coreset) * 3 + 1];
                            dmrs_sequence[((agg_level * free5GRAN::NUMBER_REG_PER_CCE * 3) / pdcch_ss_mon_occ.n_symb_coreset) * (reg%pdcch_ss_mon_occ.n_symb_coreset) +  k * height_reg_rb * 3 + (reg/pdcch_ss_mon_occ.n_symb_coreset) * 3 + 2] = global_sequence[reg%pdcch_ss_mon_occ.n_symb_coreset][reg_bundles[k] * height_reg_rb * 3 + (reg/pdcch_ss_mon_occ.n_symb_coreset) * 3 + 2];
                        }
                    }
                    /*
                     * Channel estimation
                     */
                    free5GRAN::phy::signal_processing::channelEstimation(dmrs_symbols, dmrs_sequence, channel_indexes[1],coefficients, snr, 12 * pdcch_ss_mon_occ.n_rb_coreset, pdcch_ss_mon_occ.n_symb_coreset , agg_level * free5GRAN::NUMBER_REG_PER_CCE * 3);
                    /*
                     * Channel equalization
                     */
                    for (int sc = 0; sc < agg_level * free5GRAN::NUMBER_REG_PER_CCE * 9; sc ++){
                        pdcch_symbols[sc] = (temp_pdcch_symbols[sc]) * conj(coefficients[channel_indexes[0][0][sc]][channel_indexes[0][1][sc]]) / (float) pow(abs(coefficients[channel_indexes[0][0][sc]][channel_indexes[0][1][sc]]),2);
                    }
                    /*
                     * PDCCH and DCI decoding
                     */
                    free5GRAN::phy::physical_channel::decode_pdcch(pdcch_symbols,dci_bits,agg_level, reg_bundles_ns, reg_bundles, pci);
                    free5GRAN::phy::transport_channel::decode_dci(dci_bits, agg_level * free5GRAN::NUMBER_REG_PER_CCE * 9 * 2, K, free5GRAN::SI_RNTI, validated, dci_decoded_bits);
                    /*
                     * If DCI CRC is validated, candidate is validated, blind search ends
                     */
                    if (validated){
                        data_pdcch.open("output_files/pdcch_constellation.txt");
                        for (int sc = 0; sc < agg_level * free5GRAN::NUMBER_REG_PER_CCE * 9; sc ++){
                            data_pdcch << pdcch_symbols[sc];
                            data_pdcch << "\n";
                        }
                        data_pdcch.close();
                        goto dci_found_and_validated;
                    }

                }
            }else {
                break;
            }
        }
    }

    dci_found_and_validated:
    BOOST_LOG_TRIVIAL(trace) << "## DCI FOUND AND " << ((validated) ? "VALIDATED" :  "NOT VALIDATED");

    dci_found = false;
    if (validated){
        parse_dci_1_0_si_rnti(dci_decoded_bits,freq_domain_ra_size,dci_1_0_si_rnti);
        n_size_bwp = pdcch_ss_mon_occ.n_rb_coreset;
        /*
         * In current version, only redundancy version 0 and 3 are supported for DL-SCH decoding
         */
        if (dci_1_0_si_rnti.rv == 0 || dci_1_0_si_rnti.rv == 3){
            dci_found = true;
        }

    }
}

void phy::print_dci_info() {
    /**
     * \fn print_dci_info
     * \brief Print DCI decoded informations
    */
    cout << "###### DCI" << endl;
    cout << "# RIV: " + to_string(dci_1_0_si_rnti.RIV)<< endl;
    cout << "# Time Domain RA: " + to_string(dci_1_0_si_rnti.TD_ra) << endl;
    cout << ((dci_1_0_si_rnti.vrb_prb_interleaving == 0 ) ? "# Non-interleaved VRB to PRB" :  "# Interleaved VRB to PRB") << endl;
    cout << "# Modulation coding scheme: " + to_string(dci_1_0_si_rnti.mcs) << endl;
    cout << "# Redudancy version: " + to_string(dci_1_0_si_rnti.rv) << endl;
    cout << ((dci_1_0_si_rnti.si == 0 ) ? "# SIB1 message" :  "# Other SIB message") << endl;
    cout << "#######################################################################" << endl;
    if (dci_1_0_si_rnti.rv == 1 || dci_1_0_si_rnti.rv == 2){
        cout << "WARNING: Redudancy version " + to_string(dci_1_0_si_rnti.rv) << " is not supported by current decoder. To decode SIB1 data on this cell, please use CELL_SEARCH function in config and specify the cell frequency. Retry until redundancy version is not 1 or 2" << endl;
        cout << "#######################################################################" << endl;
    }
    cout << "\n";
}

void phy::parse_dci_1_0_si_rnti(int *dci_bits, int freq_domain_ra_size, free5GRAN::dci_1_0_si_rnti &dci) {
    /**
     * \fn parse_dci_1_0_si_rnti
     * \brief Parse DCI informations
     * \param[in] dci_bits: DCI decoded bits
     * \param[in] freq_domain_ra_size: Number of bits used for frequency allocation in DCI
     * \param[out] dci: Filled DCI object
    */

    dci.RIV = 0;
    for (int i = 0 ; i < freq_domain_ra_size; i ++){
        dci.RIV += dci_bits[i] * pow(2, freq_domain_ra_size - i - 1);
    }
    dci.TD_ra = 0;
    for (int i = 0; i < 4; i ++){
        dci.TD_ra += dci_bits[i + freq_domain_ra_size] * pow(2, 4 - i - 1);
    }

    dci.vrb_prb_interleaving = dci_bits[freq_domain_ra_size + 4];

    dci.mcs = 0;
    for (int i = 0; i < 5; i ++){
        dci.mcs += dci_bits[i + freq_domain_ra_size + 4 + 1] * pow(2, 5 - i - 1);
    }

    dci.rv = 0;
    for (int i = 0; i < 2; i ++){
        dci.rv += dci_bits[i + freq_domain_ra_size + 4 + 1 + 5] * pow(2, 2 - i - 1);
    }

    dci.si = dci_bits[freq_domain_ra_size + 4 + 1 + 5 + 2];
}

void phy::extract_pdsch() {
    /**
     * \fn extract_pdsch
     * \brief PDSCH extraction, PDSCH decoding, DL-SCH decoding and SIB1 parsing
     * \details
     * - Parameters extraction from DCI and standard
     * - Phase de-compensation. Looping over different possible phase compensation:
     *  -# Signal extraction, FFT and resource element de-mapper
     *  -# Channel estimation & equalization
     *  -# PDSCH decoding
     *  -# DL-SCH decoding
     *  -# If CRC is validated, phase de-compensation is validated and functions continues. Otherwise, another phase de-compensation is tried.
     * - SIB1 parsing using ASN1C
    */

    BOOST_LOG_TRIVIAL(trace) << "#### DECODING PDSCH";
    /*
     * Extracting PDSCH time and frequency position
     */
    int lrb, rb_start, k0, S, L, mod_order, code_rate, l0;
    free5GRAN::phy::signal_processing::compute_rb_start_lrb_dci(dci_1_0_si_rnti.RIV, n_size_bwp,lrb,rb_start);
    k0 = free5GRAN::TS_38_214_TABLE_5_1_2_1_1_2[dci_1_0_si_rnti.TD_ra][mib_object.dmrs_type_a_position - 2][1];
    S = free5GRAN::TS_38_214_TABLE_5_1_2_1_1_2[dci_1_0_si_rnti.TD_ra][mib_object.dmrs_type_a_position - 2][2];
    L = free5GRAN::TS_38_214_TABLE_5_1_2_1_1_2[dci_1_0_si_rnti.TD_ra][mib_object.dmrs_type_a_position - 2][3];
    string mapping_type = ((free5GRAN::TS_38_214_TABLE_5_1_2_1_1_2[dci_1_0_si_rnti.TD_ra][mib_object.dmrs_type_a_position - 2][0] == 0 ) ? "A" :  "B");
    mod_order = free5GRAN::TS_38_214_TABLE_5_1_3_1_1[dci_1_0_si_rnti.mcs][0];
    code_rate = free5GRAN::TS_38_214_TABLE_5_1_3_1_1[dci_1_0_si_rnti.mcs][1];
    BOOST_LOG_TRIVIAL(trace) << "## Frequency domain RA: RB Start " + to_string(rb_start) + " and LRB " + to_string(lrb) ;
    BOOST_LOG_TRIVIAL(trace) << "## Time domain RA: K0 " + to_string(k0) + ", S " + to_string(S) + " and L " + to_string(L) + " (mapping type "+mapping_type+")";
    BOOST_LOG_TRIVIAL(trace) << "## MCS: Order " + to_string(mod_order) + " and code rate " + to_string(code_rate);
    BOOST_LOG_TRIVIAL(trace) << "## Slot number " + to_string(pdcch_ss_mon_occ.n0 + pdcch_ss_mon_occ.monitoring_slot + k0);

    /*
     * Compute number of additionnal DMRS positions
     */
    int additionnal_position;
    if (mapping_type == "A"){
        additionnal_position = 2;
    }else {
        if (L == 2 || L == 4){
            additionnal_position = 0;
        }else if(L == 7){
            additionnal_position = 1;
        }else {
            additionnal_position = 0;
        }
    }

    int *dmrs_symbols, num_symbols_dmrs;
    /*
     * Get PDSCH DMRS symbols indexes
     */
    free5GRAN::phy::signal_processing::get_pdsch_dmrs_symbols(mapping_type, L + S, additionnal_position, mib_object.dmrs_type_a_position, &dmrs_symbols, num_symbols_dmrs);

    float snr;

    complex<float> dmrs_sequence[6 * lrb * num_symbols_dmrs];

    int count_dmrs_symbol = 0;

    int num_symbols_per_subframe_pdsch = free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP * mib_object.scs/15;
    int cp_lengths_pdsch[num_symbols_per_subframe_pdsch];
    int cum_sum_pdsch[num_symbols_per_subframe_pdsch];

    vector<vector<complex<float>>> pdsch_ofdm_symbols(L, vector<complex<float>>(12 * pdcch_ss_mon_occ.n_rb_coreset)), pdsch_samples(L, vector<complex<float>>(12 * lrb));
    vector<vector<vector<int>>> ref(2, vector<vector<int>>(L, vector<int>(12 * lrb)));
    vector<vector<vector<int>>> channel_indexes = {vector<vector<int>>(2, vector<int>((size_t) 12 * lrb * (L - num_symbols_dmrs))), vector<vector<int>>(2, vector<int>((size_t) 6 * lrb * num_symbols_dmrs))};
    vector<vector<complex<float>>> coefficients(L, vector<complex<float>>(12 * lrb));
    complex<float> temp_dmrs_sequence[6 * pdcch_ss_mon_occ.n_rb_coreset], pdsch_samples_only[12 * lrb * (L - num_symbols_dmrs)], dmrs_samples_only[6 * lrb * num_symbols_dmrs];

    /*
     * Compute PDSCH CP lengths (same as PDCCH, as it is the same BWP)
     */
    free5GRAN::phy::signal_processing::compute_cp_lengths(mib_object.scs, fft_size, is_extended_cp, num_symbols_per_subframe_pdsch, cp_lengths_pdsch, cum_sum_pdsch);

    /*
     * Recover RE grid from time domain signal
     */
    free5GRAN::phy::signal_processing::fft(frame_data, pdsch_ofdm_symbols,fft_size,cp_lengths_pdsch,cum_sum_pdsch,L,12 * pdcch_ss_mon_occ.n_rb_coreset,S, (pdcch_ss_mon_occ.n0 + pdcch_ss_mon_occ.monitoring_slot + k0) * frame_size / num_slots_per_frame);

    bool dmrs_symbol_array[L];
    /*
     * PDSCH extraction
     */
    for (int symb = 0; symb < L; symb ++){
        bool dmrs_symbol = false;
        /*
         * Check if studied symbol is a DMRS
         */
        for (int j = 0; j < num_symbols_dmrs; j ++){
            if (symb + S == dmrs_symbols[j]){
                dmrs_symbol = true;
                break;
            }
        }
        dmrs_symbol_array[symb] = dmrs_symbol;
        /*
         * Get DMRS sequence
         */
        free5GRAN::utils::sequence_generator::generate_pdsch_dmrs_sequence(free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP, pdcch_ss_mon_occ.n0 + pdcch_ss_mon_occ.monitoring_slot + k0, symb + S, 0, pci, temp_dmrs_sequence, 6 * pdcch_ss_mon_occ.n_rb_coreset);
        if (dmrs_symbol){
            for (int i = 0; i < 6 * lrb; i ++){
                dmrs_sequence[count_dmrs_symbol * 6 * lrb + i] = temp_dmrs_sequence[rb_start * 6 + i];
            }
            count_dmrs_symbol += 1;
        }

        for (int i = 0; i < 12 * lrb; i ++){
            pdsch_samples[symb][i] = pdsch_ofdm_symbols[symb][12 * rb_start + i];
        }
    }
    free5GRAN::phy::physical_channel::compute_pdsch_indexes(ref, dmrs_symbol_array, L, lrb);
    /*
     * Channel de-mapping
     */
    complex<float>* output_channels[] = {pdsch_samples_only,dmrs_samples_only};
    free5GRAN::phy::signal_processing::channel_demapper(pdsch_samples, ref, output_channels, channel_indexes, 2, L, 12 * lrb);
    bool validated;
    float f0 = 0;
    /*
    * Phase decompensator. As phase compensation is not known a priori, we have to loop over different possibles phase compensation for decoding
    */
    for (int phase_decomp_index = 0; phase_decomp_index < 50; phase_decomp_index++){
        /*
         * Compute phase decomp value
         */
        f0 += (phase_decomp_index % 2) * pow(2,mu) * 1e3;
        float phase_offset = (phase_decomp_index % 2) ? f0 : -f0;
        BOOST_LOG_TRIVIAL(trace) << "PHASE DECOMP " << phase_offset ;
        complex<float> phase_decomp[num_symbols_per_subframe_pdsch];
        /*
         * Compute phase decompensation value for each symbol in  a subframe
         */
        free5GRAN::phy::signal_processing::compute_phase_decomp(cp_lengths_pdsch, cum_sum_pdsch, rf_device->getSampleRate(),phase_offset,num_symbols_per_subframe_pdsch,phase_decomp);
        /*
         * Phase de-compensation
         */
        for (int samp = 0; samp < 12 * lrb * (L - num_symbols_dmrs); samp ++){
            pdsch_samples_only[samp] = pdsch_samples_only[samp] * phase_decomp[((pdcch_ss_mon_occ.n0 + pdcch_ss_mon_occ.monitoring_slot + k0) % (num_slots_per_frame / 10)) * free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP + S + channel_indexes[0][0][samp]];
        }
        for (int samp = 0; samp < 6 * lrb * num_symbols_dmrs; samp ++){
            dmrs_samples_only[samp] = dmrs_samples_only[samp] * phase_decomp[((pdcch_ss_mon_occ.n0 + pdcch_ss_mon_occ.monitoring_slot + k0) % (num_slots_per_frame / 10)) * free5GRAN::NUMBER_SYMBOLS_PER_SLOT_NORMAL_CP + S + channel_indexes[1][0][samp]];
        }
        /*
         * Channel estimation
         */
        free5GRAN::phy::signal_processing::channelEstimation(dmrs_samples_only, dmrs_sequence, channel_indexes[1],coefficients, snr, 12 * lrb, L, 6 * lrb * num_symbols_dmrs);
        /*
         * Channel equalization
         */
        vector<complex<float>> pdsch_samples_vector((size_t) 12 * lrb * (L - num_symbols_dmrs));
        for (int sc = 0; sc <  12 * lrb * (L - num_symbols_dmrs); sc ++){
            pdsch_samples_vector[sc] = (pdsch_samples_only[sc]) * conj(coefficients[channel_indexes[0][0][sc]][channel_indexes[0][1][sc]]) / (float) pow(abs(coefficients[channel_indexes[0][0][sc]][channel_indexes[0][1][sc]]),2);
        }

        ofstream data_pdsch;
        data_pdsch.open("output_files/pdsch_constellation.txt");
        for (int i = 0; i < 12 * lrb * (L - num_symbols_dmrs); i ++){
            data_pdsch << pdsch_samples_vector[i];
            data_pdsch << "\n";
        }
        data_pdsch.close();

        /*
         * PDSCH and DL-SCH decoding
         */
        double dl_sch_bits[2 * pdsch_samples_vector.size()];
        free5GRAN::phy::physical_channel::decode_pdsch(pdsch_samples_vector, dl_sch_bits, pci);
        int n_re = free5GRAN::phy::signal_processing::compute_nre(L, num_symbols_dmrs);

        vector<int> desegmented = free5GRAN::phy::transport_channel::decode_dl_sch(dl_sch_bits, n_re, (float) code_rate / (float) 1024, lrb,2 * pdsch_samples_vector.size(), validated, dci_1_0_si_rnti);
        /*
         * If DL-SCH CRC is validated, Phase decompensation is validated
         */
        if (validated){
            for (int i =0; i < desegmented.size(); i ++){
                cout << desegmented[i];
            }
            cout << "\n";
            int bytes_size = (int) ceil(desegmented.size()/8.0);
            uint8_t dl_sch_bytes[bytes_size];
            for (int i = 0; i < desegmented.size(); i ++){
                if (i % 8 == 0){
                    dl_sch_bytes[i/8] = 0;
                }
                dl_sch_bytes[i/8] += desegmented[i] * pow(2, 8 - (i%8) - 1);
            }
            asn_dec_rval_t dec_rval = asn_decode(0, ATS_UNALIGNED_BASIC_PER, &asn_DEF_BCCH_DL_SCH_Message,(void **) &sib1, dl_sch_bytes, bytes_size);
            if (dec_rval.code == RC_OK) {
                BOOST_LOG_TRIVIAL(trace) << "SIB1 parsing succeeded";
            }
            else {
                BOOST_LOG_TRIVIAL(trace) << "SIB1 parsing failed";
            }
            break;
        }
    }

}

BCCH_DL_SCH_Message_t *phy::getSib() {
    return this->sib1;
}

void phy::print_sib1() {
    asn_fprint(stdout, &asn_DEF_BCCH_DL_SCH_Message, sib1);
}

int phy::getSIB1RV() {
    return dci_1_0_si_rnti.rv;
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_DecoupledCompleteDopplerBroadenedPhotonEnergyDistribution.hpp
//! \author Alex Robinson
//! \brief  The decoupled complete Doppler broadened photon energy dist. decl.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_DECOUPLED_COMPLETE_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_HPP
#define MONTE_CARLO_DECOUPLED_COMPLETE_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_HPP

// Boost Includes
#include <boost/scoped_ptr.hpp>

// FRENSE Includes
#include "MonteCarlo_CompleteDopplerBroadenedPhotonEnergyDistribution.hpp"
#include "MonteCarlo_ComptonProfileSubshellConverter.hpp"
#include "Utility_TabularOneDDistribution.hpp"

namespace MonteCarlo{

//! The decoupled complete Doppler broadenening photon energy dist. class
class DecoupledCompleteDopplerBroadenedPhotonEnergyDistribution : public CompleteDopplerBroadenedPhotonEnergyDistribution
{

public:

  //! Constructor
  DecoupledCompleteDopplerBroadenedPhotonEnergyDistribution(
	       const Teuchos::Array<double>& endf_subshell_occupancies,
	       const Teuchos::Array<SubshellType>& endf_subshell_order,
	       const Teuchos::Array<double>& old_subshell_binding_energies,
	       const Teuchos::Array<double>& old_subshell_occupancies,
	       const ElectronMomentumDistArray& electron_momentum_dist_array );

  //! Destructor
  virtual ~DecoupledCompleteDopplerBroadenedPhotonEnergyDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  double evaluate( const double incoming_energy,
		   const double outgoing_energy,
		   const double scattering_angle_cosine ) const;

  //! Evaluate the subshell distribution
  double evaluateSubshell( const double incoming_energy,
			   const double outgoing_energy,
			   const double scattering_angle_cosine,
			   const SubshellType subshell ) const;

  //! Evaluate the PDF
  double evaluatePDF( const double incoming_energy,
		      const double outgoing_energy,
		      const double scattering_angle_cosine ) const;

  //! Evaluate the PDF
  double evaluateSubshellPDF( const double incoming_energy,
			      const double outgoing_energy,
			      const double scattering_angle_cosine,
			      const SubshellType subshell ) const;

  //! Evaluate the integrated cross section (b/mu)
  double evaluateIntegratedCrossSection( const double incoming_energy,
					 const double scattering_angle_cosine,
					 const double precision ) const;

  //! Evaluate the integrated cross section (b/mu)
  double evaluateSubshellIntegratedCrossSection(
				          const double incoming_energy,
					  const double scattering_angle_cosine,
					  const SubshellType subshell,
					  const double precision ) const;

  //! Sample an outgoing energy from the distribution
  void sample( const double incoming_energy,
	       const double scattering_angle_cosine,
	       double& outgoing_energy,
	       SubshellType& shell_of_interaction ) const;

  //! Sample an outgoing energy and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
			      const double scattering_angle_cosine,
			      double& outgoing_energy,
			      SubshellType& shell_of_interaction,
			      unsigned& trials ) const;

private:

  // Sample the old subshell that is interacted with
  void sampleOldInteractionSubshell(
				   unsigned& old_shell_of_interaction,
				   double& old_subshell_binding_energy ) const;

  // The old subshell interaction probabilities
  boost::scoped_ptr<const Utility::TabularOneDDistribution>
  d_old_subshell_occupancy_distribution;

  // The old subshell binding energies
  Teuchos::Array<double> d_old_subshell_binding_energy;

  // The old subshell occupandies
  Teuchos::Array<double> d_old_subshell_occupancies;

  // Records if the Compton profiles are half (standard) or full
  bool d_half_profiles;

  // The electron momentum dist array
  ElectronMomentumDistArray d_electron_momentum_distribution;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_DECOUPLED_COMPLETE_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_DecoupledCompleteDopplerBroadenedPhotonEnergyDistribution.hpp
//---------------------------------------------------------------------------//


#include "mwoibn/hierarchical_control/actions/merge.h"
#include <boost/dynamic_bitset.hpp>

//
mwoibn::hierarchical_control::actions::Merge::Merge(actions::Compute& main_task, actions::Compute& secondary_task,  hierarchical_control::State& state, mwoibn::Scalar eps, mwoibn::Scalar p, mwoibn::hierarchical_control::tasks::Constraints& constraints, mwoibn::robot_class::Robot& robot) : Primary(main_task.getTask(), _merge_memory), _secondary(secondary_task), _eps(eps), _primary(main_task), _snap(actions::Snap(state.P, state.command, _merge_memory)), _controller_state(state), _p(p){

        if(_task.getTaskSize() != _secondary.getTask().getTaskSize())
                throw(std::invalid_argument("Couldn't intialize Merge task, incompatibile tasks sizes."));

        unsigned int size = _task.getTaskSize();

        mwoibn::VectorBool id(size);
        mwoibn::VectorN gain(size);
        mwoibn::VectorN damping(size);

        _support_tasks.assign(std::pow(2,size), tasks::BasicTask(size, _task.getTaskDofs()));
        _support_actions.reserve(std::pow(2,size ));
        for (int i = 0; i < std::pow(2,size ); i++) {
                boost::dynamic_bitset<> b(size, i);
                for(int j = 0; j < size; j++) {
                        id[j] = b[j];
                        gain[j] = id[j] ? _primary.gain()[j] : secondary_task.gain()[j];
                        damping[j] = id[j] ? std::sqrt(_primary.damping(j)) : std::sqrt(secondary_task.damping(j));

                }
                _support_actions.insert(std::make_pair(id, actions::Compute(_support_tasks[i], gain, damping, _controller_state, _merge_memory)));
        }

        _front_tasks.assign(2, merge::Front(_merge_memory, _local_map, *this));
        _end_tasks.assign(2, merge::End(_controller_state.P, _controller_state.command, _merge_memory, _local_map, *this, _controller_state.dt, _p, constraints, robot));
        _replace_tasks.assign(std::pow(2, size+1), merge::Replace(_controller_state.P, _controller_state.command, _merge_memory, _local_map, *this, _controller_state.dt, _p));
        _snaps.assign(std::pow(2, size+3), actions::Snap(_controller_state.P, _controller_state.command, _merge_memory));

        _merge_memory.release(_front_tasks);
        _merge_memory.release(_end_tasks);
        _merge_memory.release(_replace_tasks);
        _merge_memory.release(_snaps);

        mwoibn::VectorBool ones;
        ones.setConstant(_task.getTaskSize(), true);
        merge::Front *ptr = _merge_memory.local_front.get();

        ptr->assign(_support_actions.at(ones), nullptr);
        _last = ptr;

        _local_map[_support_actions.at(ones).getTask()] = ptr;
        _running_id.setOnes(_task.getTaskSize());
        _current_id.setOnes(_task.getTaskSize());

        __error.setZero(_task.getTaskSize());
        __jacobian.setZero(_task.getTaskSize(), _controller_state.dofs);
}

void mwoibn::hierarchical_control::actions::Merge::run(){
        _snap.run();
        _primary.run();
        _check();

        if (_local_map.size() != local_size){
          std::cout << "stack " << _local_map.size() << std::endl;
          for(auto& support : _support_actions){
            if(_local_map.exist(support.second.getTask())) std::cout << support.first.transpose() << std::endl;
            //break;
          }

          counter = 0;
          local_size = _local_map.size();
        }
        counter++;

//        if(counter < 10)
//            std::cout << counter << "\t" << _secondary.getTask().getError() << std::endl;

//        std::cout << "runnung " << _running_id.transpose() << std::endl;

//        if (_current_id.all() && &_last->action() == &_primary) return;
        _snap.restore();
        if (_current_id == _running_id ) {
          _secondary.getTask().update();

                _updateTasks();
                //std::cout << "STACK" << std::endl;
                _last->run();
                //_last = _last->next();

                //std::cout << _controller_state.command.head<12>().transpose() << std::endl;
                return;
        }

        std::cout << _current_id.transpose() << "\t" <<   _running_id.transpose() << std::endl;

        _secondary.getTask().update();

        for(int i = 0; i < _current_id.size(); i++){
          if(_current_id[i] == 1 & _running_id[i] == 0) _reset(i);
        }
        _secondary.getTask().update();

        _read();

        actions::Task* action = &_support_actions.at(_current_id);
        if(_local_map.exist(action->baseAction().getTask())) {
                std::cout << "swap" << std::endl;
                _local_map[action->baseAction().getTask()]->swap(_last->baseAction());
        }
        else{
                //std::cout << "add" << std::endl;
                merge::End* _end =  _merge_memory.local_end.get();
                mwoibn::hierarchical_control::tasks::BasicTask& old = _last->baseAction().getTask();
                _last->push(*_end);
                _end->assign(*action, *_local_map[old]);
                _last = _end;
        }

        _updateTasks();
        _running_id.noalias() = _current_id;
        //std::cout << "STACK" << std::endl;
        _last->run();
        //std::cout << _controller_state.command.head<12>().transpose() << std::endl;

}

void mwoibn::hierarchical_control::actions::Merge::_updateTasks(){

        // simplest way for now
        // iterate through all tasks and check if it is in the make_pair

        for(auto& pair : _support_actions) {
                if (!_local_map.exist(pair.second.baseAction().getTask())) continue;
                for(int i = 0; i < pair.first.size(); i++) {
                        __error[i] = pair.first[i] ? _primary.getTask().getError()[i] : _secondary.getTask().getError()[i];

                        __jacobian.row(i) = pair.first[i] ? _primary.getTask().getJacobian().row(i) : _secondary.getTask().getJacobian().row(i);
                }
                pair.second.baseAction().getTask().updateError(__error);
                pair.second.baseAction().getTask().updateJacobian(__jacobian);

        }
}

void mwoibn::hierarchical_control::actions::Merge::_check(){
//        std::cout << "max coeffs" << std::endl;
        for(int i = 0; i < _task.getTaskSize(); i++) {
                _current_id[i] = _primary.getJacobian().row(i).cwiseAbs().maxCoeff() > _eps;
                //if(i == 0){
                  std::cout << _primary.getJacobian().row(i).cwiseAbs().maxCoeff() << "\t";
                  //std::cout << _secondary.getTask().getReference()[i] << std::endl;
                //}
//                std::cout << _primary.getJacobian().row(i).cwiseAbs().transpose() << std::endl;
 }
 std::cout << std::endl;
// std::cout << _current_id.transpose() << std::endl;
}

void mwoibn::hierarchical_control::actions::Merge::release(){

}
//
// void mwoibn::hierarchical_control::actions::Merge::_add(){
//         // _checkStack();
// }


#ifndef BOOST_UTILITY_DETAIL_MINSTD_RAND_HPP_INCLUDED
#define BOOST_UTILITY_DETAIL_MINSTD_RAND_HPP_INCLUDED

// Copyright 2017 Peter Dimov
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// An implementation of minstd_rand that does not require
// the Random library

#include <boost/cstdint.hpp>

namespace boost
{
namespace detail
{

class minstd_rand
{
private:

    boost::uint_least32_t x_;

    enum { a = 48271, m = 2147483647 };

public:

    minstd_rand(): x_( 1 )
    {
    }

    explicit minstd_rand( boost::uint_least32_t x ): x_( x % m )
    {
        if( x_ == 0 )
        {
            x_ = 1;
        }
    }

    boost::uint_least32_t operator()()
    {
        boost::uint_least64_t y = x_;

        y = ( a * y ) % m;

        x_ = static_cast<boost::uint_least32_t>( y );

        return x_;
    }
};

} // namespace detail
} // namespace boost

#endif // #ifndef BOOST_UTILITY_DETAIL_MINSTD_RAND_HPP_INCLUDED