/* -*- c++ -*- */
/*
 * @file
 * @author Piotr Krysik <ptrkrysik@gmail.com>
 * @section LICENSE
 *
 * Gr-gsm is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * Gr-gsm is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with gr-gsm; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include <gnuradio/math.h>
#include <math.h>
#include <boost/circular_buffer.hpp>
#include <algorithm>
#include <numeric>
#include <vector>
#include <viterbi_detector.h>
#include <string.h>
#include <iostream>
#include <iomanip>
#include <boost/scoped_ptr.hpp>

#include <sch.h>
#include "receiver_impl.h"
#include <grgsm/endian.h>

//files included for debuging
//#include "plotting/plotting.hpp"
//#include <pthread.h>

#define SYNC_SEARCH_RANGE 30

namespace gr
{
namespace gsm
{
receiver::sptr
receiver::make(int osr, const std::vector<int> &cell_allocation, const std::vector<int> &tseq_nums)
{
    return gnuradio::get_initial_sptr
           (new receiver_impl(osr, cell_allocation, tseq_nums));
}

/*
 * The private constructor
 */
receiver_impl::receiver_impl(int osr, const std::vector<int> &cell_allocation, const std::vector<int> &tseq_nums)
    : gr::sync_block("receiver",
                gr::io_signature::make(1, -1, sizeof(gr_complex)),
                gr::io_signature::make(0, 0, 0)),
    d_OSR(osr),
    d_chan_imp_length(CHAN_IMP_RESP_LENGTH),
    d_counter(0),
    d_fcch_start_pos(0),
    d_freq_offset_setting(0),
    d_state(fcch_search),
    d_burst_nr(osr),
    d_failed_sch(0),
    d_signal_dbm(-120),
    d_tseq_nums(tseq_nums),
    d_cell_allocation(cell_allocation),
    d_last_time(0.0)
{
    int i;
                                                                      //don't send samples to the receiver until there are at least samples for one
    set_output_multiple(floor((TS_BITS + 2 * GUARD_PERIOD) * d_OSR)); // burst and two gurad periods (one gurard period is an arbitrary overlap)
    gmsk_mapper(SYNC_BITS, N_SYNC_BITS, d_sch_training_seq, gr_complex(0.0, -1.0));
    for (i = 0; i < TRAIN_SEQ_NUM; i++)
    {
        gr_complex startpoint = (train_seq[i][0]==0) ? gr_complex(1.0, 0.0) : gr_complex(-1.0, 0.0); //if first bit of the seqeunce ==0  first symbol ==1
                                                                                //if first bit of the seqeunce ==1  first symbol ==-1
        gmsk_mapper(train_seq[i], N_TRAIN_BITS, d_norm_training_seq[i], startpoint);
    }
    message_port_register_out(pmt::mp("C0"));
    message_port_register_out(pmt::mp("CX"));
    message_port_register_out(pmt::mp("measurements"));
    configure_receiver();  //configure the receiver - tell it where to find which burst type
}

/*
 * Our virtual destructor.
 */
receiver_impl::~receiver_impl()
{
}

int
receiver_impl::work(int noutput_items,
	               gr_vector_const_void_star &input_items,
	               gr_vector_void_star &output_items)
{
//    std::vector<const gr_complex *> iii = (std::vector<const gr_complex *>) input_items; // jak zrobić to rzutowanie poprawnie
    gr_complex * input = (gr_complex *) input_items[0];
    std::vector<tag_t> freq_offset_tags;
    uint64_t start = nitems_read(0);
    uint64_t stop = start + noutput_items;

    float current_time = static_cast<float>(start)/(GSM_SYMBOL_RATE*d_OSR);
    if((current_time - d_last_time) > 0.1)
    {
        pmt::pmt_t msg = pmt::make_tuple(pmt::mp("current_time"),pmt::from_double(current_time));
        message_port_pub(pmt::mp("measurements"), msg);
        d_last_time = current_time;
    }

    pmt::pmt_t key = pmt::string_to_symbol("setting_freq_offset");
    get_tags_in_range(freq_offset_tags, 0, start, stop, key);
    bool freq_offset_tag_in_fcch = false;
    uint64_t tag_offset=-1; //-1 - just some clearly invalid value

    if(!freq_offset_tags.empty()){
        tag_t freq_offset_tag = freq_offset_tags[0];
        tag_offset = freq_offset_tag.offset - start;

        burst_type b_type = d_channel_conf.get_burst_type(d_burst_nr);
        if(d_state == synchronized && b_type == fcch_burst){
            uint64_t last_sample_nr = ceil((GUARD_PERIOD + 2.0 * TAIL_BITS + 156.25) * d_OSR) + 1;
            if(tag_offset < last_sample_nr){
                freq_offset_tag_in_fcch = true;
            }
            d_freq_offset_setting = pmt::to_double(freq_offset_tag.value);
        } else {
            d_freq_offset_setting = pmt::to_double(freq_offset_tag.value);
        }
    }

    switch (d_state)
    {
        //bootstrapping
    case fcch_search:
    {
        double freq_offset_tmp;
        if (find_fcch_burst(input, noutput_items,freq_offset_tmp))
        {
            pmt::pmt_t msg = pmt::make_tuple(pmt::mp("freq_offset"),pmt::from_double(freq_offset_tmp-d_freq_offset_setting),pmt::mp("fcch_search"));
            message_port_pub(pmt::mp("measurements"), msg);

            d_state = sch_search;
        }
        else
        {
            d_state = fcch_search;
        }
        break;
    }

    case sch_search:
    {
        std::vector<gr_complex> channel_imp_resp(CHAN_IMP_RESP_LENGTH*d_OSR);
        int t1, t2, t3;
        int burst_start = 0;
        unsigned char output_binary[BURST_SIZE];

        if (reach_sch_burst(noutput_items))                                //wait for a SCH burst
        {
            burst_start = get_sch_chan_imp_resp(input, &channel_imp_resp[0]); //get channel impulse response from it
            detect_burst(input, &channel_imp_resp[0], burst_start, output_binary); //detect bits using MLSE detection
            if (decode_sch(&output_binary[3], &t1, &t2, &t3, &d_ncc, &d_bcc) == 0)   //decode SCH burst
            {
                d_burst_nr.set(t1, t2, t3, 0);                                  //set counter of bursts value
                d_burst_nr++;

                consume_each(burst_start + BURST_SIZE * d_OSR + 4*d_OSR);   //consume samples up to next guard period
                d_state = synchronized;
            }
            else
            {
                d_state = fcch_search;                       //if there is error in the sch burst go back to fcch search phase
            }
        }
        else
        {
            d_state = sch_search;
        }
        break;
    }
    //in this state receiver is synchronized and it processes bursts according to burst type for given burst number
    case synchronized:
    {
        std::vector<gr_complex> channel_imp_resp(CHAN_IMP_RESP_LENGTH*d_OSR);
        int offset = 0;
        int to_consume = 0;
        unsigned char output_binary[BURST_SIZE];

        burst_type b_type;

        for(int input_nr=0; input_nr<d_cell_allocation.size(); input_nr++)
        {
            double signal_pwr = 0;
            input = (gr_complex *)input_items[input_nr];

            for(int ii=GUARD_PERIOD;ii<TS_BITS;ii++)
            {
                signal_pwr += abs(input[ii])*abs(input[ii]);
            }
            signal_pwr = signal_pwr/(TS_BITS);
            d_signal_dbm = round(10*log10(signal_pwr/50));
            if(input_nr==0){
                d_c0_signal_dbm = d_signal_dbm;
            }

            if(input_nr==0) //for c0 channel burst type is controlled by channel configuration
            {
                b_type = d_channel_conf.get_burst_type(d_burst_nr); //get burst type for given burst number
            }
            else
            {
                b_type = normal_or_noise; //for the rest it can be only normal burst or noise (at least at this moment of development)
            }

            switch (b_type)
            {
            case fcch_burst:                                                                      //if it's FCCH  burst
            {
                const unsigned first_sample = ceil((GUARD_PERIOD + 2 * TAIL_BITS) * d_OSR) + 1;
                const unsigned last_sample = first_sample + USEFUL_BITS * d_OSR - TAIL_BITS * d_OSR;
                double freq_offset_tmp = compute_freq_offset(input, first_sample, last_sample);       //extract frequency offset from it

                send_burst(d_burst_nr, fc_fb, GSMTAP_BURST_FCCH, input_nr);

                pmt::pmt_t msg = pmt::make_tuple(pmt::mp("freq_offset"),pmt::from_double(freq_offset_tmp-d_freq_offset_setting),pmt::mp("synchronized"));
                message_port_pub(pmt::mp("measurements"), msg);
                break;
            }
            case sch_burst:                                                                      //if it's SCH burst
            {
                int t1, t2, t3, d_ncc, d_bcc;
                d_c0_burst_start = get_sch_chan_imp_resp(input, &channel_imp_resp[0]);                //get channel impulse response

                detect_burst(input, &channel_imp_resp[0], d_c0_burst_start, output_binary);           //MLSE detection of bits
                send_burst(d_burst_nr, output_binary, GSMTAP_BURST_SCH, input_nr);
                if (decode_sch(&output_binary[3], &t1, &t2, &t3, &d_ncc, &d_bcc) == 0)           //and decode SCH data
                {
                    // d_burst_nr.set(t1, t2, t3, 0);                                              //but only to check if burst_start value is correct
                    d_failed_sch = 0;
                    offset =  d_c0_burst_start - floor((GUARD_PERIOD) * d_OSR);                         //compute offset from burst_start - burst should start after a guard period
                    to_consume += offset;                                                          //adjust with offset number of samples to be consumed
                }
                else
                {
                    d_failed_sch++;
                    if (d_failed_sch >= MAX_SCH_ERRORS)
                    {
                        d_state = fcch_search;
                        pmt::pmt_t msg = pmt::make_tuple(pmt::mp("freq_offset"),pmt::from_double(0.0),pmt::mp("sync_loss"));
                        message_port_pub(pmt::mp("measurements"), msg);
                        //DCOUT("Re-Synchronization!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
                    }
                }
                break;
            }
            case normal_burst:
            {
                float normal_corr_max;                                                    //if it's normal burst
                d_c0_burst_start = get_norm_chan_imp_resp(input, &channel_imp_resp[0], &normal_corr_max, d_bcc); //get channel impulse response for given training sequence number - d_bcc
                detect_burst(input, &channel_imp_resp[0], d_c0_burst_start, output_binary);            //MLSE detection of bits
                send_burst(d_burst_nr, output_binary, GSMTAP_BURST_NORMAL, input_nr);
                break;
            }
            case dummy_or_normal:
            {
                unsigned int normal_burst_start, dummy_burst_start;
                float dummy_corr_max, normal_corr_max;

                dummy_burst_start = get_norm_chan_imp_resp(input, &channel_imp_resp[0], &dummy_corr_max, TS_DUMMY);
                normal_burst_start = get_norm_chan_imp_resp(input, &channel_imp_resp[0], &normal_corr_max, d_bcc);

                if (normal_corr_max > dummy_corr_max)
                {
                    d_c0_burst_start = normal_burst_start;
                    detect_burst(input, &channel_imp_resp[0], normal_burst_start, output_binary);
                    send_burst(d_burst_nr, output_binary, GSMTAP_BURST_NORMAL, input_nr);
                }
                else
                {
                    d_c0_burst_start = dummy_burst_start;
                    send_burst(d_burst_nr, dummy_burst, GSMTAP_BURST_DUMMY, input_nr);
                }
                break;
            }
            case rach_burst:
                break;
            case dummy:
                send_burst(d_burst_nr, dummy_burst, GSMTAP_BURST_DUMMY, input_nr);
                break;
            case normal_or_noise:
            {
                unsigned int burst_start;
                float normal_corr_max_tmp;
                float normal_corr_max=-1e6;
                int max_tn;
                std::vector<gr_complex> v(input, input + noutput_items);
                if(d_signal_dbm>=d_c0_signal_dbm-13)
                {
                    if(d_tseq_nums.size()==0)              //there is no information about training sequence
                    {                                      //however the receiver can detect it
                        get_norm_chan_imp_resp(input, &channel_imp_resp[0], &normal_corr_max, 0);
                        float ts_max=normal_corr_max;     //with use of a very simple algorithm based on finding
                        int ts_max_num=0;                 //maximum correlation
                        for(int ss=1; ss<=7; ss++)
                        {
                            get_norm_chan_imp_resp(input, &channel_imp_resp[0], &normal_corr_max, ss);
                            if(ts_max<normal_corr_max)
                            {
                                ts_max = normal_corr_max;
                                ts_max_num = ss;
                            }
                        }
                        d_tseq_nums.push_back(ts_max_num);
                    }
                    int tseq_num;
                    if(input_nr<=d_tseq_nums.size()){
                        tseq_num = d_tseq_nums[input_nr-1];
                    } else {
                        tseq_num = d_tseq_nums.back();
                    }
                    burst_start = get_norm_chan_imp_resp(input, &channel_imp_resp[0], &normal_corr_max, tseq_num);
//                  if(abs(d_c0_burst_start-burst_start)<=2){ //unused check/filter based on timing
                    if((normal_corr_max/sqrt(signal_pwr))>=0.9){
                        detect_burst(input, &channel_imp_resp[0], burst_start, output_binary);
                        send_burst(d_burst_nr, output_binary, GSMTAP_BURST_NORMAL, input_nr);
                    }
                }
                break;
            }
            case empty:   //if it's empty burst
                break;      //do nothing
            }

            if(input_nr==input_items.size()-1)
            {
                d_burst_nr++;   //go to next burst
                to_consume += TS_BITS * d_OSR + d_burst_nr.get_offset();  //consume samples of the burst up to next guard period
                consume_each(to_consume);
            }
            //and add offset which is introduced by
            //0.25 fractional part of a guard period
        }
    }
    break;
    }
    return 0;
}

bool receiver_impl::find_fcch_burst(const gr_complex *input, const int nitems, double & computed_freq_offset)
{
    boost::circular_buffer<float> phase_diff_buffer(FCCH_HITS_NEEDED * d_OSR); //circular buffer used to scan throug signal to find
    //best match for FCCH burst
    float phase_diff = 0;
    gr_complex conjprod;
    int start_pos = -1;
    int hit_count = 0;
    int miss_count = 0;
    float min_phase_diff;
    float max_phase_diff;
    double best_sum = 0;
    float lowest_max_min_diff = 99999;

    int to_consume = 0;
    int sample_number = 0;
    bool end = false;
    bool result = false;
    boost::circular_buffer<float>::iterator buffer_iter;

    /**@name Possible states of FCCH search algorithm*/
    //@{
    enum states
    {
        init,               ///< initialize variables
        search,             ///< search for positive samples
        found_something,    ///< search for FCCH and the best position of it
        fcch_found,         ///< when FCCH was found
        search_fail         ///< when there is no FCCH in the input vector
    } fcch_search_state;
    //@}

    fcch_search_state = init;

    while (!end)
    {
        switch (fcch_search_state)
        {

        case init: //initialize variables
            hit_count = 0;
            miss_count = 0;
            start_pos = -1;
            lowest_max_min_diff = 99999;
            phase_diff_buffer.clear();
            fcch_search_state = search;

            break;

        case search:                                                // search for positive samples
            sample_number++;

            if (sample_number > nitems - FCCH_HITS_NEEDED * d_OSR)   //if it isn't possible to find FCCH because
            {
                                                                       //there's too few samples left to look into,
                to_consume = sample_number;                            //don't do anything with those samples which are left
                                                                       //and consume only those which were checked
                fcch_search_state = search_fail;
            }
            else
            {
                phase_diff = compute_phase_diff(input[sample_number], input[sample_number-1]);

                if (phase_diff > 0)                                   //if a positive phase difference was found
                {
                    to_consume = sample_number;
                    fcch_search_state = found_something;                //switch to state in which searches for FCCH
                }
                else
                {
                    fcch_search_state = search;
                }
            }

            break;

        case found_something:  // search for FCCH and the best position of it
        {
            if (phase_diff > 0)
            {
                hit_count++;       //positive phase differencies increases hits_count
            }
            else
            {
                miss_count++;      //negative increases miss_count
            }

            if ((miss_count >= FCCH_MAX_MISSES * d_OSR) && (hit_count <= FCCH_HITS_NEEDED * d_OSR))
            {
                //if miss_count exceeds limit before hit_count
                fcch_search_state = init;       //go to init
                continue;
            }
            else if (((miss_count >= FCCH_MAX_MISSES * d_OSR) && (hit_count > FCCH_HITS_NEEDED * d_OSR)) || (hit_count > 2 * FCCH_HITS_NEEDED * d_OSR))
            {
                //if hit_count and miss_count exceeds limit then FCCH was found
                fcch_search_state = fcch_found;
                continue;
            }
            else if ((miss_count < FCCH_MAX_MISSES * d_OSR) && (hit_count > FCCH_HITS_NEEDED * d_OSR))
            {
                //find difference between minimal and maximal element in the buffer
                //for FCCH this value should be low
                //this part is searching for a region where this value is lowest
                min_phase_diff = * (min_element(phase_diff_buffer.begin(), phase_diff_buffer.end()));
                max_phase_diff = * (max_element(phase_diff_buffer.begin(), phase_diff_buffer.end()));

                if (lowest_max_min_diff > max_phase_diff - min_phase_diff)
                {
                    lowest_max_min_diff = max_phase_diff - min_phase_diff;
                    start_pos = sample_number - FCCH_HITS_NEEDED * d_OSR - FCCH_MAX_MISSES * d_OSR; //store start pos
                    best_sum = 0;

                    for (buffer_iter = phase_diff_buffer.begin();
                            buffer_iter != (phase_diff_buffer.end());
                            buffer_iter++)
                    {
                        best_sum += *buffer_iter - (M_PI / 2) / d_OSR;   //store best value of phase offset sum
                    }
                }
            }

            sample_number++;

            if (sample_number >= nitems)      //if there's no single sample left to check
            {
                fcch_search_state = search_fail;//FCCH search failed
                continue;
            }

            phase_diff = compute_phase_diff(input[sample_number], input[sample_number-1]);
            phase_diff_buffer.push_back(phase_diff);
            fcch_search_state = found_something;
        }
        break;

        case fcch_found:
        {
            to_consume = start_pos + FCCH_HITS_NEEDED * d_OSR + 1; //consume one FCCH burst

            d_fcch_start_pos = d_counter + start_pos;

            //compute frequency offset
            double phase_offset = best_sum / FCCH_HITS_NEEDED;
            double freq_offset = phase_offset * 1625000.0/6 / (2 * M_PI); //1625000.0/6 - GMSK symbol rate in GSM
            computed_freq_offset = freq_offset;

            end = true;
            result = true;
            break;
        }

        case search_fail:
            end = true;
            result = false;
            break;
        }
    }

    d_counter += to_consume;
    consume_each(to_consume);

    return result;
}

double receiver_impl::compute_freq_offset(const gr_complex * input, unsigned first_sample, unsigned last_sample)
{
    double phase_sum = 0;
    unsigned ii;

    for (ii = first_sample; ii < last_sample; ii++)
    {
        double phase_diff = compute_phase_diff(input[ii], input[ii-1]) - (M_PI / 2) / d_OSR;
        phase_sum += phase_diff;
    }

    double phase_offset = phase_sum / (last_sample - first_sample);
    double freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);
    return freq_offset;
}

inline float receiver_impl::compute_phase_diff(gr_complex val1, gr_complex val2)
{
    gr_complex conjprod = val1 * conj(val2);
    return fast_atan2f(imag(conjprod), real(conjprod));
}

bool receiver_impl::reach_sch_burst(const int nitems)
{
    //it just consumes samples to get near to a SCH burst
    int to_consume = 0;
    bool result = false;
    unsigned sample_nr_near_sch_start = d_fcch_start_pos + (FRAME_BITS - SAFETY_MARGIN) * d_OSR;

    //consume samples until d_counter will be equal to sample_nr_near_sch_start
    if (d_counter < sample_nr_near_sch_start)
    {
        if (d_counter + nitems >= sample_nr_near_sch_start)
        {
            to_consume = sample_nr_near_sch_start - d_counter;
        }
        else
        {
            to_consume = nitems;
        }
        result = false;
    }
    else
    {
        to_consume = 0;
        result = true;
    }

    d_counter += to_consume;
    consume_each(to_consume);
    return result;
}

int receiver_impl::get_sch_chan_imp_resp(const gr_complex *input, gr_complex * chan_imp_resp)
{
    std::vector<gr_complex> correlation_buffer;
    std::vector<float> power_buffer;
    std::vector<float> window_energy_buffer;

    int strongest_window_nr;
    int burst_start = 0;
    int chan_imp_resp_center = 0;
    float max_correlation = 0;
    float energy = 0;

    for (int ii = SYNC_POS * d_OSR; ii < (SYNC_POS + SYNC_SEARCH_RANGE) *d_OSR; ii++)
    {
        gr_complex correlation = correlate_sequence(&d_sch_training_seq[5], N_SYNC_BITS - 10, &input[ii]);
        correlation_buffer.push_back(correlation);
        power_buffer.push_back(std::pow(abs(correlation), 2));
    }
    //compute window energies
    std::vector<float>::iterator iter = power_buffer.begin();
    bool loop_end = false;
    while (iter != power_buffer.end())
    {
        std::vector<float>::iterator iter_ii = iter;
        energy = 0;

        for (int ii = 0; ii < (d_chan_imp_length) *d_OSR; ii++, iter_ii++)
        {
            if (iter_ii == power_buffer.end())
            {
                loop_end = true;
                break;
            }
            energy += (*iter_ii);
        }
        if (loop_end)
        {
            break;
        }
        iter++;
        window_energy_buffer.push_back(energy);
    }

    strongest_window_nr = max_element(window_energy_buffer.begin(), window_energy_buffer.end()) - window_energy_buffer.begin();
    //   d_channel_imp_resp.clear();

    max_correlation = 0;
    for (int ii = 0; ii < (d_chan_imp_length) *d_OSR; ii++)
    {
        gr_complex correlation = correlation_buffer[strongest_window_nr + ii];
        if (abs(correlation) > max_correlation)
        {
            chan_imp_resp_center = ii;
            max_correlation = abs(correlation);
        }
        //     d_channel_imp_resp.push_back(correlation);
        chan_imp_resp[ii] = correlation;
    }

    burst_start = strongest_window_nr + chan_imp_resp_center - 48 * d_OSR - 2 * d_OSR + 2 + SYNC_POS * d_OSR;
    return burst_start;
}

void receiver_impl::detect_burst(const gr_complex * input, gr_complex * chan_imp_resp, int burst_start, unsigned char * output_binary)
{
    float output[BURST_SIZE];
    std::vector<gr_complex> rhh_temp(CHAN_IMP_RESP_LENGTH*d_OSR);
    gr_complex rhh[CHAN_IMP_RESP_LENGTH];
    gr_complex filtered_burst[BURST_SIZE];
    int start_state = 3;
    unsigned int stop_states[2] = {4, 12};

    autocorrelation(chan_imp_resp, &rhh_temp[0], d_chan_imp_length*d_OSR);
    for (int ii = 0; ii < (d_chan_imp_length); ii++)
    {
        rhh[ii] = conj(rhh_temp[ii*d_OSR]);
    }

    mafi(&input[burst_start], BURST_SIZE, chan_imp_resp, d_chan_imp_length*d_OSR, filtered_burst);

    viterbi_detector(filtered_burst, BURST_SIZE, rhh, start_state, stop_states, 2, output);

    for (int i = 0; i < BURST_SIZE ; i++)
    {
        output_binary[i] = (output[i] > 0);
    }
}

void receiver_impl::gmsk_mapper(const unsigned char * input, int nitems, gr_complex * gmsk_output, gr_complex start_point)
{
    gr_complex j = gr_complex(0.0, 1.0);

    int current_symbol;
    int encoded_symbol;
    int previous_symbol = 2 * input[0] - 1;
    gmsk_output[0] = start_point;

    for (int i = 1; i < nitems; i++)
    {
        //change bits representation to NRZ
        current_symbol = 2 * input[i] - 1;
        //differentially encode
        encoded_symbol = current_symbol * previous_symbol;
        //and do gmsk mapping
        gmsk_output[i] = j * gr_complex(encoded_symbol, 0.0) * gmsk_output[i-1];
        previous_symbol = current_symbol;
    }
}

gr_complex receiver_impl::correlate_sequence(const gr_complex * sequence, int length, const gr_complex * input)
{
    gr_complex result(0.0, 0.0);
    int sample_number = 0;

    for (int ii = 0; ii < length; ii++)
    {
        sample_number = (ii * d_OSR) ;
        result += sequence[ii] * conj(input[sample_number]);
    }

    result = result / gr_complex(length, 0);
    return result;
}

//computes autocorrelation for positive arguments
inline void receiver_impl::autocorrelation(const gr_complex * input, gr_complex * out, int nitems)
{
    int i, k;
    for (k = nitems - 1; k >= 0; k--)
    {
        out[k] = gr_complex(0, 0);
        for (i = k; i < nitems; i++)
        {
            out[k] += input[i] * conj(input[i-k]);
        }
    }
}

inline void receiver_impl::mafi(const gr_complex * input, int nitems, gr_complex * filter, int filter_length, gr_complex * output)
{
    int ii = 0, n, a;

    for (n = 0; n < nitems; n++)
    {
        a = n * d_OSR;
        output[n] = 0;
        ii = 0;

        while (ii < filter_length)
        {
            if ((a + ii) >= nitems*d_OSR){
                break;
            }
            output[n] += input[a+ii] * filter[ii];
            ii++;
        }
    }
}

//especially computations of strongest_window_nr
int receiver_impl::get_norm_chan_imp_resp(const gr_complex *input, gr_complex * chan_imp_resp, float *corr_max, int bcc)
{
    std::vector<gr_complex> correlation_buffer;
    std::vector<float> power_buffer;
    std::vector<float> window_energy_buffer;

    int strongest_window_nr;
    int burst_start = 0;
    int chan_imp_resp_center = 0;
    float max_correlation = 0;
    float energy = 0;

    int search_center = (int)((TRAIN_POS + GUARD_PERIOD) * d_OSR);
    int search_start_pos = search_center + 1 - 5*d_OSR;
    //   int search_start_pos = search_center -  d_chan_imp_length * d_OSR;
    int search_stop_pos = search_center + d_chan_imp_length * d_OSR + 5 * d_OSR;

    for(int ii = search_start_pos; ii < search_stop_pos; ii++)
    {
        gr_complex correlation = correlate_sequence(&d_norm_training_seq[bcc][TRAIN_BEGINNING], N_TRAIN_BITS - 10, &input[ii]);
        correlation_buffer.push_back(correlation);
        power_buffer.push_back(std::pow(abs(correlation), 2));
    }
//    plot(power_buffer);
    //compute window energies
    std::vector<float>::iterator iter = power_buffer.begin();
    bool loop_end = false;
    while (iter != power_buffer.end())
    {
        std::vector<float>::iterator iter_ii = iter;
        energy = 0;

        for (int ii = 0; ii < (d_chan_imp_length)*d_OSR; ii++, iter_ii++)
        {
            if (iter_ii == power_buffer.end())
            {
                loop_end = true;
                break;
            }
            energy += (*iter_ii);
        }
        if (loop_end)
        {
            break;
        }
        iter++;

        window_energy_buffer.push_back(energy);
    }

    strongest_window_nr = max_element(window_energy_buffer.begin(), window_energy_buffer.end()-((d_chan_imp_length)*d_OSR)) - window_energy_buffer.begin();
    //strongest_window_nr = strongest_window_nr-d_OSR;
    if(strongest_window_nr<0){
       strongest_window_nr = 0;
    }

    max_correlation = 0;
    for (int ii = 0; ii < (d_chan_imp_length)*d_OSR; ii++)
    {
        gr_complex correlation = correlation_buffer[strongest_window_nr + ii];
        if (abs(correlation) > max_correlation)
        {
            chan_imp_resp_center = ii;
            max_correlation = abs(correlation);
        }
        //     d_channel_imp_resp.push_back(correlation);
        chan_imp_resp[ii] = correlation;
    }

    *corr_max = max_correlation;

    //DCOUT("strongest_window_nr_new: " << strongest_window_nr);
    burst_start = search_start_pos + strongest_window_nr - TRAIN_POS * d_OSR; //compute first sample posiiton which corresponds to the first sample of the impulse response

    //DCOUT("burst_start: " << burst_start);
    return burst_start;
}

void receiver_impl::send_burst(burst_counter burst_nr, const unsigned char * burst_binary, uint8_t burst_type, unsigned int input_nr)
{
    boost::scoped_ptr<gsmtap_hdr> tap_header(new gsmtap_hdr());

    tap_header->version = GSMTAP_VERSION;
    tap_header->hdr_len = sizeof(gsmtap_hdr)/4;
    tap_header->type = GSMTAP_TYPE_UM_BURST;
    tap_header->timeslot = static_cast<uint8_t>(d_burst_nr.get_timeslot_nr());
    tap_header->frame_number = htobe32(d_burst_nr.get_frame_nr());
    tap_header->sub_type = burst_type;
    tap_header->arfcn = htobe16(d_cell_allocation[input_nr]) ;
    tap_header->signal_dbm = static_cast<int8_t>(d_signal_dbm);
    tap_header->snr_db = 0;

    int8_t header_plus_burst[sizeof(gsmtap_hdr)+BURST_SIZE];
    memcpy(header_plus_burst, tap_header.get(), sizeof(gsmtap_hdr));
    memcpy(header_plus_burst+sizeof(gsmtap_hdr), burst_binary, BURST_SIZE);

    pmt::pmt_t blob_header_plus_burst = pmt::make_blob(header_plus_burst,sizeof(gsmtap_hdr)+BURST_SIZE);
    pmt::pmt_t msg = pmt::cons(pmt::PMT_NIL, blob_header_plus_burst);

    if(input_nr==0){
        message_port_pub(pmt::mp("C0"), msg);
    } else {
        message_port_pub(pmt::mp("CX"), msg);
    }
}

void receiver_impl::configure_receiver()
{
    d_channel_conf.set_multiframe_type(TIMESLOT0, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT0, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);

    d_channel_conf.set_burst_types(TIMESLOT0, TEST_CCH_FRAMES, sizeof(TEST_CCH_FRAMES) / sizeof(unsigned), dummy_or_normal);
    d_channel_conf.set_burst_types(TIMESLOT0, FCCH_FRAMES, sizeof(FCCH_FRAMES) / sizeof(unsigned), fcch_burst);
    d_channel_conf.set_burst_types(TIMESLOT0, SCH_FRAMES, sizeof(SCH_FRAMES) / sizeof(unsigned), sch_burst);

    d_channel_conf.set_multiframe_type(TIMESLOT1, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT1, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);
    d_channel_conf.set_multiframe_type(TIMESLOT2, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT2, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);
    d_channel_conf.set_multiframe_type(TIMESLOT3, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT3, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);
    d_channel_conf.set_multiframe_type(TIMESLOT4, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT4, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);
    d_channel_conf.set_multiframe_type(TIMESLOT5, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT5, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);
    d_channel_conf.set_multiframe_type(TIMESLOT6, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT6, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);
    d_channel_conf.set_multiframe_type(TIMESLOT7, multiframe_51);
    d_channel_conf.set_burst_types(TIMESLOT7, TEST51, sizeof(TEST51) / sizeof(unsigned), dummy_or_normal);
}

void receiver_impl::set_cell_allocation(const std::vector<int> &cell_allocation)
{
    d_cell_allocation = cell_allocation;
}

void receiver_impl::set_tseq_nums(const std::vector<int> & tseq_nums)
{
    d_tseq_nums = tseq_nums;
}

void receiver_impl::reset()
{
    d_state = fcch_search;
}

} /* namespace gsm */
} /* namespace gr */


/**
 * @file environment.hpp
 * @author Licheng Wen (wenlc@zju.edu.cn)
 * @brief Environment class header
 * @date 2020-11-12
 *
 * @copyright Copyright (c) 2020
 *
 */

#pragma once
#include <ompl/base/State.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>

#include <boost/functional/hash.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/heap/fibonacci_heap.hpp>
#include <unordered_map>
#include <unordered_set>

#include "neighbor.hpp"
#include "planresult.hpp"

namespace Constants {
// [m] --- The minimum turning radius of the vehicle
static float r = 3;
static float deltat = 6.75 * 6 / 180.0 * M_PI;
// [#] --- A movement cost penalty for turning (choosing non straight motion
// primitives)
static float penaltyTurning = 1.5;
// [#] --- A movement cost penalty for reversing (choosing motion primitives >
// 2)
static float penaltyReversing = 2.0;
// [#] --- A movement cost penalty for change of direction (changing from
// primitives < 3 to primitives > 2)
static float penaltyCOD = 2.0;
// map resolution
static float mapResolution = 2.0;
// change to set calcIndex resolution
static float xyResolution = r * deltat;
static float yawResolution = deltat;

// width of car
static float carWidth = 2.0;
// distance from rear to vehicle front end
static float LF = 2.0;
// distance from rear to vehicle back end
static float LB = 1.0;
// obstacle default radius
static float obsRadius = 1;
// least time to wait for constraint
static int constraintWaitTime = 2;

// R = 3, 6.75 DEG
std::vector<double> dyaw = {0, deltat, -deltat, 0, -deltat, deltat};
std::vector<double> dx = {r * deltat, r *sin(deltat),  r *sin(deltat),
                          -r *deltat, -r *sin(deltat), -r *sin(deltat)};
std::vector<double> dy = {0, -r *(1 - cos(deltat)), r *(1 - cos(deltat)),
                          0, -r *(1 - cos(deltat)), r *(1 - cos(deltat))};

static inline float normalizeHeadingRad(float t) {
  if (t < 0) {
    t = t - 2.f * M_PI * (int)(t / (2.f * M_PI));
    return 2.f * M_PI + t;
  }

  return t - 2.f * M_PI * (int)(t / (2.f * M_PI));
}
}  // namespace Constants

namespace libMultiRobotPlanning {

using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;
using namespace libMultiRobotPlanning;
typedef ompl::base::SE2StateSpace::StateType OmplState;
typedef boost::geometry::model::d2::point_xy<double> Point;
typedef boost::geometry::model::segment<Point> Segment;
/**
 * @brief  Environment class
 *
 * @tparam Location
 * @tparam State
 * @tparam Action
 * @tparam Cost
 * @tparam Conflict
 * @tparam Constraint
 * @tparam Constraints
 */
template <typename Location, typename State, typename Action, typename Cost,
          typename Conflict, typename Constraint, typename Constraints>
class Environment {
 public:
  Environment(size_t maxx, size_t maxy, std::unordered_set<Location> obstacles,
              std::multimap<int, State> dynamic_obstacles,
              std::vector<State> goals)
      : m_obstacles(std::move(obstacles)),
        m_dynamic_obstacles(std::move(dynamic_obstacles)),
        m_agentIdx(0),
        m_constraints(nullptr),
        m_lastGoalConstraint(-1),
        m_highLevelExpanded(0),
        m_lowLevelExpanded(0) {
    m_dimx = (int)maxx / Constants::mapResolution;
    m_dimy = (int)maxy / Constants::mapResolution;
    // std::cout << "env build " << m_dimx << " " << m_dimy << " "
    //           << m_obstacles.size() << std::endl;
    holonomic_cost_maps = std::vector<std::vector<std::vector<double>>>(
        goals.size(), std::vector<std::vector<double>>(
                          m_dimx, std::vector<double>(m_dimy, 0)));
    m_goals.clear();
    for (const auto &g : goals) {
      if (g.x < 0 || g.x > maxx || g.y < 0 || g.y > maxy) {
        std::cout << "\033[1m\033[31m Goal out of boundary, Fail to build "
                     "environment \033[0m\n";
        return;
      }
      m_goals.emplace_back(
          State(g.x, g.y, Constants::normalizeHeadingRad(g.yaw)));
    }
    updateCostmap();
  }

  Environment(const Environment &) = delete;
  Environment &operator=(const Environment &) = delete;

  /// High Level Environment functions
  bool getFirstConflict(
      const std::vector<PlanResult<State, Action, double>> &solution,
      Conflict &result) {
    int max_t = 0;
    for (const auto &sol : solution) {
      max_t = std::max<int>(max_t, sol.states.size() - 1);
    }
    for (int t = 0; t < max_t; ++t) {
      // check drive-drive collisions
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1 = getState(i, solution, t);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2 = getState(j, solution, t);
          if (state1.agentCollision(state2)) {
            result.time = t;
            result.agent1 = i;
            result.agent2 = j;
            result.s1 = state1;
            result.s2 = state2;
            return true;
          }
        }
      }
    }
    return false;
  }

  void createConstraintsFromConflict(
      const Conflict &conflict, std::map<size_t, Constraints> &constraints) {
    Constraints c1;
    c1.constraints.emplace(
        Constraint(conflict.time, conflict.s2, conflict.agent2));
    constraints[conflict.agent1] = c1;
    Constraints c2;
    c2.constraints.emplace(
        Constraint(conflict.time, conflict.s1, conflict.agent1));
    constraints[conflict.agent2] = c2;
  }

  void onExpandHighLevelNode(int /*cost*/) {
    m_highLevelExpanded++;
    if (m_highLevelExpanded % 50 == 0)
      std::cout << "Now expand " << m_highLevelExpanded
                << " high level nodes.\n";
  }

  int highLevelExpanded() { return m_highLevelExpanded; }

  /// Low Level Environment functions
  void setLowLevelContext(size_t agentIdx, const Constraints *constraints) {
    assert(constraints);  // NOLINT
    m_agentIdx = agentIdx;
    m_constraints = constraints;
    m_lastGoalConstraint = -1;
    for (const auto &c : constraints->constraints) {
      if (m_goals[m_agentIdx].agentCollision(c.s)) {
        m_lastGoalConstraint = std::max(m_lastGoalConstraint, c.time);
      }
    }

    // std::cout << "Setting Lowlevel agent idx:" << agentIdx
    //           << " Constraints:" << constraints->constraints.size()
    //           << "  lastGoalConstraints:" << m_lastGoalConstraint <<
    //           std::endl;
  }

  int admissibleHeuristic(const State &s) {
    // non-holonomic-without-obstacles heuristic: use a Reeds-Shepp
    ompl::base::ReedsSheppStateSpace reedsSheppPath(Constants::r);
    OmplState *rsStart = (OmplState *)reedsSheppPath.allocState();
    OmplState *rsEnd = (OmplState *)reedsSheppPath.allocState();
    rsStart->setXY(s.x, s.y);
    rsStart->setYaw(s.yaw);
    rsEnd->setXY(m_goals[m_agentIdx].x, m_goals[m_agentIdx].y);
    rsEnd->setYaw(m_goals[m_agentIdx].yaw);
    double reedsSheppCost = reedsSheppPath.distance(rsStart, rsEnd);
    // std::cout << "ReedsShepps cost:" << reedsSheppCost << std::endl;
    // Euclidean distance
    double euclideanCost = sqrt(pow(m_goals[m_agentIdx].x - s.x, 2) +
                                pow(m_goals[m_agentIdx].y - s.y, 2));
    // std::cout << "Euclidean cost:" << euclideanCost << std::endl;
    // holonomic-with-obstacles heuristic
    double twoDoffset =
        sqrt(pow((s.x - (int)s.x) -
                     (m_goals[m_agentIdx].x - (int)m_goals[m_agentIdx].x),
                 2) +
             pow((s.y - (int)s.y) -
                     (m_goals[m_agentIdx].y - (int)m_goals[m_agentIdx].y),
                 2));
    double twoDCost =
        holonomic_cost_maps[m_agentIdx][(int)s.x / Constants::mapResolution]
                           [(int)s.y / Constants::mapResolution] -
        twoDoffset;
    // std::cout << "holonomic cost:" << twoDCost << std::endl;

    return std::max({reedsSheppCost, euclideanCost, twoDCost});
    return 0;
  }

  bool isSolution(
      const State &state, double gscore,
      std::unordered_map<State, std::tuple<State, Action, double, double>,
                         std::hash<State>> &_camefrom) {
    double goal_distance =
        sqrt(pow(state.x - getGoal().x, 2) + pow(state.y - getGoal().y, 2));
    if (goal_distance > 3 * (Constants::LB + Constants::LF)) return false;
    ompl::base::ReedsSheppStateSpace reedsSheppSpace(Constants::r);
    OmplState *rsStart = (OmplState *)reedsSheppSpace.allocState();
    OmplState *rsEnd = (OmplState *)reedsSheppSpace.allocState();
    rsStart->setXY(state.x, state.y);
    rsStart->setYaw(-state.yaw);
    rsEnd->setXY(getGoal().x, getGoal().y);
    rsEnd->setYaw(-getGoal().yaw);
    ompl::base::ReedsSheppStateSpace::ReedsSheppPath reedsShepppath =
        reedsSheppSpace.reedsShepp(rsStart, rsEnd);

    std::vector<State> path;
    std::unordered_map<State, std::tuple<State, Action, double, double>,
                       std::hash<State>>
        cameFrom;
    cameFrom.clear();
    path.emplace_back(state);
    for (auto pathidx = 0; pathidx < 5; pathidx++) {
      if (fabs(reedsShepppath.length_[pathidx]) < 1e-6) continue;
      double deltat = 0, dx = 0, act = 0, cost = 0;
      switch (reedsShepppath.type_[pathidx]) {
        case 0:  // RS_NOP
          continue;
          break;
        case 1:  // RS_LEFT
          deltat = -reedsShepppath.length_[pathidx];
          dx = Constants::r * sin(-deltat);
          // dy = Constants::r * (1 - cos(-deltat));
          act = 2;
          cost = reedsShepppath.length_[pathidx] * Constants::r *
                 Constants::penaltyTurning;
          break;
        case 2:  // RS_STRAIGHT
          deltat = 0;
          dx = reedsShepppath.length_[pathidx] * Constants::r;
          // dy = 0;
          act = 0;
          cost = dx;
          break;
        case 3:  // RS_RIGHT
          deltat = reedsShepppath.length_[pathidx];
          dx = Constants::r * sin(deltat);
          // dy = -Constants::r * (1 - cos(deltat));
          act = 1;
          cost = reedsShepppath.length_[pathidx] * Constants::r *
                 Constants::penaltyTurning;
          break;
        default:
          std::cout << "\033[1m\033[31m"
                    << "Warning: Receive unknown ReedsSheppPath type"
                    << "\033[0m\n";
          break;
      }
      if (cost < 0) {
        cost = -cost * Constants::penaltyReversing;
        act = act + 3;
      }
      State s = path.back();
      std::vector<std::pair<State, double>> next_path;
      if (generatePath(s, act, deltat, dx, next_path)) {
        for (auto iter = next_path.begin(); iter != next_path.end(); iter++) {
          State next_s = iter->first;
          gscore += iter->second;
          if (!(next_s == path.back())) {
            cameFrom.insert(std::make_pair<>(
                next_s,
                std::make_tuple<>(path.back(), act, iter->second, gscore)));
          }
          path.emplace_back(next_s);
        }
      } else {
        return false;
      }
    }

    if (path.back().time <= m_lastGoalConstraint) {
      return false;
    }

    m_goals[m_agentIdx] = path.back();

    _camefrom.insert(cameFrom.begin(), cameFrom.end());
    return true;
  }

  void getNeighbors(const State &s, Action action,
                    std::vector<Neighbor<State, Action, double>> &neighbors) {
    neighbors.clear();
    double g = Constants::dx[0];
    for (Action act = 0; act < 6; act++) {  // has 6 directions for Reeds-Shepp
      double xSucc, ySucc, yawSucc;
      g = Constants::dx[0];
      xSucc = s.x + Constants::dx[act] * cos(-s.yaw) -
              Constants::dy[act] * sin(-s.yaw);
      ySucc = s.y + Constants::dx[act] * sin(-s.yaw) +
              Constants::dy[act] * cos(-s.yaw);
      yawSucc = Constants::normalizeHeadingRad(s.yaw + Constants::dyaw[act]);
      // if (act != action) {  // penalize turning
      //   g = g * Constants::penaltyTurning;
      //   if (act >= 3)  // penalize change of direction
      //     g = g * Constants::penaltyCOD;
      // }
      // if (act > 3) {  // backwards
      //   g = g * Constants::penaltyReversing;
      // }
      if (act % 3 != 0) {  // penalize turning
        g = g * Constants::penaltyTurning;
      }
      if ((act < 3 && action >= 3) || (action < 3 && act >= 3)) {
        // penalize change of direction
        g = g * Constants::penaltyCOD;
      }
      if (act >= 3) {  // backwards
        g = g * Constants::penaltyReversing;
      }
      State tempState(xSucc, ySucc, yawSucc, s.time + 1);
      if (stateValid(tempState)) {
        neighbors.emplace_back(
            Neighbor<State, Action, double>(tempState, act, g));
      }
    }
    // wait
    g = Constants::dx[0];
    State tempState(s.x, s.y, s.yaw, s.time + 1);
    if (stateValid(tempState)) {
      neighbors.emplace_back(Neighbor<State, Action, double>(tempState, 6, g));
    }
  }
  State getGoal() { return m_goals[m_agentIdx]; }
  uint64_t calcIndex(const State &s) {
    return (uint64_t)s.time * (2 * M_PI / Constants::deltat) *
               (m_dimx / Constants::xyResolution) *
               (m_dimy / Constants::xyResolution) +
           (uint64_t)(Constants::normalizeHeadingRad(s.yaw) /
                      Constants::yawResolution) *
               (m_dimx / Constants::xyResolution) *
               (m_dimy / Constants::xyResolution) +
           (uint64_t)(s.y / Constants::xyResolution) *
               (m_dimx / Constants::xyResolution) +
           (uint64_t)(s.x / Constants::xyResolution);
  }

  void onExpandLowLevelNode(const State & /*s*/, int /*fScore*/,
                            int /*gScore*/) {
    m_lowLevelExpanded++;
  }

  int lowLevelExpanded() const { return m_lowLevelExpanded; }

  bool startAndGoalValid(const std::vector<State> &m_starts, const size_t iter,
                         const int batchsize) {
    assert(m_goals.size() == m_starts.size());
    for (size_t i = 0; i < m_goals.size(); i++)
      for (size_t j = i + 1; j < m_goals.size(); j++) {
        if (m_goals[i].agentCollision(m_goals[j])) {
          std::cout << "ERROR: Goal point of " << i + iter * batchsize << " & "
                    << j + iter * batchsize << " collide!\n";
          return false;
        }
        if (m_starts[i].agentCollision(m_starts[j])) {
          std::cout << "ERROR: Start point of " << i + iter * batchsize << " & "
                    << j + iter * batchsize << " collide!\n";
          return false;
        }
      }
    return true;
  }

 private:
  State getState(size_t agentIdx,
                 const std::vector<PlanResult<State, Action, double>> &solution,
                 size_t t) {
    assert(agentIdx < solution.size());
    if (t < solution[agentIdx].states.size()) {
      return solution[agentIdx].states[t].first;
    }
    assert(!solution[agentIdx].states.empty());
    return solution[agentIdx].states.back().first;
  }

  bool stateValid(const State &s) {
    double x_ind = s.x / Constants::mapResolution;
    double y_ind = s.y / Constants::mapResolution;
    if (x_ind < 0 || x_ind >= m_dimx || y_ind < 0 || y_ind >= m_dimy)
      return false;

    for (auto it = m_obstacles.begin(); it != m_obstacles.end(); it++) {
      if (s.obsCollision(*it)) return false;
    }

    auto it = m_dynamic_obstacles.equal_range(s.time);
    for (auto itr = it.first; itr != it.second; ++itr) {
      if (s.agentCollision(itr->second)) return false;
    }
    auto itlow = m_dynamic_obstacles.lower_bound(-s.time);
    auto itup = m_dynamic_obstacles.upper_bound(-1);
    for (auto it = itlow; it != itup; ++it)
      if (s.agentCollision(it->second)) return false;

    for (auto it = m_constraints->constraints.begin();
         it != m_constraints->constraints.end(); it++) {
      if (!it->satisfyConstraint(s)) return false;
    }

    return true;
  }

 private:
  struct compare_node {
    bool operator()(const std::pair<State, double> &n1,
                    const std::pair<State, double> &n2) const {
      return (n1.second > n2.second);
    }
  };
  void updateCostmap() {
    boost::heap::fibonacci_heap<std::pair<State, double>,
                                boost::heap::compare<compare_node>>
        heap;

    std::set<std::pair<int, int>> temp_obs_set;
    for (auto it = m_obstacles.begin(); it != m_obstacles.end(); it++) {
      temp_obs_set.insert(
          std::make_pair((int)it->x / Constants::mapResolution,
                         (int)it->y / Constants::mapResolution));
    }

    for (size_t idx = 0; idx < m_goals.size(); idx++) {
      heap.clear();
      int goal_x = (int)m_goals[idx].x / Constants::mapResolution;
      int goal_y = (int)m_goals[idx].y / Constants::mapResolution;
      heap.push(std::make_pair(State(goal_x, goal_y, 0), 0));

      while (!heap.empty()) {
        std::pair<State, double> node = heap.top();
        heap.pop();

        int x = node.first.x;
        int y = node.first.y;
        for (int dx = -1; dx <= 1; dx++)
          for (int dy = -1; dy <= 1; dy++) {
            if (dx == 0 && dy == 0) continue;
            int new_x = x + dx;
            int new_y = y + dy;
            if (new_x == goal_x && new_y == goal_y) continue;
            if (new_x >= 0 && new_x < m_dimx && new_y >= 0 && new_y < m_dimy &&
                holonomic_cost_maps[idx][new_x][new_y] == 0 &&
                temp_obs_set.find(std::make_pair(new_x, new_y)) ==
                    temp_obs_set.end()) {
              holonomic_cost_maps[idx][new_x][new_y] =
                  holonomic_cost_maps[idx][x][y] +
                  sqrt(pow(dx * Constants::mapResolution, 2) +
                       pow(dy * Constants::mapResolution, 2));
              heap.push(std::make_pair(State(new_x, new_y, 0),
                                       holonomic_cost_maps[idx][new_x][new_y]));
            }
          }
      }
    }

    // for (size_t idx = 0; idx < m_goals.size(); idx++) {
    //   std::cout << "---------Cost Map -------Agent: " << idx
    //             << "------------\n";
    //   for (size_t i = 0; i < m_dimx; i++) {
    //     for (size_t j = 0; j < m_dimy; j++)
    //       std::cout << holonomic_cost_maps[idx][i][j] << "\t";
    //     std::cout << std::endl;
    //   }
    // }
  }

  bool generatePath(State startState, int act, double deltaSteer,
                    double deltaLength,
                    std::vector<std::pair<State, double>> &result) {
    double xSucc, ySucc, yawSucc, dx, dy, dyaw, ratio;
    result.emplace_back(std::make_pair<>(startState, 0));
    if (act == 0 || act == 3) {
      for (size_t i = 0; i < (size_t)(deltaLength / Constants::dx[act]); i++) {
        State s = result.back().first;
        xSucc = s.x + Constants::dx[act] * cos(-s.yaw) -
                Constants::dy[act] * sin(-s.yaw);
        ySucc = s.y + Constants::dx[act] * sin(-s.yaw) +
                Constants::dy[act] * cos(-s.yaw);
        yawSucc = Constants::normalizeHeadingRad(s.yaw + Constants::dyaw[act]);
        State nextState(xSucc, ySucc, yawSucc, result.back().first.time + 1);
        if (!stateValid(nextState)) return false;
        result.emplace_back(std::make_pair<>(nextState, Constants::dx[0]));
      }
      ratio = (deltaLength -
               (int)(deltaLength / Constants::dx[act]) * Constants::dx[act]) /
              Constants::dx[act];
      dyaw = 0;
      dx = ratio * Constants::dx[act];
      dy = 0;
    } else {
      for (size_t i = 0; i < (size_t)(deltaSteer / Constants::dyaw[act]); i++) {
        State s = result.back().first;
        xSucc = s.x + Constants::dx[act] * cos(-s.yaw) -
                Constants::dy[act] * sin(-s.yaw);
        ySucc = s.y + Constants::dx[act] * sin(-s.yaw) +
                Constants::dy[act] * cos(-s.yaw);
        yawSucc = Constants::normalizeHeadingRad(s.yaw + Constants::dyaw[act]);
        State nextState(xSucc, ySucc, yawSucc, result.back().first.time + 1);
        if (!stateValid(nextState)) return false;
        result.emplace_back(std::make_pair<>(
            nextState, Constants::dx[0] * Constants::penaltyTurning));
      }
      ratio = (deltaSteer - (int)(deltaSteer / Constants::dyaw[act]) *
                                Constants::dyaw[act]) /
              Constants::dyaw[act];
      dyaw = ratio * Constants::dyaw[act];
      dx = Constants::r * sin(dyaw);
      dy = -Constants::r * (1 - cos(dyaw));
      if (act == 2 || act == 5) {
        dx = -dx;
        dy = -dy;
      }
    }
    State s = result.back().first;
    xSucc = s.x + dx * cos(-s.yaw) - dy * sin(-s.yaw);
    ySucc = s.y + dx * sin(-s.yaw) + dy * cos(-s.yaw);
    yawSucc = Constants::normalizeHeadingRad(s.yaw + dyaw);
    // std::cout << m_agentIdx << " ratio::" << ratio << std::endl;
    State nextState(xSucc, ySucc, yawSucc, result.back().first.time + 1);
    if (!stateValid(nextState)) return false;
    result.emplace_back(std::make_pair<>(nextState, ratio * Constants::dx[0]));

    // std::cout << "Have generate " << result.size() << " path segments:\n\t";
    // for (auto iter = result.begin(); iter != result.end(); iter++)
    //   std::cout << iter->first << ":" << iter->second << "->";
    // std::cout << std::endl;

    return true;
  }

 private:
  int m_dimx;
  int m_dimy;
  std::vector<std::vector<std::vector<double>>> holonomic_cost_maps;
  std::unordered_set<Location> m_obstacles;
  std::multimap<int, State> m_dynamic_obstacles;
  std::vector<State> m_goals;
  // std::vector< std::vector<int> > m_heuristic;
  std::vector<double> m_vel_limit;
  size_t m_agentIdx;
  const Constraints *m_constraints;
  int m_lastGoalConstraint;
  int m_highLevelExpanded;
  int m_lowLevelExpanded;
};

}  // namespace libMultiRobotPlanning


/**
 * @file cmp_speed.cc
 *
 * Compare the speed of the model to the STD14 scenario from CASS.
 * In this scenario, CASS specifies a grid of sound speeds and
 * bottom depths in latitude and longitude coordinates for an
 * area near Hawaii.
 *
 *      - Area 16.2N-24.6N, 164.7-155.4W
 *      - Source: 19.52N 160.5W, 200 meters deep (SW of Hawaii)
 *      - Targets: ring of receivers at 100 km from source
 *      - Frequency: 250 Hz
 *      - Travel Time: 80 seconds
 *      - Time Step: 100 msec
 *      - D/E: [-90,90] as 181 tangent spaced rays
 *      - AZ: [0,360] in 15.0 deg steps
 *
 */
#include <usml/waveq3d/waveq3d.h>
#include <usml/netcdf/netcdf_files.h>
#include <fstream>
#include <boost/progress.hpp>

using namespace usml::waveq3d ;
using namespace usml::netcdf ;

/**
 * Command line interface.
 */
int main( int argc, char* argv[] ) {
    cout << "=== cmp_speed ===" << endl ;

    int num_targets = 100;
    if ( argc > 1 ) {
        num_targets = atoi( argv[1] ) ;
    }

    // define scenario parameters

    seq_linear freq( 250.0, 1.0, 1 ) ;
    wposition::compute_earth_radius( 19.52 ) ;
    wposition1 src_pos( 19.52, -160.5, -200.0 ) ;
    seq_rayfan de( -90.0, 90.0, 181 ) ;
    seq_linear az( 0.0, 15.0, 360.0 ) ;
    const double target_depth = 100.0; // Meters
    const double target_range = 100000.0; // Meters
    const double time_max = 80.0 ;
    const double time_step = 0.100 ;

    // load STD14 environmental data from netCDF files

    const double lat1 = 16.2 ;
    const double lat2 = 24.6 ;
    const double lng1 = -164.4;
    const double lng2 = -155.5 ;

    cout << "load STD14 environmental profile data" << endl ;

// Original data_grid
//    profile_model* profile = new profile_grid<double,3>( new netcdf_profile(
//            USML_STUDIES_DIR "/cmp_speed/std14profile.nc", 0.0, lat1, lat2, lng1, lng2,
//            wposition::earth_radius ) ) ;

    //fast_grid_3d
    data_grid<double,3>* ssp = new netcdf_profile( USML_STUDIES_DIR "/cmp_speed/std14profile.nc",
            0.0, lat1, lat2, lng1, lng2, wposition::earth_radius ) ;
    data_grid_svp* fast_ssp = new data_grid_svp(ssp) ;
    profile_model* profile = new profile_grid_fast( fast_ssp ) ;

//  attenuation_model* attn = new attenuation_constant(0.0);
//  profile_model* profile = new profile_linear(1500.0,attn);

    cout << "load STD14 environmental bathy data" << endl ;

// Original data_grid
//    boundary_model* bottom = new boundary_grid<double,2>( new netcdf_bathy(
//            USML_STUDIES_DIR "/cmp_speed/std14bathy.nc", lat1, lat2, lng1, lng2,
//            wposition::earth_radius ) ) ;

    //fast_grid_2d
    data_grid<double,2>* grid = new netcdf_bathy( USML_STUDIES_DIR "/cmp_speed/std14bathy.nc",
        lat1, lat2, lng1, lng2, wposition::earth_radius );
    data_grid_bathy* fast_grid = new data_grid_bathy(grid) ;
    boundary_model* bottom = new boundary_grid_fast( fast_grid ) ;

//    boundary_model* bottom = new boundary_flat(4000.0) ;
//    bottom->reflect_loss(new reflect_loss_constant(0.0)) ;
//    bottom->reflect_loss(new reflect_loss_rayleigh(reflect_loss_rayleigh::MUD));

//// Bathy Testing
//    wposition1 seaPoint( 22.332, -162.615 ) ;
//    double rho;
//    bottom->height(seaPoint, &rho);
//    cout << "height  " <<  wposition::earth_radius - rho << endl;

    boundary_model* surface = new boundary_flat() ;

    ocean_model ocean( surface, bottom, profile ) ;

    // initialize eigenray_collection targets and wavefront

    cout << "initialize " << num_targets << " targets" << endl ;

    wposition target( num_targets, 1, src_pos.latitude(), src_pos.longitude(), target_depth ) ;
    // build a series of targets at 100 km
    double angle = TWO_PI/num_targets;
    double bearing_inc = angle;
    for (size_t n = 0; n < num_targets; ++n) {
        wposition1 aTarget( src_pos, target_range, bearing_inc) ;
        target.latitude( n, 0, aTarget.latitude());
        target.longitude( n, 0, aTarget.longitude());
        target.altitude( n, 0, aTarget.altitude());
        bearing_inc = bearing_inc + angle;
    }

    eigenray_collection loss(freq, src_pos, de, az, time_step, &target);
    wave_queue wave( ocean, freq, src_pos, de, az, time_step, &target ) ;

    wave.add_eigenray_listener(&loss);

    // propagate wavefront

    cout << "propagate wavefronts for " << time_max << " secs" << endl ;
    {
        boost::progress_timer timer ;
        while ( wave.time() < time_max ) {
            wave.step() ;
        }
    }
}


#include <iostream>
#include <math.h>
#include <mutex>
#include <time.h>

//PCL
/*
//This solves the problem of compiling pcl search with C++11
#include <pcl/search/impl/search.hpp>
#ifndef PCL_NO_PRECOMPILE
#include <pcl/impl/instantiate.hpp>
#include <pcl/point_types.h>
PCL_INSTANTIATE(Search, PCL_POINT_TYPES)
#endif // PCL_NO_PRECOMPILE
*/
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl_ros/transforms.h>

//ROS
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <std_msgs/Bool.h>

//Eigen
#include <Eigen/Geometry>
#include <Eigen/Dense>

std::mutex mutex;

bool store = false;

std::string output_file;

tf::TransformListener *listener;

void pcCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{

	//Transform the coordinates of the pointcloud
	sensor_msgs::PointCloud2 local;
	pcl_ros::transformPointCloud("indires_rover/odom", *msg, local, *listener);

	//pc_frame = msg->header.frame_id;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    //pcl::fromROSMsg(*msg, *cloud);
	pcl::fromROSMsg(local, *cloud);

	mutex.lock();
	if(store)
	{
		printf("\nSTORING FILE TO PLY IN:\n %s \n", output_file.c_str());
		pcl::io::savePLYFileBinary(output_file, *cloud);
		pcl::io::savePLYFileASCII(output_file+"_ascci.ply", *cloud);

		//pcl::PCLPointCloud2 cloud
		//pcl::PLYWriter writer;
 		//writer.write (filename, cloud, Eigen::Vector4f::Zero (),
               //Eigen::Quaternionf::Identity (), binary, use_camera);

	}
	store = false;
	mutex.unlock();

}

/*void poseCallback(const nav_msgs::OdometryConstPtr &msg)
{
	geometry_msgs::PoseStamped pose;

	pose.header = msg->header;
	pose.pose = msg->pose.pose;

	geometry_msgs::PoseStamped pose_out;
	//if(!pc_frame.empty())
	pose_out = transformPoseTo(pose, odom_frame, false);

	mutex.lock();
	robot_pose = pose_out;
	mutex.unlock();

}*/

void storeCallback(const std_msgs::BoolConstPtr &msg)
{
	mutex.lock();
	store = true;
	mutex.unlock();
}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "pcl_filters");
	ROS_INFO("---PCL to PLY---");
    ros::NodeHandle n;
	ros::NodeHandle nh("~");

	listener = new tf::TransformListener(ros::Duration(10));

	ros::Publisher store_pub = n.advertise<std_msgs::Bool>("store_ply", 1);

	std::string pointcloud_topic = "indires_rover/front_rgbd_camera/front_rgbd_camera/depth/points";
	//camera_tf = "indires_rover/front_rgbd_camera/depth_frame";
	nh.getParam("pointcloud_topic", pointcloud_topic);
	output_file = "PointCloud_filtered";
	nh.getParam("output_file", output_file);

	ros::Subscriber sub;
	sub  = n.subscribe(pointcloud_topic, 1, pcCallback);

	//ros::Subscriber sub2;
	//if(use_robot_pose)
	//	sub2 = n.subscribe(robot_pose_topic, 1, poseCallback);

	ros::Subscriber action;
	action = n.subscribe("store_ply", 1, storeCallback);

	ros::spin();

}


// Copyright (c) 2019, The Monero And Italo Project
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other
//    materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors may be
//    used to endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
// THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <boost/multiprecision/cpp_int.hpp>

void div128_64(uint64_t dividend_hi, uint64_t dividend_lo, uint64_t divisor, uint64_t* quotient_hi, uint64_t *quotient_lo, uint64_t *remainder_hi, uint64_t *remainder_lo)
{
  typedef boost::multiprecision::uint128_t uint128_t;

  uint128_t dividend = dividend_hi;
  dividend <<= 64;
  dividend |= dividend_lo;

  uint128_t q, r;
  divide_qr(dividend, uint128_t(divisor), q, r);

  *quotient_hi = ((q >> 64) & 0xffffffffffffffffull).convert_to<uint64_t>();
  *quotient_lo = (q & 0xffffffffffffffffull).convert_to<uint64_t>();
  if (remainder_hi)
    *remainder_hi = ((r >> 64) & 0xffffffffffffffffull).convert_to<uint64_t>();
  if (remainder_lo)
    *remainder_lo = (r & 0xffffffffffffffffull).convert_to<uint64_t>();
}


#include <iostream>
#include <fstream>
#include <stdexcept>

#include <Nwn/character.hpp>
#include <Nwn/ablblock.hpp>
#include <Nwn/feat.hpp>

#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>

namespace Nwn {

const int Character::ablPointBuy;
const int Character::minAblScore;
const int Character::maxAblScore;
const int Character::maxLevel;

Character::Character() :
    abls{ std::make_unique< AblBlock >( minAblScore ) }
{
}

Character::~Character() noexcept
{
}

int Character::incCost(int curScore) const
{
    switch( curScore ) {
    case 8: return 1;
    case 9: return 1;
    case 10: return 1;
    case 11: return 1;
    case 12: return 1;
    case 13: return 1;
    case 14: return 2;
    case 15: return 2;
    case 16: return 3;
    case 17: return 3;

    default:
        std::cerr << "ability range exceeded\n";
        return 0;
    }
}

void Character::incAbl( AblScore abl )
{
    const auto curValue = abls->getAbl( abl );
    if( curValue < maxAblScore ) {
        const auto cost = incCost( curValue );
        if( cost <= ablPointsRemain ) {
            ablPointsRemain -= cost;
            abls->setAbl( abl, curValue + 1);
        }
    }
}

void Character::decAbl( AblScore abl )
{
    const auto curValue = abls->getAbl( abl );
    if( curValue > minAblScore ) {
        const auto cost = incCost( curValue - 1 );
        ablPointsRemain += cost;
        abls->setAbl( abl, curValue - 1);
    }
}

AblScore Character::getAblInc( int n ) const
{
    assert( n >= 0 && n <= 6 );
    return ablIncs[ n ];
}

void Character::setAblInc( int n, AblScore abl )
{
    assert( n >= 0 && n <= 6 );
    ablIncs[ n ] = abl;
}

void Character::save( const char* fileName ) const
{
    std::ofstream ofs( fileName );
    assert( ofs.good() );
    boost::archive::xml_oarchive oa( ofs );
    oa << boost::serialization::make_nvp( "character", *this );
}

void Character::restore( const char* fileName )
{
    std::ifstream ifs( fileName );
    assert( ifs.good() );
    boost::archive::xml_iarchive ia( ifs );
    ia >> boost::serialization::make_nvp( "character", *this );
}

void Character::pushLevel( const std::string& chclass )
{
    if( levels.size() < maxLevel ) {
        levels.push_back( chclass );
    }
    else {
        throw std::length_error( "max level exceeded" );
    }
}

void Character::popLevel()
{
    if( !levels.empty() ) {
        levels.pop_back();
    }
    else {
        throw std::length_error( "levels already empty" );
    }
}

const std::string& Character::getLevel( int lvl ) const
{
    assert( lvl < maxLevel );
    return levels[ lvl ];
}

void Character::setLevel( int lvl, const std::string& chclass )
{
    assert( lvl < maxLevel );
    if( lvl >= 0 && static_cast<size_t>( lvl ) < levels.size() ) {
        levels[ lvl ] = chclass;
    }
    else {
        throw std::length_error( "level not present" );
    }
}

std::map< std::string, int > Character::getChClassCountsAtLvl( int lvl ) const
{
    assert( lvl < maxLevel );
    std::map< std::string, int > chclassCounts;
    for( int i = 0; i <= std::min( lvl, getNumLevels() - 1 ); ++i ) {
        const auto& lvlClass = getLevel( i );
        if( chclassCounts.count( lvlClass ) == 0 ) {
            chclassCounts[ lvlClass ] = 1;
        }
        else {
            ++chclassCounts[ lvlClass ];
        }
    }
    return chclassCounts;
}

const std::vector< int >& Character::getFeatChoicesAtLvl( int lvl ) const
{
    static const std::vector< int > emptyReturn;

    assert( lvl < maxLevel );
    const auto choicesItr = featChoices.find( lvl );
    if( choicesItr != featChoices.end() ) {
        return *( ( *choicesItr ).second );
    }
    return emptyReturn;
}

void Character::setFeatChoiceAtLvl( int lvl, int index, int featId )
{
    assert( lvl < maxLevel );
    const auto choicesItr = featChoices.find( lvl );
    if( choicesItr == featChoices.end() ) {
        featChoices[ lvl ] = std::make_unique< std::vector< int > >();
    }

    auto& choices = *( featChoices[ lvl ] );
    const auto i = static_cast< size_t >( index );
    if( i >= choices.size() ) {
        choices.resize( i + 1, INVALID_FEAT_ID );
    }
    choices[ i ] = featId;
}

} // namespace Nwn


#include "whoop.hpp"
#include <math.h>
#include <map>
#include "csf.h"
#include <boost/timer/timer.hpp>
#include <chrono>

// using namespace whoop;
using namespace std;

/*********************************************************************************************************************/
/************************************  COMMAND LINE ARGUMENTS ********************************************************/
/*********************************************************************************************************************/

int ARG_BUFFET_GRANULARITY = LINE_SIZE_BYTES / BYTES_PER_VERTEX;  // buffet granularity
int ARG_S_TILE_SIZE        = 32;
int ARG_D_TILE_SIZE        = 32;
int ARG_TILE_SIZE_OVERRIDE = 0;
int ARG_PRINT_GRAPH        = 0;
int ARG_NUM_COMPUTE_TILE   = 1;
int ARG_NUM_DOT_C          = NUM_DOT_C;
int ARG_NUM_DOT_M          = -1;
int ARG_RF_BUFFET_SIZE_KB  = 32;
int ARG_L1_BUFFET_SIZE_KB  = 256;
int ARG_L2_BUFFET_SIZE_KB  = 2048;
int ARG_RUN_SERIAL         = 0;
int ARG_RUN_MODE           = 0;
int ARG_COMPRESSION_FORMAT = 0;
int ARG_MAX_ITERATIONS     = 1024;
int ARG_USE_SEED           = 0;

void MyInit(int argc, char** argv)
{
    AddOption( &ARG_S_TILE_SIZE, "src_tile_size", "Source      Tile Size");
    AddOption( &ARG_D_TILE_SIZE, "dst_tile_size", "Destination Tile Size");
    AddOption( &ARG_TILE_SIZE_OVERRIDE, "override_tile_size", "Override The Tile Size");
    AddOption( &ARG_BUFFET_GRANULARITY, "granularity", "fetch granularity (in item count) from next level of memory");
    AddOption( &ARG_PRINT_GRAPH, "print", "print the input graph in CSF");
    AddOption( &ARG_NUM_COMPUTE_TILE, "numCT", "Number of Compute Tiles");
    AddOption( &ARG_NUM_DOT_C, "dotC", "Number of Compute Tiles");
    AddOption( &ARG_NUM_DOT_M, "dotM", "Number of Compute Tiles");
    AddOption( &ARG_RF_BUFFET_SIZE_KB, "RF", "RF Buffet Size");
    AddOption( &ARG_L1_BUFFET_SIZE_KB, "L1", "L1 Buffet Size");
    AddOption( &ARG_L2_BUFFET_SIZE_KB, "L2", "L2 Buffet Size");
    AddOption( &ARG_RUN_SERIAL, "serial", "Serial Mode");
    AddOption( &ARG_RUN_MODE, "mode", "Run Mode");
    AddOption( &ARG_COMPRESSION_FORMAT, "format", "Compression Format");
    AddOption( &ARG_MAX_ITERATIONS, "max_iter", "Max Iterations");
    AddOption( &ARG_USE_SEED, "seed", "seed");

    whoop::Init(argc, argv);
}

/*********************************************************************************************************************/
/**************************************  TILING PARAMETERS ***********************************************************/
/*********************************************************************************************************************/

void CreateBufferBasedTileSizes( int V, int& S0, int& D0, int& S1, int& D1, int& S2, int& D2 )
{
    int L1_Vertex_Capacity = ARG_L1_BUFFET_SIZE_KB * KILO / BYTES_PER_VERTEX;
    int L2_Vertex_Capacity = ARG_L2_BUFFET_SIZE_KB * KILO / BYTES_PER_VERTEX;

    S0 = L1_Vertex_Capacity;
    D0 = L2_Vertex_Capacity;

    if( S0 > V ) S0 = V;
    if( D0 > V ) D0 = V;

    S1 = (V % S0) ? (V/S0+1) : (V/S0);
    D1 = (V % D0) ? (V/D0+1) : (V/D0);

    if( S1 < NUM_DOT_C )
    {
        S2 = 1;
    }
    else
    {
        S2 = S1/NUM_DOT_C + ( (S1%NUM_DOT_C) ? 1 : 0 );
        S1 = NUM_DOT_C;
    }

//     if( D1 < NUM_DOT_M )
//     {
        D2 = 1;
//     }
}

bool SetupTileSizes( int V, int& S0, int& D0, int& S1, int& D1, int& S2, int& D2 )
{
    //
    bool amTiled = true;

    S0 = (ARG_S_TILE_SIZE == 0) ? V : ARG_S_TILE_SIZE;
    D0 = (ARG_D_TILE_SIZE == 0) ? V : ARG_D_TILE_SIZE;

    if( (ARG_S_TILE_SIZE==0 ) && (ARG_D_TILE_SIZE == 0) )
    {
        amTiled = false;
    }

    if( S0 > V ) S0 = V;
    if( D0 > V ) D0 = V;

    S1 = (V % S0) ? (V/S0+1) : (V/S0);
    D1 = (V % D0) ? (V/D0+1) : (V/D0);

    if( S1 < NUM_DOT_C )
    {
        S2 = 1;
    }
    else
    {
        S2 = S1/NUM_DOT_C + ( (S1%NUM_DOT_C) ? 1 : 0 );
        S1 = NUM_DOT_C;
    }

//     if( D1 < NUM_DOT_M )
//     {
        D2 = 1;
//     }

    return amTiled;
}

/*********************************************************************************************************************/
/*********************************************************************************************************************/

void PrintInfo( GraphAlgorithm &graphAlg, bool print_arch=true )
{
    graphAlg.PrintInfo();

    if( print_arch )
    {
        cout<<"================================================="<<endl;
        cout<<"SPMV On Symphony With Architecture Parameters"<<endl;
        cout<<"\tNumber of Compute Tiles:    "<<ARG_NUM_COMPUTE_TILE<<endl;
        cout<<"\tNumber of DOT-Cs:           "<<ARG_NUM_DOT_C<<endl;
        cout<<"\tNumber of DOT-Ms:           "<<ARG_NUM_DOT_M<<endl;
        cout<<"\tL1 Buffer Size (KB):        "<<ARG_L1_BUFFET_SIZE_KB<<endl;
        cout<<"\tL2 Buffer Size (KB):        "<<ARG_L2_BUFFET_SIZE_KB<<endl;
        cout<<"================================================="<<endl;
    }

    cout<<endl;
}

void PrintTileInfo( const vector<int>& tile_sizes )
{

    cout<<"================================================="<<endl;
    cout<<"Tile Sizes"<<endl;
    cout<<"\tTile Size  (S0):           "<<tile_sizes[0]<<endl;
    cout<<"\tTile Size  (D0):           "<<tile_sizes[1]<<endl;
    cout<<endl;
    cout<<"\t# of Tiles (S1):           "<<tile_sizes[2]<<endl;
    cout<<"\t# of Tiles (D1):           "<<tile_sizes[3]<<endl;
    cout<<endl;
    cout<<"\t# of Tiles (S2):           "<<tile_sizes[4]<<endl;
    cout<<"\t# of Tiles (D2):           "<<tile_sizes[5]<<endl;
    cout<<"\tTot # of Tiles:            "<<(tile_sizes[2]*tile_sizes[3]*tile_sizes[4]*tile_sizes[5])<<endl;
    cout<<"================================================="<<endl;

    cout<<endl;
}

/*********************************************************************************************************************/
/*********************************************************************************************************************/

int main(int argc, char** argv)
{

    /******************************************************/
    // Set up the graph inputs
    /******************************************************/
    VecIn  SegmentArray("segarray");
    VecIn  SrcCoords("coordarray");

    /******************************************************/
    // Initialize and read the segment and cordinate arrays
    /******************************************************/
    MyInit( argc, argv);

    /******************************************************/
    // Set up the input graph and tiled related parameters
    /******************************************************/
    int numVertices  = SegmentArray.Size()-1;
    int numEdges     = SrcCoords.Size();

    const int E      = numEdges;
    const int V      = numVertices;
    const int S      = numVertices;
    const int D      = numVertices;

    int S0, D0, S1, D1, S2, D2;
    /******************************************************/
    // Set up the graph algorithm
    /******************************************************/
    FORMAT_TYPE formatIn = (FORMAT_TYPE) ARG_COMPRESSION_FORMAT;
    GraphAlgorithm graphAlg( formatIn, &SegmentArray, &SrcCoords, NULL );

    /******************************************************/
    /******************************************************/

    if( ARG_RUN_MODE == MODE_UNTILED )
    {

        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Run Native
        cout<<"(+)  Running Untiled Natively ..."<<endl;
        graphAlg.Untiled( S0, S1, S2, formatIn );

        cout<<endl;
        cout<<"(+)  Running Untiled Whoop Mode ..."<<endl;
        graphAlg.WhoopUntiled( S0, S1, S2, ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn );
    }
    else if( ARG_RUN_MODE == MODE_TILED_PRE_PROCESSED_SERIAL )
    {
        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Pre-Process and Create The Meta-Data
        graphAlg.SetTileSizes( {S0,D0,S1,D1,S2,D2}, formatIn, STATIONARY_DST_TILE );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Run Native
        cout<<"(+)  Running Natively ..."<<endl;
        graphAlg.Run5( formatIn );

        // Run Whoop
        cout<<endl;
        cout<<"(+)  Running In Serial Whoop Mode ..."<<endl;
        graphAlg.RunWhoop5( ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn );
    }
    else if( ARG_RUN_MODE == MODE_TILED_PRE_PROCESSED_PARALLEL )
    {
        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Pre-Process and Create The Meta-Data
        graphAlg.SetTileSizes( {S0,D0,S1,D1,S2,D2}, formatIn, STATIONARY_DST_TILE );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Print Tile Info
        PrintTileInfo( {S0,D0,S1,D1,S2,D2} );

        // Run Native
        cout<<"(+)  Running Natively ..."<<endl;
        graphAlg.Run5( formatIn );

        // Run Whoop
        cout<<endl;
        cout<<"(+)  Running In Parallel Whoop Mode ..."<<endl;
        graphAlg.RunWhoop5_Parallel( ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn );
    }
    else if( ARG_RUN_MODE == MODE_TILED_PRE_PROCESSED_V2_PARALLEL )
    {
        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Pre-Process and Create The Meta-Data
        graphAlg.SetTileSizes( {S0,D0,S1,D1,S2,D2}, formatIn, STATIONARY_DST_TILE_AND_CT );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Print Tile Info
        PrintTileInfo( {S0,D0,S1,D1,S2,D2} );

        if( formatIn != FORMAT_CSR )
        {
            cout<<"Unsupported Format"<<endl;
            exit(0);
        }

        // Run Native
        cout<<"(+)  Running Natively ..."<<endl;

        auto start = chrono::steady_clock::now();
        graphAlg.Run5_CSR_ver2();
        auto end = chrono::steady_clock::now();

        auto diff = end - start;
        cout << chrono::duration <double, milli> (diff).count() << " ms" << endl;

        // Run Whoop
        cout<<endl;
        cout<<"(+)  Running In Parallel Whoop Mode ..."<<endl;
        graphAlg.RunWhoop5_Parallel_CSR_ver2( ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB );
    }
    else if( ARG_RUN_MODE == MODE_TILED_ONLINE )
    {
        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Print Tile Info
        PrintTileInfo( {S0,D0,S1,D1,S2,D2} );

        // Run Natively
        cout<<endl;
        cout<<"(+)  Running Online Tiling Natively ..."<<endl;
        graphAlg.OnlineTiling(D1, S2, S1, D0, S0, formatIn);

        // Run Whoop
        cout<<endl;
        cout<<"(+)  Running Online Tiling Whoop ..."<<endl;
        graphAlg.WhoopOnlineTiling(D1, S2, S1, D0, S0,  ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn );
    }
    else if( ARG_RUN_MODE == MODE_TILED_ONLINE_VER3 )
    {
        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Print Tile Info
        PrintTileInfo( {S0,D0,S1,D1,S2,D2} );

        // Run Natively
        cout<<endl;
        cout<<"(+)  Running Online Tiling v2 Natively ..."<<endl;
        graphAlg.OnlineTiling_ver2(D1, S2, S1, D0, S0, formatIn);

        // Run Whoop
        cout<<endl;
        cout<<"(+)  Running Online Tiling v3 Whoop ..."<<endl;
        graphAlg.WhoopOnlineTiling_ver3(D1, S2, S1, D0, S0,  ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn );
    }
    else if( ARG_RUN_MODE == MODE_TILED_ONLINE_COPLAND )
    {
        // Print Setup Information
        PrintInfo( graphAlg, false );

        // Run Natively
        cout<<endl;
        cout<<"(+)  Running Online Tiling Copland ..."<<endl;
        graphAlg.OnlineTiling_CSR_Copland();

        // Run Whoop
        cout<<endl;
        cout<<"(+)  Running Whoop Online Tiling Copland ..."<<endl;
        graphAlg.WhoopOnlineTiling_CSR_Copland();
    }
    else if( ARG_RUN_MODE == MODE_TILED_PREPROCESSED_COPLAND )
    {
        // Print Setup Information
        PrintInfo( graphAlg, false );

        // Run Natively
        cout<<endl;
        cout<<"(+)  Running Online Tiling Copland ..."<<endl;
        graphAlg.OnlineTiling_CSR_Copland();

    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_LGC_NIBBLE )
    {
        int seed = ARG_USE_SEED;

        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Print Tile Info
        PrintTileInfo( {S0,D0,S1,D1,S2,D2} );

//     graphAlg.PageRankNibble_Untiled( seed );
//     graphAlg.Whoop_PageRankNibble_Untiled(seed, ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn);

        auto start = chrono::steady_clock::now();
        graphAlg.PageRankNibble_Untiled_Compressed( seed );
        auto end = chrono::steady_clock::now();

        auto diff = end - start;
        cout << chrono::duration <double, milli> (diff).count() << " ms" << endl;

        graphAlg.Whoop_PageRankNibble_Untiled_Compressed(seed, ARG_RF_BUFFET_SIZE_KB, ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn);
    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_PARALLEL_LGC_NIBBLE )
    {
        int seed = ARG_USE_SEED;

        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Print Tile Info
        PrintTileInfo( {S0,D0,S1,D1,S2,D2} );

        graphAlg.PageRankNibble_Untiled_Compressed_Parallel( seed );
        graphAlg.Whoop_PageRankNibble_Untiled_Compressed_Parallel(seed, ARG_RF_BUFFET_SIZE_KB, ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn);
    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_PARALLEL_TILED_LGC_NIBBLE )
    {
        int seed = ARG_USE_SEED;

        // Determine Tile Sizes Using On-Chip Buffer Sizes
        CreateBufferBasedTileSizes( V, S0, D0, S1, D1, S2, D2 );

        // Print Setup Information
        PrintInfo( graphAlg );

        // Print Tile Info
        PrintTileInfo( {S0,D0,S1,D1,S2,D2} );

        graphAlg.PageRankNibble_Untiled_Compressed_Parallel_Tiled( seed );
        graphAlg.Whoop_PageRankNibble_Untiled_Compressed_Parallel_Tiled(seed, ARG_RF_BUFFET_SIZE_KB, ARG_L1_BUFFET_SIZE_KB, ARG_L2_BUFFET_SIZE_KB, formatIn);
    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_PARALLEL_REFERENCE )
    {
        int seed = ARG_USE_SEED;

        graphAlg.PageRankNibble_Untiled_Compressed_Parallel_Tiled( seed );
    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_ITER_TRACE )
    {
        int seed = ARG_USE_SEED;

        graphAlg.PageRankNibble_Untiled_Compressed_TraceIter( seed );
    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_PARALLEL_ITER_TRACE )
    {
        int seed = ARG_USE_SEED;

        graphAlg.PageRankNibble_Untiled_Compressed_Parallel_TraceIter( seed );
    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_PARALLEL_FGEN_TRACE )
    {
        int seed = ARG_USE_SEED;

        graphAlg.PageRankNibble_Untiled_Compressed_Parallel_TraceFrontierGeneration( seed );
    }
    else if( ARG_RUN_MODE == MODE_UNTILED_COMPRESSED_PARALLEL_PCOMP_TRACE )
    {
        int seed = ARG_USE_SEED;

        graphAlg.PageRankNibble_Untiled_Compressed_Parallel_TracePRComp( seed );
    }
    else if( ARG_RUN_MODE == MODE_ISTA_TRACE )
    {
        int seed = ARG_USE_SEED;

        graphAlg.Whoop_PageRankNibble_Untiled_Compressed_Parallel_TraceISTAIter( seed );
    }
    else
    {
        cout<<"Unknown Mode"<<endl;
        exit(0);
    }
}


#include <boost/icl/interval_base_map.hpp>


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_GEOMETRIC_HH
#define GRAPH_GEOMETRIC_HH

#include <iostream>
#include <unordered_map>

#include <boost/functional/hash.hpp>
#include "graph_util.hh"

#ifndef __clang__
#include <ext/numeric>
using __gnu_cxx::power;
#else
template <class Value>
Value power(Value value, int n)
{
    return pow(value, n);
}
#endif

namespace graph_tool
{
using namespace std;
using namespace boost;

template <class Point, class Range>
void get_box(const Point& p, double w, vector<int>& box,
             const Range& ranges, bool periodic)
{
    if (box.size() != p.size())
        box.resize(p.size());
    for (size_t i = 0; i < p.size(); ++i)
    {
        box[i] = int(floor(p[i] / w));
        if (periodic && p[i] == ranges[i].second)
            box[i] -= 1;
    }
}

template <class Point, class Range>
double get_dist(const Point& p1, const Point& p2,
                const Range& ranges, bool periodic)
{
    double r = 0, diff, size;
    for (size_t i = 0; i < p1.size(); ++i)
    {
        diff = abs(p1[i] - p2[i]);
        if (periodic)
        {
            size = abs(ranges[i].second - ranges[i].first);
            diff = min(diff, abs(diff - size));
        }
        r += pow(diff, 2);
    }
    return sqrt(r);
}

void periodic(vector<int>& box, const vector<pair<int,int> >& ranges)
{
    for (size_t i = 0; i < box.size(); ++i)
    {
        if (box[i] >= ranges[i].second)
            box[i] = ranges[i].first;
        if (box[i] < ranges[i].first)
            box[i] = ranges[i].second - 1;
    }
}

template <class Graph>
bool is_adjacent(typename graph_traits<Graph>::vertex_descriptor v1,
                 typename graph_traits<Graph>::vertex_descriptor v2,
                 Graph& g)
{
    typename graph_traits<Graph>::out_edge_iterator e, e_end;
    for (tie(e, e_end) = out_edges(v1, g); e != e_end; ++e)
        if (target(*e, g) == v2)
            return true;
    return false;
}

struct get_geometric
{
    template <class Graph, class Pos>
    void operator()(Graph& g, Pos upos, vector<vector<double> >& points,
                    vector<pair<double, double> >& ranges,
                    double r, bool periodic_boundary) const
    {
        int N = points.size();
        typename Pos::checked_t pos = upos.get_checked();
        vector<int> box;
        vector<pair<int, int> > box_ranges;

        double w = 2 * r;
        if (periodic_boundary)
        {
            box_ranges.resize(ranges.size());
            for (size_t i = 0; i < ranges.size(); ++i)
            {
                box_ranges[i].first = floor(ranges[i].first / w);
                box_ranges[i].second = ceil(ranges[i].second / w);
            }
        }

        std::unordered_multimap<vector<int>,
                                typename graph_traits<Graph>::vertex_descriptor> boxes;

        for (int i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = add_vertex(g);
            pos[v].resize(points[i].size());
            copy(points[i].begin(), points[i].end(), pos[v].begin());
            get_box(points[i], w, box, ranges, periodic_boundary);
            boxes.insert(make_pair(box, v));
        }

        int i;
        #pragma omp parallel for default(shared) private(i, box) \
            schedule(runtime) if (N > 100)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);

            get_box(points[i], w, box, ranges, periodic_boundary);
            for (int k = 0; k < power(3, int(box.size())); ++k)
            {
                for (int j = 0; j < int(box.size()); ++j)
                  box[j] += ((k / power(3, j)) % 3) - 1;

                if (periodic_boundary)
                    periodic(box, box_ranges);

                typeof(boxes.begin()) iter, end;
                for (tie(iter, end) = boxes.equal_range(box);
                     iter != end; ++iter)
                {
                    typename graph_traits<Graph>::vertex_descriptor w =
                        iter->second;
                    double d = get_dist(pos[v], pos[w], ranges,
                                        periodic_boundary);

                    if (w > v && d <= r &&
                        (!periodic_boundary || !is_adjacent(v, w, g)))
                    {
                        #pragma omp critical
                        add_edge(v, w, g);
                    }
                }
                get_box(points[i], w, box, ranges, periodic_boundary);
            }
        }
    }
};

} // namespace graph_tool

#endif // GRAPH_GEOMETRIC_HH


/**
 * @file test_trajectoryGenerator.cpp
 * @brief This file defines the test cases for the trajectory generator.
 *
 * @copyright Copyright (c) 2022
 *
 */

#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <memory>

#include "proseco_planning/action/action.h"
#include "proseco_planning/action/actionClass.h"
#include "proseco_planning/agent/vehicle.h"
#include "proseco_planning/config/configuration.h"
#include "proseco_planning/config/defaultConfiguration.h"
#include "proseco_planning/trajectory/trajectory.h"
#include "proseco_planning/trajectory/trajectorygenerator.h"

using namespace proseco_planning;

struct TrajectoryPlannerFixture {
  TrajectoryPlannerFixture() { Config::create(config::scenarioSimple, config::optionsSimple); }

  ~TrajectoryPlannerFixture() { Config::get()->reset(); }
};

BOOST_FIXTURE_TEST_SUITE(trajectoryPlanner, TrajectoryPlannerFixture)

BOOST_AUTO_TEST_CASE(laneDetection) {
  // Create road

  float lateralChange{-1.0};
  float velocityChange{5.0};
  Vehicle vehicleConstantAcceleration(config::vehicle);
  Vehicle vehiclePolynomialTrajectory(config::vehicle);
  auto action = std::make_shared<Action>(Action(velocityChange, lateralChange));

  // constant Acceleration
  const auto trajectoryGeneratorC =
      TrajectoryGenerator::createTrajectoryGenerator("constantAcceleration");

  auto trajectoryConstantAcceleration =
      trajectoryGeneratorC->createTrajectory(0.0, action, vehicleConstantAcceleration);

  vehicleConstantAcceleration.updateState(trajectoryConstantAcceleration.m_finalState);

  auto finalStateConstantAcceleration = trajectoryConstantAcceleration.m_finalState;

  // polynomial
  const auto trajectoryGeneratorP = TrajectoryGenerator::createTrajectoryGenerator("jerkOptimal");

  auto trajectoryPolynomial =
      trajectoryGeneratorP->createTrajectory(0.0, action, vehiclePolynomialTrajectory);

  vehiclePolynomialTrajectory.updateState(trajectoryPolynomial.m_finalState);

  auto finalStatePolynomial = trajectoryPolynomial.m_finalState;

  // correct lane detection
  BOOST_REQUIRE(vehicleConstantAcceleration.m_lane < 0);
  BOOST_REQUIRE(vehiclePolynomialTrajectory.m_lane < 0);

  // equality of trajectory variants
  BOOST_REQUIRE(vehicleConstantAcceleration.m_positionY == lateralChange);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_velocityX == velocityChange);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_velocityY == 0);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_accelerationX == 0);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_accelerationY == 0);

  BOOST_REQUIRE(vehiclePolynomialTrajectory.m_positionY == lateralChange);
  BOOST_REQUIRE(vehiclePolynomialTrajectory.m_velocityX == velocityChange);
  BOOST_REQUIRE_SMALL(vehiclePolynomialTrajectory.m_velocityY,
                      float(0.001));  // velocity deviation is less than 1mm/s
  BOOST_REQUIRE(vehiclePolynomialTrajectory.m_accelerationX == 0);
  BOOST_REQUIRE(vehiclePolynomialTrajectory.m_accelerationY == 0);
}

BOOST_AUTO_TEST_CASE(trajectoryGeneration) {
  // create the environment
  float lateralChange{10.0};
  float velocityChange{10.0};
  Vehicle vehicleConstantAcceleration(config::vehicle);
  Vehicle vehiclePolynomialTrajectory(config::vehicle);
  auto action = std::make_shared<Action>(Action(velocityChange, lateralChange));

  // constant Acceleration
  const auto trajectoryGeneratorC =
      TrajectoryGenerator::createTrajectoryGenerator("constantAcceleration");

  auto trajectoryConstantAcceleration =
      trajectoryGeneratorC->createTrajectory(0.0, action, vehicleConstantAcceleration);

  vehicleConstantAcceleration.updateState(trajectoryConstantAcceleration.m_finalState);

  // polynomial
  const auto trajectoryGeneratorP = TrajectoryGenerator::createTrajectoryGenerator("jerkOptimal");

  auto trajectoryPolynomial =
      trajectoryGeneratorP->createTrajectory(0.0, action, vehiclePolynomialTrajectory);

  vehiclePolynomialTrajectory.updateState(trajectoryPolynomial.m_finalState);

  BOOST_REQUIRE(vehicleConstantAcceleration.m_positionY == lateralChange);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_velocityX == velocityChange);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_velocityY == 0);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_accelerationX == 0);
  BOOST_REQUIRE(vehicleConstantAcceleration.m_accelerationY == 0);

  BOOST_REQUIRE_SMALL(vehiclePolynomialTrajectory.m_positionY - lateralChange,
                      (float)(std::abs(lateralChange) * 10e-06));
  BOOST_REQUIRE(vehiclePolynomialTrajectory.m_velocityX == velocityChange);
  BOOST_REQUIRE_SMALL(vehiclePolynomialTrajectory.m_velocityY,
                      float(0.001));  // velocity deviation is less than 1mm/s
  BOOST_REQUIRE(vehiclePolynomialTrajectory.m_accelerationX == 0);
  BOOST_REQUIRE_SMALL(vehiclePolynomialTrajectory.m_accelerationY,
                      float(0.001));  // acceleration deviation is less than 1mm/s^2
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef BREEDER_HH
#define BREEDER_HH

#include <unordered_map>
#include <boost/functional/hash.hpp>
#include <future>
#include <vector>

#include "configrange.hh"
#include "evaluator.hh"

struct BreederOptions
{
  ConfigRange config_range = ConfigRange();
};

template <typename T, typename A>
class ActionImprover
{
protected:
  const double MAX_PERCENT_ERROR = 0.05;
  const Evaluator< T > eval_;

  T tree_;

  std::unordered_map< A, double, boost::hash< A > > eval_cache_ {};

  double score_to_beat_;

  virtual std::vector< A > get_replacements( A & action_to_improve ) = 0;

  void evaluate_replacements(const std::vector< A > &replacements,
    std::vector< std::pair< const A &, std::future< std::pair< bool, double > > > > &scores,
    const double carefulness);

  std::vector< A > early_bail_out(const std::vector< A > &replacements,
        const double carefulness, const double quantile_to_keep);

public:
  ActionImprover( const Evaluator<  T > & evaluator, const T & tree,
                   const double score_to_beat );
  virtual ~ActionImprover() {};

  double improve( A & action_to_improve );
};

template <typename T>
class Breeder
{
protected:
  BreederOptions _options;

  void apply_best_split( T & tree, const unsigned int generation ) const;

public:
  Breeder( const BreederOptions & s_options ) : _options( s_options ) {};
  virtual ~Breeder() {};

  virtual typename Evaluator< T >::Outcome improve( T & whiskers ) = 0;
};

#endif