#include <utility/io/generate_many.hpp>
#include <utility/io/generate_many_sorted.hpp>

#include <burst/iterator/binary_ostream_iterator.hpp>

#include <boost/program_options.hpp>

#include <chrono>
#include <cstddef>
#include <cstdint>
#include <exception>
#include <functional>
#include <iostream>
#include <limits>
#include <random>

void
    do_generate
    (
        std::ostream & stream,
        std::size_t block_size,
        std::size_t range_count,
        std::size_t range_length,
        std::int64_t min,
        std::int64_t max,
        bool seed,
        bool sort,
        bool descending
    )
{
    const auto time = std::chrono::system_clock::now().time_since_epoch().count();
    const auto seed_value = seed ? static_cast<std::default_random_engine::result_type>(time) : 0;
    std::default_random_engine generator(seed_value);

    auto result = burst::make_binary_ostream_iterator(stream);
    if (sort)
    {
        std::vector<std::int64_t> buffer(block_size);
        if (descending)
        {
            utility::generate_many_sorted
            (
                generator,
                block_size,
                range_count,
                range_length,
                min,
                max,
                std::greater<>{},
                result,
                buffer.begin()
            );
        }
        else
        {
            utility::generate_many_sorted
            (
                generator,
                block_size,
                range_count,
                range_length,
                min,
                max,
                std::less<>{},
                result,
                buffer.begin()
            );
        }
    }
    else
    {
        utility::generate_many(generator, range_count, range_length, min, max, result);
    }
}

int main (int argc, const char * argv[])
{
    namespace bpo = boost::program_options;

    bpo::options_description description("Опции");
    description.add_options()
        ("help,h", "Подсказка")
        ("block", bpo::value<std::size_t>()->default_value(1 << 20),
            "Размер используемой памяти в элементах")
        ("count", bpo::value<std::size_t>(),
            "Количество генерируемых наборов")
        ("length", bpo::value<std::size_t>(),
            "Длина одного набора")
        ("min", bpo::value<std::int64_t>()->default_value(std::numeric_limits<std::int64_t>::min()),
            "Минимальное значение элемента набора")
        ("max", bpo::value<std::int64_t>()->default_value(std::numeric_limits<std::int64_t>::max()),
            "Максимальное значение элемента набора")
        ("seed", bpo::value<bool>()->implicit_value(true)->default_value(false),
            "Выбрать произвольную точку начала последовательности псевдослучайных чисел")
        ("sort", bpo::value<bool>()->implicit_value(true)->default_value(false),
            "Породить упорядоченный набор")
        ("descending", bpo::value<bool>()->implicit_value(true)->default_value(false),
            "Упорядочить набор по убыванию");

    try
    {
        bpo::variables_map vm;
        bpo::store(bpo::parse_command_line(argc, argv, description), vm);
        bpo::notify(vm);

        if (vm.count("help"))
        {
            std::cout << description << std::endl;
        }
        else
        {
            std::size_t block_size = vm["block"].as<std::size_t>();
            std::size_t range_count = vm["count"].as<std::size_t>();
            std::size_t range_length = vm["length"].as<std::size_t>();
            std::int64_t min = vm["min"].as<std::int64_t>();
            std::int64_t max = vm["max"].as<std::int64_t>();

            bool seed = vm["seed"].as<bool>();
            bool sort = vm["sort"].as<bool>();
            bool descending = vm["descending"].as<bool>();

            do_generate(std::cout, block_size, range_count, range_length, min, max, seed, sort, descending);
        }
    }
    catch (std::exception &)
    {
        std::cout << description << std::endl;
    }
}


#define BOOST_TEST_MODULE "C++ Unit Tests for metaLBM"
#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
namespace tt = boost::test_tools;

#include <iostream>
#include <vector>
template<class T, std::size_t Alignment = 1>
#ifdef ALIGN_MEMORY
using vector = std::vector<T,
                           boost::alignment::aligned_allocator<T, Alignment> >;
#else
using vector = std::vector<T>;
#endif

#include "input.h"
#include "commons.h"
#include "field_tmp.h"

using namespace lbm;

BOOST_AUTO_TEST_SUITE(TestField)

BOOST_AUTO_TEST_CASE(TestFields) {
  vector<MathVector<valueType, L::dimQ>> fPop_field((L::lX_l+2*L::hX)
                                                    *(L::lY_l+2*L::hY)
                                                    *(L::lZ_l+2*L::hZ));

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    for(int iQ = 0; iQ < L::dimQ; ++iQ) {
      fPop_field[idx][iQ] = (valueType) (idx + iQ);
    }
  }

  std::string fPop_name = "fPop";
  auto fPop_ptr = std::shared_ptr<Field<valueType>>(new DistributionField<valueType>(fPop_name,
                                                                                fPop_field));
  constexpr int numberFields = 1;
  auto fieldsName = std::array<std::string, numberFields>{fPop_name};
  auto fieldsArray = std::array<std::shared_ptr<Field<valueType>>, numberFields>{fPop_ptr};

  Fields<valueType, numberFields> F(fieldsName, fieldsArray);

  BOOST_TEST(F(fPop_name)->fieldName == fPop_name);

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    for(int iQ = 0; iQ < L::dimQ; ++iQ) {
      BOOST_TEST(F(fPop_name)->field[idx][iQ] == (valueType)(L::dimQ*idx + (L::dimQ-1)*L::dimQ/2),
                 tt::tolerance(1e-15));
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
#include "pxr/base/gf/line.h"

#include "pxr/base/tf/pyUtils.h"
#include "pxr/base/tf/wrapTypeHelpers.h"

#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/operators.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/python/tuple.hpp>

#include <string>

using namespace boost::python;

using std::string;

static string _Repr(GfLine const &self) {
    return TF_PY_REPR_PREFIX + "Line(" + TfPyRepr(self.GetPoint(0.0)) + ", " +
        TfPyRepr(self.GetDirection()) + ")";
}

static tuple
FindClosestPointsHelper( const GfLine &l1, const GfLine &l2 )
{
    GfVec3d p1(0), p2(0);
    double t1 = 0.0, t2 = 0.0;
    bool result = GfFindClosestPoints( l1, l2, &p1, &p2, &t1, &t2 );
    return boost::python::make_tuple( result, p1, p2, t1, t2 );
}

static tuple
FindClosestPointHelper( const GfLine &self, const GfVec3d &point )
{
    double t;
    GfVec3d result = self.FindClosestPoint( point, &t );
    return boost::python::make_tuple( result, t );
}

static void
SetDirectionHelper( GfLine &self, const GfVec3d &dir )
{
    self.Set( self.GetPoint(0.0), dir );
}

void wrapLine()
{
    typedef GfLine This;

    def("FindClosestPoints", FindClosestPointsHelper,
        "FindClosestPoints( l1, l2 ) -> tuple<intersects = bool, p1 = GfVec3d, p2 = GfVec3d,"
        " t1 = double, t2 = double>\n"
        "\n"
        "l1 : GfLine\n"
        "l2 : GfLine\n"
        "\n"
        "Computes the closest points between two lines, returning a "
        "tuple.  The first item in the tuple is true if the lines"
        "intersect.  The two points are returned in p1 and p2.  The "
        "parametric distance of each point on the lines is returned "
        "in t1 and t2.\n"
        "----------------------------------------------------------------------" );

    class_<This>( "Line", "Line class", init<>() )
        .def(init< const GfVec3d &, const GfVec3d & >())

        .def( TfTypePythonClass() )

        .def("Set", &This::Set, return_self<>())

        .def("GetPoint", &This::GetPoint)

        .def("GetDirection", &This::GetDirection,
             return_value_policy<copy_const_reference>())

        .add_property( "direction",
                       make_function(&This::GetDirection,
                                     return_value_policy
                                     <copy_const_reference>()),
                       SetDirectionHelper )

        .def( "FindClosestPoint", FindClosestPointHelper )

        .def( str(self) )
        .def( self == self )
        .def( self != self )

        .def("__repr__", _Repr)

        ;

}


#include <boost/algorithm/string.hpp>
#include <boost/unordered_map.hpp>
#include <chrono>
#include <cmath>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <malloc.h>

class Bee {
public:
    double x, y, z;
    Bee* previous = nullptr;
    Bee* following = nullptr;

    Bee(double x1, double y1, double z1)
    {
        x = x1 * 111111;
        y = y1 * 111111;
        z = z1;
    };

    Bee(const Bee& b)
    {
        x = b.x;
        y = b.y;
        z = b.z;
    };
};

class BeeList {
public:
    Bee* first = nullptr;
    Bee* last = nullptr;
    int size = 0;

    void add_bee(Bee* b)
    {
        b->previous = nullptr;
        b->following = this->first;
        if (this->first != nullptr) {
            this->first->previous = b;
            this->last = this->first;
        }
        this->first = b;
        this->size++;
    }

    void add_first_bee(Bee* b)
    {
        b->following = nullptr;
        b->previous = nullptr;
        this->first = b;
        this->last = b;
        this->size++;
    }

    void concatenate_beelist_end(BeeList* bl)
    {
        if (this->size == 0) {
            this->first = bl->first;
            this->last = bl->last;
            this->size = bl->size;
        } else {
            bl->first->previous = this->last;
            last->following = bl->first;
            this->last = bl->last;
            this->size += bl->size;
        }
    }
};

typedef std::pair<BeeList, bool> Pair;
BeeList resultant_blist;

class Cube {
public:
    int x, y, z;
    Cube(int x, int y, int z)
    {
        this->x = x * 111111;
        this->y = y * 111111;
        this->z = z;
    }
};

std::string find_cube_key(double x, double y, double z)
{
    // round each coordinate, concatenate them into a string and convert this string to an integer
    return ((((std::to_string((int)std::round(x / 57.735026919)) += " ") += std::to_string((int)std::round(y / 57.735026919))) += " ") += std::to_string((int)std::round(z / 57.735026919)));
}

inline void parse_file(std::string input_file, std::vector<std::string>& v, boost::unordered_map<std::string, Pair>& um)
{
    std::ifstream inpp(input_file);
    std::string line;
    std::getline(inpp, line);
    if (inpp.is_open()) {
        Pair* p;
        while (std::getline(inpp, line)) {
            std::vector<std::string> string_coordinates;

            //The data from the file is split and stored in the previously declared vector.
            boost::split(string_coordinates, line, boost::is_any_of(","));

            //A new bee is created using the extracted coordinates.
            Bee* c = new Bee(std::stod(string_coordinates[0]), std::stod(string_coordinates[1]), std::stod(string_coordinates[2]));

            //The coordinates assigned to the generated bee are used to generate a key in order to store said bee in the unordered map.
            std::string key = find_cube_key(c->x, c->y, c->z);
            p = &um[key];
            if ((p->first.size) == 0) {
                p->first.add_first_bee(c);
                p->second = false;
                v.push_back(key);
            } else {
                p->first.add_bee(c);
                p->second = true;
            }
        }
    }
    inpp.close();
}

inline bool compare_adjacent(std::string adj_key, std::string unique_bee_key, boost::unordered_map<std::string, Pair>& cubes, double x, double y, double z)
{

    Bee* current_bee;
    Pair* p;
    BeeList* blist;
    for (int i = 0; i < 26; ++i) {
        p = &cubes[adj_key];
        blist = &p->first;
        if (blist->first != nullptr) {
            current_bee = blist->first;
            double x_distance, y_distance, z_distance;
            while (current_bee != nullptr) {
                x_distance = x - current_bee->x;
                if (x_distance >= -100 && x_distance <= 100) {
                    y_distance = y - current_bee->y;
                    if (y_distance >= -100 && y_distance <= 100) {
                        z_distance = z - current_bee->z;
                        if (z_distance >= -100 && z_distance <= 100) {
                            if (p->second == true) {
                                cubes[unique_bee_key].second = true;
                                resultant_blist.concatenate_beelist_end(&cubes[unique_bee_key].first);
                                return true;
                            } else {
                                p->second = true;
                                cubes[unique_bee_key].second = true;
                                //resultant_blist.concatenate_beelist_end(blist);
                                resultant_blist.concatenate_beelist_end(&cubes[unique_bee_key].first);
                                return true;
                            }
                        }
                    }
                }
                current_bee = current_bee->following;
            }
        }
    }
    return false;
}

void find_for_unique_bee(std::string unique_bee_key, boost::unordered_map<std::string, Pair>& cubes)
{
    Bee* unique_bee = cubes[unique_bee_key].first.first;
    double x = unique_bee->x, y = unique_bee->y, z = unique_bee->z;
    std::vector<std::string> xyz_from_key;
    boost::split(xyz_from_key, unique_bee_key, boost::is_any_of(" "));
    int x_idx = stoi(xyz_from_key[0]), y_idx = stoi(xyz_from_key[1]), z_idx = stoi(xyz_from_key[2]);
    std::vector<std::string> keys;
    keys.reserve(32);
    keys.assign(32, "");

    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    };
    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx + 1) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx + 1)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx + 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx - 1) += " ") += std::to_string(y_idx - 1)) += " ") += std::to_string(z_idx - 1)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx + 2)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx - 2)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx + 2)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx) += " ") += std::to_string(y_idx - 2)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }

    if (compare_adjacent(((((std::to_string(x_idx + 2) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
    if (compare_adjacent(((((std::to_string(x_idx - 2) += " ") += std::to_string(y_idx)) += " ") += std::to_string(z_idx)), unique_bee_key, cubes, x, y, z)) {
        return;
    }
}

BeeList* tony;
double x, y, z;

int main()
{

    std::vector<std::string> files = { "1500beesSet.csv", "150000beesSet.csv   ", "100000beesSet.csv   ", "1000000beesSet.csv  " };
    // string storing name of file that will be parsed
    for (std::string inFileName : files) {
	inFileName = "../datasets/" + inFileName;

        // map trepresent cubes
        boost::unordered_map<std::string, Pair> cubes;

        // vector to store keys and int representing how many bees are in each cube
        std::vector<std::string> keys;
        keys.reserve(500000);

        auto duration_mean = std::chrono::system_clock::duration::zero().count();
        auto duration_max = std::chrono::system_clock::duration::min().count();
        auto duration_min = std::chrono::system_clock::duration::max().count();
        for (int i = 0; i < 100; ++i) {
            auto st = std::chrono::high_resolution_clock::now();
            parse_file(inFileName, keys, cubes);

            auto stop1 = std::chrono::high_resolution_clock::now();
            auto dif = std::chrono::duration_cast<std::chrono::microseconds>(stop1 - st).count();
            if (dif > duration_max) {
                duration_max = dif;
            } else if (dif < duration_min) {
                duration_min = dif;
            }
            duration_mean += dif;
        }
        std::cout << "PARSE_FILE - " << inFileName << " "
                  << "mean: " << duration_mean / 100 << " microseconds" << std::endl;
        std::cout << "PARSE_FILE - " << inFileName << " "
                  << "max: " << duration_max << " microseconds" << std::endl;
        std::cout << "PARSE_FILE - " << inFileName << " "
                  << "min: " << duration_min << " microseconds" << std::endl;

        duration_mean = std::chrono::system_clock::duration::zero().count();
        duration_max = std::chrono::system_clock::duration::min().count();
        duration_min = std::chrono::system_clock::duration::max().count();
        for (int i = 0; i < 100; ++i) {
            auto st = std::chrono::high_resolution_clock::now();
            std::string key;
            Pair* p;
            for (int i = 0; i < keys.size(); i++) {
                key = keys[i];
                p = &cubes[key];
                tony = &p->first;
                if (p->second == true) {
                    resultant_blist.concatenate_beelist_end(tony);
                } else {
                    find_for_unique_bee(key, cubes);
                }
            }
            auto stop1 = std::chrono::high_resolution_clock::now();
            auto dif = std::chrono::duration_cast<std::chrono::microseconds>(stop1 - st).count();
            if (dif > duration_max) {
                duration_max = dif;
            } else if (dif < duration_min) {
                duration_min = dif;
            }
            duration_mean += dif;
        }
        std::cout << "FIND_COLLISIONS - " << inFileName << " "
                  << "mean: " << duration_mean / 100 << " microseconds" << std::endl;
        std::cout << "FIND_COLLISIONS - " << inFileName << " "
                  << "max: " << duration_max << " microseconds" << std::endl;
        std::cout << "FIND_COLLISIONS - " << inFileName << " "
                  << "min: " << duration_min << " microseconds" << std::endl;
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_C180_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_C180_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the value 180 in the chosen type

    @code
    T r = C180<T>();
    @endcode

    is similar to

    @code
    T r = T(180);
    @endcode

    @return The C180 constant for the proper type
  **/
  template<typename T> T C180();

  namespace functional
  {
    /*!
      @ingroup group-constant
      Generate the  constant c180.

      @return The C180 constant for the proper type
    **/
    const boost::dispatch::functor<tag::c180_> c180 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/c180.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#include <iostream>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

using namespace Eigen;
using std::string;

class Controller
{
    private:
        ros::NodeHandle nh;
        ros::Publisher local_pos_pub, vel_pub;
        ros::Subscriber state_sub, target_found_sub, rtag_ekf_sub,rtag_quad_sub, quad_odom_sub;
        ros::Subscriber rtag_ekf_vel_sub, land_permit_sub, true_quad_odom_sub;

        //mavros service clients for arming and setting modes
        ros::ServiceClient arming_client, set_mode_client;

        mavros_msgs::State current_state;
        mavros_msgs::SetMode set_mode;
        mavros_msgs::CommandBool arm_cmd;

        //
        geometry_msgs::PoseStamped pose;

        //apriltag found
        bool target_found;
        bool land_permit;
        //Position vectors
        float r_x; // x tag wrt quad
        float r_y; // y tag wrt quad
        float r_z; // z tag wrt quad -> Don't really need this can use actual quad to true ground
        // Quaternion orientation vectors
        float r_qx;
        float r_qy;
        float r_qz;
        float r_qw;

        //kalman filter estimate
        float kf_x;
        float kf_y;
        float kf_vel_x;
        float kf_vel_y;
        //quad odom

        //Odometry of quad with offset because Airsim does not like to play nice
        float odom_x;
        float odom_y;
        float odom_z;

        float true_odom_z; // this is the true odometry

        float pre_error_x;
        float pre_error_y;

        float pre_ierror_x;
        float pre_ierror_y;

        float kp;
        float ki;
        float kd;

        int decision_case;
        int landing_decision_case;

        //initial pose commands
        float init_x = 0.0;
        float init_y = 3.0;
        float init_z = 4.0;

    public:
        Controller()
        {
            //subscribers
            state_sub = nh.subscribe<mavros_msgs::State>
                    ("mavros/state", 10, &Controller::state_cb, this);
            target_found_sub = nh.subscribe<std_msgs::Bool>
                    ("target_found", 10, &Controller::target_found_cb,this);
            rtag_quad_sub = nh.subscribe<geometry_msgs::PoseStamped>
                    ("tag/pose", 10, &Controller::rtagquad_cb,this);
            rtag_ekf_sub = nh.subscribe<geometry_msgs::PoseStamped>
                    ("kf_tag/pose", 10, &Controller::kftag_cb,this);
            quad_odom_sub = nh.subscribe<geometry_msgs::PoseStamped>
                ("mavros/offset_local_position/pose",15, &Controller::quad_odom_callback, this);
            true_quad_odom_sub = nh.subscribe<geometry_msgs::PoseStamped>
                            ("mavros/local_position/pose",15, &Controller::true_quad_odom_callback, this);
            rtag_ekf_vel_sub = nh.subscribe<geometry_msgs::TwistStamped>
                ("kf_tag/vel", 10, &Controller::kftagvel_cb,this);
            land_permit_sub = nh.subscribe<std_msgs::Bool>
                    ("precland", 10, &Controller::land_permit_cb,this);

            local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
                    ("mavros/setpoint_position/local", 10);
            vel_pub = nh.advertise<geometry_msgs::TwistStamped>
                    ("/mavros/setpoint_velocity/cmd_vel", 10);

            //services
            arming_client = nh.serviceClient<mavros_msgs::CommandBool>
                ("mavros/cmd/arming");
            set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
                    ("mavros/set_mode");

            //the setpoint publishing rate MUST be faster than 2Hz
            ros::Rate rate(20.0);

            // wait for FCU connection
            while(ros::ok() && !current_state.connected){
                ros::spinOnce();
                rate.sleep();
            }

            //send initial points
            pose.pose.position.x = init_x;
            pose.pose.position.y = init_y;
            pose.pose.position.z = init_z;

            //send a few setpoints before starting
            for(int i = 100; ros::ok() && i > 0; --i){
                local_pos_pub.publish(pose);
                ros::spinOnce();
                rate.sleep();
            }

            init_vals();

            set_mode.request.custom_mode = "OFFBOARD";
            arm_cmd.request.value = true;

            ros::Time last_request = ros::Time::now();

            while(ros::ok()){
                if( current_state.mode != "OFFBOARD" &&
                    (ros::Time::now() - last_request > ros::Duration(5.0))){
                    if( set_mode_client.call(set_mode) &&
                        set_mode.response.mode_sent){
                        ROS_INFO("Offboard enabled");
                    }
                    last_request = ros::Time::now();
                } else {
                    if( !current_state.armed &&
                        (ros::Time::now() - last_request > ros::Duration(5.0))){
                        if( arming_client.call(arm_cmd) &&
                            arm_cmd.response.success){
                            ROS_INFO("Vehicle armed");
                        }
                        last_request = ros::Time::now();
                    }
                }

                //set value to case then check case
                check_case();
                //std::cout << "case number:" << decision_case << std::endl;
                switch(decision_case)
                {
                    case 1: // we found the tag
                        go_follow();
                        //ROS_INFO("Follow");
                        break;
                    case 2: //begin landing protocol
                        being_land_protocol();
                        //ROS_INFO("landing");
                        break;
                    case 3: //we don't have the tag
                        go_home();
                        //ROS_INFO("go home");
                        break;
                }

                ros::spinOnce();
                rate.sleep();
            }
        }

    void init_vals()
    {
        //set target found and landing permit to false initially
        target_found = false;
        land_permit = false;

        //rotation matrix
        r_x = 0.0;
        r_y = 0.0;
        r_z = 0.0;
        r_qx = 0.0;
        r_qy = 0.0;
        r_qz = 0.0;
        r_qw = 0.0;

        //kalman filter
        kf_x = 0.0;
        kf_y = 0.0;
        kf_vel_x = 0.0;
        kf_vel_y = 0.0;

        //odometry of quad
        odom_x = 0.0;
        odom_y = 0.0;
        odom_z = 0.0;

        true_odom_z = 0.0;

        //intial error estimates
        pre_error_x = 0.0;
        pre_error_y = 0.0;

        pre_ierror_x = 0.0;
        pre_ierror_y = 0.0;

        //set this into a text/config file
        kp = 0.45;
        ki = 1E-3;
        kd = 0.0;

        //set decision case to go stay where they are at initially
        decision_case = 3;

        landing_decision_case = 4;

    }

    //recieve state of quad
    void state_cb(const mavros_msgs::State::ConstPtr& msg)
    {
        current_state = *msg;
    }

    void target_found_cb(const std_msgs::Bool::ConstPtr& msg)
    {
        target_found = msg->data;
        //std::cout << "target_found:" << target_found << std::endl;
    }

    void land_permit_cb(const std_msgs::Bool::ConstPtr& msg)
    {
        land_permit = msg->data;
        //std::cout << "target_found:" << target_found << std::endl;
    }

    void rtagquad_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        r_z = msg->pose.position.x;
        r_y = msg->pose.position.y;
        r_z = msg->pose.position.z;

        r_qx = msg->pose.orientation.x;
        r_qy = msg->pose.orientation.y;
        r_qz = msg->pose.orientation.z;
        r_qw = msg->pose.orientation.w;

        //Eigen::Vector3d r_pos(r_x, r_y, r_z);
    }

    void kftag_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        kf_x = msg->pose.position.x;
        kf_y = msg->pose.position.y;
    }

    void quad_odom_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        odom_x = msg->pose.position.x;
        odom_y = msg->pose.position.y;
        odom_z = msg->pose.position.z;
        //ROS_INFO("odom_x: %f, odom_y:", odom_x, odom_y);
    }

    void true_quad_odom_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        true_odom_z = msg->pose.position.z;
        //ROS_INFO("odom_x: %f, odom_y:", odom_x, odom_y);
    }

    void kftagvel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
    {
        kf_vel_x = msg->twist.linear.x;
        kf_vel_y = msg->twist.linear.y;
    }

    Eigen::Vector2d calc_PID(float target_x, float target_y, float curr_x, float curr_y, double dt)
    {
        //ROS_INFO("calculating");
        //target x and target y is the position relative to quad
        float error_x = target_x;
        float error_y = target_y;
        //std::cout << "error x " << error_x << "error y" << error_y << std::endl;

        //proportional val
        float Pgain_x = kp * error_x;
        float Pgain_y = kp * error_y;

        //integral error
        float int_error_x = pre_ierror_x + ((error_x + pre_error_x)/ 2) * dt;
        float int_error_y = pre_ierror_y + ((error_y + pre_error_y)/ 2) * dt;
        //integral gain
        float Igain_x = ki * int_error_x;
        float Igain_y = ki * int_error_y;

        //derivative error
        double der_error_x = (error_x - pre_error_x) / dt;
        double der_error_y = (error_y - pre_error_y) / dt;
        float Dgain_x = kd * der_error_x;
        float Dgain_y = kd * der_error_y;

        float PID_x = Pgain_x + Igain_x + Dgain_x;
        float PID_y = Pgain_y + Igain_y + Dgain_y;

        //set gain constraint to prevent the guy from going to crazy
        float gain_constraint = 2.0;
        if (PID_x >= gain_constraint)
            {
                PID_x = gain_constraint;
            }
        else if (PID_y >= gain_constraint)
            {
                PID_y = gain_constraint;
            }

        if (PID_x <= -gain_constraint)
            {
                PID_x = -gain_constraint;
            }
        else if (PID_y <= -gain_constraint)
            {
                PID_y = -gain_constraint;
            }

        //save error
        pre_error_x = error_x;
        pre_error_y = error_y;

        pre_ierror_x = int_error_x;
        pre_ierror_y = int_error_y;

        std::cout << "PID_x" << PID_x << std::endl;
        Eigen::Vector2d PID(PID_x, PID_y);
        return PID;
    }

    void check_case()
    {
        if(target_found==true and land_permit == false){
            decision_case = 1; // we found the tag
        }
        else if (land_permit == true){
            decision_case = 2; //we can land
        }
        else{
            decision_case = 3;
        }
    }

    //Pointer functions -> probably put in a seperate library like how ardupilot does it
    void go_follow()
    {
        ROS_INFO("following");
        Eigen::Vector2d gain = calc_PID(kf_x, kf_y, odom_x, odom_y, 0.1);
        //publish position of tag with kalman fitler
        pose.pose.position.x = odom_x - gain[0];
        pose.pose.position.y = odom_y - gain[1];
        //std::cout << "gain:" << gain << std::endl;
        pose.pose.position.z = 4.0; // just testing the loiter
        local_pos_pub.publish(pose);
        ros::spinOnce();
    }

    // this function is too long
    void being_land_protocol()
    {
        Eigen::Vector2d gain = calc_PID(kf_x, kf_y, odom_x, odom_y, 0.1);
        //publish position of tag with kalman fitler
        std::cout << "gain:" << gain << std::endl;

        check_landing_cases();

        switch(landing_decision_case)
        {
            case 1: //drop slowly to landing target
                pre_land_protocol(gain);
                break;
            case 2:
                land_disarm_protcol();
                break;
            case 3:
                already_landed();
                break;
            case 4:
                pre_land_protocol(gain);
                break;
        }
        local_pos_pub.publish(pose);
    }

    void check_landing_cases()
    {

        float pre_land = 0.5; //this is an offset from the actual height
        float z_land = 0.3; //need to set this as offset
        float tol = 0.15;

        if ((true_odom_z < pre_land) && (true_odom_z > z_land) && (abs(kf_x) < tol) && (abs(kf_y) < tol))
        {
            landing_decision_case = 1; // we're above the prelanding zone so keep dropping down
        }
        else if ((true_odom_z < z_land) && (abs(kf_x) > tol) && (abs(kf_y) > tol))
        {
            landing_decision_case = 2; // we're about to land
        }
        else if (current_state.armed == false && odom_z <= 0.6)
        {
            landing_decision_case = 3; // we lost sight?
        }
        else
        {
            landing_decision_case = 4; // keep dropping
        }
    }

    void pre_land_protocol(Eigen::Vector2d some_gain)
    {
        ROS_INFO("Beginning Prelanding");
        pose.pose.position.x = odom_x - some_gain[0];
        pose.pose.position.y = odom_y - some_gain[1]; // pretty much keep at where we are
        pose.pose.position.z = odom_z - 0.001; //keep it at this general area
    }

    void land_disarm_protcol()
    {
        while(ros::ok())
        {
            ROS_INFO("Beginning Land");
            ros::Rate rate(20.0);

            //set to prelanding hover/ hover to around
            set_mode.request.custom_mode = "AUTO.LAND";
            arm_cmd.request.value = false;

            ros::Time last_request = ros::Time::now();

            //had to set
            while(ros::ok()){
                if( current_state.mode != "AUTO.LAND" &&
                    (ros::Time::now() - last_request > ros::Duration(5.0)))
                    {
                        if( set_mode_client.call(set_mode) &&
                            set_mode.response.mode_sent){
                            ROS_INFO("Landing");
                    }
                    last_request = ros::Time::now();
                } else {
                    if( !current_state.armed &&
                        (ros::Time::now() - last_request > ros::Duration(5.0)))
                        {
                            if( arming_client.call(arm_cmd) &&
                                arm_cmd.response.success){
                                ROS_INFO("Vehicle Disarmed");
                        }
                        last_request = ros::Time::now();
                    }
                }
                ros::spinOnce();
                rate.sleep();
            }
        }
    }

    void already_landed()
    {
        ROS_INFO("I have landed");
    }

    void go_home()
    {
        pose.pose.position.x = init_x;
        pose.pose.position.y = init_y;
        pose.pose.position.z = init_z;
        //std::cout << "no target:" << target_found << std::endl;
        local_pos_pub.publish(pose);
        //printf("I want to go home");
    }

    void go_find()
    {
        //want to keep last time we heard target found if we have it
        printf("I'm lost");
    }

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_tracking");
    Controller control;

    return 0;
}


// Copyright 2015-2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <TH1I.h>
#include <TH2I.h>
#include <TH3I.h>
#include <THn.h>
#include <benchmark/benchmark.h>
#include "generator.hpp"

#include <boost/assert.hpp>
struct assert_check {
  assert_check() {
    BOOST_ASSERT(false); // don't run with asserts enabled
  }
} _;

template <class Distribution>
static void fill_1d(benchmark::State& state) {
  TH1I h("", "", 100, 0, 1);
  generator<Distribution> gen;
  for (auto _ : state) benchmark::DoNotOptimize(h.Fill(gen()));
}

template <class Distribution>
static void fill_2d(benchmark::State& state) {
  TH2I h("", "", 100, 0, 1, 100, 0, 1);
  generator<Distribution> gen;
  for (auto _ : state) benchmark::DoNotOptimize(h.Fill(gen(), gen()));
}

template <class Distribution>
static void fill_3d(benchmark::State& state) {
  TH3I h("", "", 100, 0, 1, 100, 0, 1, 100, 0, 1);
  generator<Distribution> gen;
  for (auto _ : state) benchmark::DoNotOptimize(h.Fill(gen(), gen(), gen()));
}

template <class Distribution>
static void fill_6d(benchmark::State& state) {
  int bin[] = {10, 10, 10, 10, 10, 10};
  double min[] = {0, 0, 0, 0, 0, 0};
  double max[] = {1, 1, 1, 1, 1, 1};
  THnI h("", "", 6, bin, min, max);
  generator<Distribution> gen;
  for (auto _ : state) {
    const double buf[6] = {gen(), gen(), gen(), gen(), gen(), gen()};
    benchmark::DoNotOptimize(h.Fill(buf));
  }
}

BENCHMARK_TEMPLATE(fill_1d, uniform);
BENCHMARK_TEMPLATE(fill_2d, uniform);
BENCHMARK_TEMPLATE(fill_3d, uniform);
BENCHMARK_TEMPLATE(fill_6d, uniform);

BENCHMARK_TEMPLATE(fill_1d, normal);
BENCHMARK_TEMPLATE(fill_2d, normal);
BENCHMARK_TEMPLATE(fill_3d, normal);
BENCHMARK_TEMPLATE(fill_6d, normal);


//
// Created by System Administrator on 2019-08-02.
//

#include "clickhouse_diamonds_t.hpp"

//
// Created by System Administrator on 2019-07-23.
//

#include <iostream>

#include <boost/graph/adjacency_list.hpp>

using namespace clickhouse;

clickhouse_diamonds_t::clickhouse_diamonds_t(const std::string& host) : clickhouse_t(host){

}

clickhouse_diamonds_t::diamonds_t clickhouse_diamonds_t::diamonds(const std::string &table, int round){

    auto ipv4_split = 64;
    uint64_t batch_row_limit = 100000000;
    diamonds_t diamonds;
    diamond_t  current_diamond;
    uint32_t current_prefix = 0;
    for (auto i = 0; i < ipv4_split; ++i) {

//        if (i == 1){
//            break;
//        }

        auto inf_born = static_cast<uint32_t> (i * ((std::pow(2, 32) - 1) / ipv4_split));
        auto sup_born = static_cast<uint32_t >((i + 1) * ((std::pow(2, 32) - 1) / ipv4_split));
        diamonds_recurse(table, round,
                      inf_born, sup_born,
                      batch_row_limit, diamonds, current_diamond, current_prefix
        );
        std::cout << i << " of " << ipv4_split << " IPv4 space done\n";
    }
    return diamonds;
}

void clickhouse_diamonds_t::diamonds_recurse(const std::string & table,  int round,
        // Shrinking recursion arguments,
                                       uint32_t inf_born, uint32_t sup_born,
                                       uint64_t batch_row_limit,
                                       diamonds_t & diamonds,
                                       diamond_t  & current_diamond,
                                       uint32_t   & current_prefix){

    auto sup_born_division = 1;
    auto temporary_sup_born = sup_born;
    uint64_t n_rows = batch_row_limit + 1;
    while (n_rows > batch_row_limit){
        std::string count_query = "SELECT count()\n"
                                  "FROM " + table + "\n"
                                                    "WHERE dst_ip > " + std::to_string(inf_born) + " AND dst_ip <= " + std::to_string(temporary_sup_born);

//        std::cout << count_query << "\n";
        m_client.Select(count_query, [&n_rows](const Block &block) {
            for (size_t k = 0; k < block.GetRowCount(); ++k) {
                n_rows = block[0]->As<ColumnUInt64>()->At(k);
//              std::cout << n_rows << "\n";
            }
        });
        if (n_rows > batch_row_limit){
            temporary_sup_born = (temporary_sup_born - inf_born) / 2 + inf_born;
            sup_born_division += 1;
        }

    }

    for (auto j = 0; j < std::pow(2, sup_born_division-1); ++j) {

        auto inf_born_div = static_cast<uint32_t >(inf_born +
                                                   j * ((sup_born - inf_born) / std::pow(2, sup_born_division - 1)));
        auto sup_born_div = static_cast<uint32_t >(inf_born + (j + 1) * ((sup_born - inf_born) /
                                                                         std::pow(2, sup_born_division - 1)));

        if (sup_born_division > 1) {
            diamonds_recurse(table, round,
                          inf_born_div, sup_born_div,
                          batch_row_limit,
                          diamonds, current_diamond, current_prefix
            );
        } else {
            std::cout << "IPv4 subspace : " << inf_born_div << " AND " << sup_born_div << "\n";
            std::string diamonds_query = "WITH groupUniqArray((reply_ip, ttl, dst_ip, dst_port)) as replies,\n"
                                         "arraySort(x->x.2, replies) as sorted_replies,\n"
                                         "arrayMap(x->x.1, sorted_replies) as ips,\n"
                                         "arrayMap(x->x.2, sorted_replies) as ttls,\n"
                                         "arrayMap(x->x.3, sorted_replies) as dst_ips,\n"
                                         "arrayMap(x->x.4, sorted_replies) as dst_ports\n"
                                         "SELECT dst_prefix, "
                                         "ips,\n"
                                         "ttls,\n"
                                         "dst_ips,\n"
                                         "dst_ports "
                                         "FROM " + table + "\n"
                                         "WHERE dst_ip > " + std::to_string(inf_born_div) + " AND dst_ip <= " + std::to_string(sup_born_div) +
                                         " AND (dst_ip NOT IN \n"
                                         "(\n"
                                         "    SELECT DISTINCT dst_ip\n"
                                         "    FROM \n"
                                         "    (\n"
                                         "        SELECT \n"
                                         "            src_ip, \n"
                                         "            dst_ip, \n"
                                         "            ttl, \n"
                                         "            COUNTDistinct(reply_ip) AS n_ips_per_ttl_flow, \n"
                                         "            COUNT((src_ip, dst_ip, ttl, src_port, dst_port)) AS cnt\n"
                                         "        FROM versioned_probes\n"
                                         "        WHERE dst_ip > " + std::to_string(inf_born_div) + " AND dst_ip <= " + std::to_string(sup_born_div) +
                                         "        GROUP BY (src_ip, dst_ip, ttl, src_port, dst_port, round, snapshot)\n"
                                         "        HAVING (cnt > 2) OR (n_ips_per_ttl_flow > 1)\n"
                                         "    )\n"
                                         "))\n"
                                         "ORDER BY (src_ip, dst_prefix, ttl) ASC";

            m_client.Select(diamonds_query, [&diamonds, &current_diamond, &current_prefix](const Block &block) {
//                std::cout << block.GetRowCount() << " rows in this block\n";
                for (size_t k = 0; k < block.GetRowCount(); ++k) {
                    auto dst_prefix = block[0]->As<ColumnUInt32>()->At(k);

                    auto reply_ips  = block[1]->As<ColumnArray>()->GetAsColumn(k);
                    auto ttls       = block[2]->As<ColumnArray>()->GetAsColumn(k);
                    auto dst_ips    = block[3]->As<ColumnArray>()->GetAsColumn(k);
                    auto dst_ports  = block[4]->As<ColumnArray>()->GetAsColumn(k);

                    if (dst_prefix != current_prefix){
                        // Insert the current diamond to the diamond set.
                        diamonds.insert(current_diamond);
                        current_diamond = diamond_t();
                        current_prefix  = dst_prefix;
                    }

                    for (size_t i = 0; i < reply_ips->Size(); ++i) {
                        uint32_t reply_ip = (*reply_ips->As<ColumnUInt32>())[i];
                        uint8_t  ttl      = (*ttls->As<ColumnUInt8>())[i];
                        uint32_t dst_ip   = (*dst_ips->As<ColumnUInt32>())[i];
                        uint16_t dst_port = (*dst_ports->As<ColumnUInt32>())[i];
                    }

                }
            });
            std::cout << diamonds.size() << " unique diamonds\n";
        }
    }

}


#include <libs/numeric/linear_algebra/test/accumulation.hpp>

#include <boost/timer.hpp>

concept AccurateArithmetic<typename T> {}

template <typename T>
    requires std::Integral<T>
concept_map AccurateArithmetic<T> {}

concept TolerateRoundingErrors<typename T> {}

#define CONSIDER_FLOAT_ROUNDING_ERRORS

# ifndef CONSIDER_FLOAT_ROUNDING_ERRORS
    template <typename T>
        requires math::Float<T>
    concept_map TolerateRoundingErrors<T> {}
# endif

concept_map TolerateRoundingErrors<double> {}

concept SelectiveOperation<typename Operation, typename Element> {}
 // : std::CopyConstructible<std::vector<std::vector<Element> > >

template <typename Element>
concept_map SelectiveOperation<math::min<Element>, Element> {}

template <typename Element>
concept_map SelectiveOperation<math::max<Element>, Element> {}

concept RegularReduction<typename Operation, typename Element> {} // : std::Integral<float> {}

template <typename Operation, typename Element>
  requires AccurateArithmetic<Element>
concept_map RegularReduction<Operation, Element> {}

template <typename Operation, typename Element>
  requires TolerateRoundingErrors<Element>
concept_map RegularReduction<Operation, Element> {}

template <typename Operation, typename Element>
  requires SelectiveOperation<Operation, Element>
        && !AccurateArithmetic<Element>
        && !TolerateRoundingErrors<Element>
concept_map RegularReduction<Operation, Element> {}

// ######################################################################

namespace mtl {

// Dispatching between simple and unrolled version
template <typename Iter, typename Value, typename Op>
  requires std::ForwardIterator<Iter>
                  && std::Convertible<Value, std::ForwardIterator<Iter>::value_type>
                  && math::Magma<Op, std::ForwardIterator<Iter>::value_type>
                  && RegularReduction<Op, std::ForwardIterator<Iter>::value_type>
typename std::ForwardIterator<Iter>::value_type
inline my_accumulate(Iter first, Iter last, Value init, Op op)
{
    std::cout << "Simple accumulate\n";
    return mtl::accumulate_simple(first, last, init, op);
}

template <typename Iter, typename Value, typename Op>
    requires  std::RandomAccessIterator<Iter>
	          && std::Convertible<Value, std::RandomAccessIterator<Iter>::value_type>
		  && math::Monoid<Op, std::RandomAccessIterator<Iter>::value_type>
		  && math::Commutative<Op, std::RandomAccessIterator<Iter>::value_type>
                  && RegularReduction<Op, std::RandomAccessIterator<Iter>::value_type>
typename std::RandomAccessIterator<Iter>::value_type
inline my_accumulate(Iter first, Iter last, Value init, Op op)
{
    std::cout << "Unrolled accumulate\n";
    return mtl::accumulate_unrolled(first, last, init, op);
}

// Special Treatment
template <typename Iter, typename Value, typename Op>
  requires std::ForwardIterator<Iter>
                  && std::Convertible<Value, std::ForwardIterator<Iter>::value_type>
                  && math::Magma<Op, std::ForwardIterator<Iter>::value_type>
typename std::ForwardIterator<Iter>::value_type
inline my_accumulate(Iter first, Iter last, Value init, Op op)
{
    std::cout << "Special accumulate\n";
    return mtl::accumulate_simple(first, last, init, op);
}

} // namespace mtl

// ######################################################################

using math::identity; using math::add;

const int   array_size= 10;

template <typename Element>
void test_accumulate(const char* name)
{
    Element     array[array_size];
    for (int i= 0; i < array_size; i++)
	array[i]= Element(i);

    std::cout << '\n' << name << '\n' << " Add: ";
    mtl::my_accumulate(&array[0], array+array_size, Element(0), math::add<Element>());
    std::cout << "Mult: ";
    mtl::my_accumulate(array, array+array_size, Element(1), math::mult<Element>());
    std::cout << " Min: ";
    mtl::my_accumulate(array, array+array_size, Element(1), math::min<Element>());
    std::cout << " Max: ";
    mtl::my_accumulate(array, array+array_size, Element(1), math::max<Element>());
}

int main(int, char* [])
{
    test_accumulate<int>("int");
    test_accumulate<float>("float");
    test_accumulate<double>("double");

    return 0;
}


/*
  (c) Copyright 2003-2007, Hewlett-Packard Development Company, LP

  See the file named COPYING for license details
*/

#include <string>
#include <list>

#include <boost/format.hpp>

#include <Lintel/ConstantString.hpp>
#include <Lintel/HashMap.hpp>
#include <Lintel/StatsQuantile.hpp>
#include <Lintel/StringUtil.hpp>

#include "analysis/nfs/mod1.hpp"

using namespace std;
using boost::format;

class NFSOpPayload : public NFSDSModule {
  public:
    NFSOpPayload(DataSeriesModule &_source)
    : source(_source), s(ExtentSeries::typeExact),
      payload_length(s,"payload-length"),
      is_udp(s,"is-udp"),
      is_request(s,"is-request"),
      op_id(s,"op-id",Field::flag_nullable),
      operation(s,"operation")
    {}
    virtual ~NFSOpPayload() { }
    struct hteData {
        bool is_udp, is_request;
        int op_id;
        hteData(bool a, bool b, int c) : is_udp(a), is_request(b), op_id(c), payload_length(NULL) {}
        Stats *payload_length;
        string operation;
    };

    class hteHash {
      public:
        unsigned int operator()(const hteData &k) const {
            return (k.is_udp ? 256 : 0) + (k.is_request ? 512 : 0) + k.op_id;
        }
    };

    class hteEqual {
      public:
        bool operator()(const hteData &a, const hteData &b) const {
            return a.is_udp == b.is_udp && a.is_request == b.is_request &&
                    a.op_id == b.op_id;
        }
    };
    HashTable<hteData, hteHash, hteEqual> stats_table;

    virtual Extent::Ptr getSharedExtent() {
        Extent::Ptr e = source.getSharedExtent();
        if (e == NULL)
            return e;
        if (e->getTypePtr()->getName() != "NFS trace: common")
            return e;
        for (s.setExtent(e);s.morerecords();++s) {
            if (op_id.isNull())
                continue;
            hteData *d = stats_table.lookup(hteData(is_udp.val(),is_request.val(),op_id.val()));
            if (d == NULL) {
                hteData newd(is_udp.val(),is_request.val(),op_id.val());
                newd.payload_length = new Stats;
                newd.operation = operation.stringval();
                stats_table.add(newd);
                d = stats_table.lookup(newd);
            }
            d->payload_length->add(payload_length.val());
        }
        return e;
    }

    class sortByTotal {
      public:
        bool operator()(hteData *a, hteData *b) const {
            return a->payload_length->total() > b->payload_length->total();
        }
    };

    virtual void printResult() {
        printf("Begin-%s\n",__PRETTY_FUNCTION__);
        vector<hteData *> vals;
        for (HashTable<hteData, hteHash, hteEqual>::iterator i = stats_table.begin();
            i != stats_table.end();++i) {
            vals.push_back(&(*i));
        }
        sort(vals.begin(),vals.end(),sortByTotal());
        for (vector<hteData *>::iterator i = vals.begin();
            i != vals.end();++i) {
            hteData *j = *i;
            printf("%s %12s %7s: %7.3f MB, %6.2f bytes/op, %4.0f max bytes/op, %6ld ops\n",
                   j->is_udp ? "UDP" : "TCP", j->operation.c_str(),
                   j->is_request ? "Request" : "Reply",
                   j->payload_length->total()/(1024*1024.0),
                   j->payload_length->mean(),
                   j->payload_length->max(),
                   j->payload_length->count());
        }
        printf("End-%s\n",__PRETTY_FUNCTION__);
    }
    DataSeriesModule &source;
    ExtentSeries s;
    Int32Field payload_length;
    BoolField is_udp;
    BoolField is_request;
    ByteField op_id;
    Variable32Field operation;
};

NFSDSModule *
NFSDSAnalysisMod::newNFSOpPayload(DataSeriesModule &prev)
{
    return new NFSOpPayload(prev);
}

class ClientServerPairInfo : public NFSDSModule {
  public:
    ClientServerPairInfo(DataSeriesModule &_source)
    : source(_source), s(ExtentSeries::typeExact),
      payload_length(s,"payload-length"),
      is_udp(s,"is-udp"),is_request(s,"is-request"),
      op_id(s,"op-id",Field::flag_nullable),
      sourceip(s,"source"),destip(s,"dest")
    {}
    virtual ~ClientServerPairInfo() { }
    struct hteData {
        bool is_udp;
        ExtentType::int32 client, server;
        hteData(bool a, ExtentType::int32 b, ExtentType::int32 c)
                : is_udp(a), client(b), server(c),payload_length(NULL) {}
        Stats *payload_length;
    };

    class hteHash {
      public:
        unsigned int operator()(const hteData &k) const {
            return lintel::BobJenkinsHashMix3(k.client,k.server,k.is_udp ? 0x55555555 : 0x0);
        }
    };

    class hteEqual {
      public:
        bool operator()(const hteData &a, const hteData &b) const {
            return a.is_udp == b.is_udp && a.client == b.client &&
                    a.server == b.server;
        }
    };

    HashTable<hteData, hteHash, hteEqual> stats_table;

    virtual Extent::Ptr getSharedExtent() {
        Extent::Ptr e = source.getSharedExtent();
        if (e == NULL)
            return e;
        if (e->getTypePtr()->getName() != "NFS trace: common")
            return e;
        for (s.setExtent(e);s.morerecords();++s) {
            if (op_id.isNull())
                continue;
            ExtentType::int32 client,server;
            if (is_request.val()) {
                client = sourceip.val();
                server = destip.val();
            } else {
                client = destip.val();
                server = sourceip.val();
            }
            hteData *d = stats_table.lookup(hteData(is_udp.val(),client,server));
            if (d == NULL) {
                hteData newd(is_udp.val(),client,server);
                newd.payload_length = new Stats;
                stats_table.add(newd);
                d = stats_table.lookup(newd);
            }
            d->payload_length->add(payload_length.val());
        }
        return e;
    }

    class sortByTotal {
      public:
        bool operator()(hteData *a, hteData *b) const {
            return a->payload_length->total() > b->payload_length->total();
        }
    };

    virtual void printResult() {
        printf("Begin-%s\n",__PRETTY_FUNCTION__);
        printf("protocol client server: total data ...\n");
        vector<hteData *> vals;
        for (HashTable<hteData, hteHash, hteEqual>::iterator i = stats_table.begin();
            i != stats_table.end();++i) {
            vals.push_back(&(*i));
        }
        sort(vals.begin(),vals.end(),sortByTotal());
        int count = 0;
        for (vector<hteData *>::iterator i = vals.begin();
            i != vals.end();++i) {
            hteData *j = *i;
            printf("%s %12s %12s: %.3f MB, %.2f bytes/op, %.0f max bytes/op, %ld ops\n",
                   j->is_udp ? "UDP" : "TCP",
                   ipv4tostring(j->client).c_str(),
                   ipv4tostring(j->server).c_str(),
                   j->payload_length->total()/(1024.0*1024.0),
                   j->payload_length->mean(),
                   j->payload_length->max(),
                   j->payload_length->count());
            if (++count > 100) break;
        }
        printf("End-%s\n",__PRETTY_FUNCTION__);
    }
    DataSeriesModule &source;
    ExtentSeries s;
    Int32Field payload_length;
    BoolField is_udp,is_request;
    ByteField op_id;
    Int32Field sourceip, destip;
};

NFSDSModule *
NFSDSAnalysisMod::newClientServerPairInfo(DataSeriesModule &prev)
{
    return new ClientServerPairInfo(prev);
}

class PayloadInfo : public NFSDSModule {
  public:
    PayloadInfo(DataSeriesModule &_source)
    : source(_source), s(ExtentSeries::typeExact),
      packet_at(s,"packet-at"),
      payload_length(s,"payload-length"),
      is_udp(s,"is-udp"),
      op_id(s,"op-id",Field::flag_nullable),
      min_time(numeric_limits<int64_t>::max()),
      max_time(numeric_limits<int64_t>::min()) {
        FATAL_ERROR("need to fix time code");
    }
    virtual ~PayloadInfo() {}

    virtual Extent::Ptr getSharedExtent() {
        Extent::Ptr e = source.getSharedExtent();
        if (e == NULL) return e;
        if (e->getTypePtr()->getName() != "NFS trace: common") return e;
        for (s.setExtent(e);s.morerecords();++s) {
            if (op_id.isNull()) continue;
            stat_payload_length.add(payload_length.val());
            if (packet_at.val() < min_time) {
                min_time = packet_at.val();
            }
            if (packet_at.val() > max_time) {
                max_time = packet_at.val();
            }
            if (is_udp.val()) {
                stat_payload_length_udp.add(payload_length.val());
            } else {
                stat_payload_length_tcp.add(payload_length.val());
            }
        }
        return e;
    }
    virtual void printResult() {
        printf("Begin-%s\n",__PRETTY_FUNCTION__);
        const double oneb = (1000.0*1000.0*1000.0);
        double time_range = (double)(max_time - min_time)/oneb;
        printf("time range: %.2fs .. %.2fs = %.2fs, %.2fhours\n",
               (double)min_time/oneb, (double)max_time/oneb, time_range,
               time_range / (3600.0));
        printf("payload_length: avg = %.2f bytes, stddev = %.4f bytes, sum = %.4f GB, %.4fMB/s, count=%.2f million %.2f k/s\n",
               stat_payload_length.mean(),
               stat_payload_length.stddev(),
               stat_payload_length.total()/(1024.0*1024.0*1024.0),
               stat_payload_length.total()/(time_range * 1024.0*1024.0),
               (double)stat_payload_length.count()/(1000000.0),
               (double)stat_payload_length.count()/(time_range * 1000.0));

        printf("payload_length(udp): avg = %.2f bytes, stddev = %.4f bytes, sum = %.4f GB, %.4fMB/s, count=%.2f million %.2f k/s\n",
               stat_payload_length_udp.mean(),
               stat_payload_length_udp.stddev(),
               stat_payload_length_udp.total()/(1024.0*1024.0*1024.0),
               stat_payload_length_udp.total()/(time_range * 1024.0*1024.0),
               (double)stat_payload_length_udp.count()/(1000000.0),
               (double)stat_payload_length_udp.count()/(time_range * 1000.0));

        printf("payload_length(tcp): avg = %.2f bytes, stddev = %.4f bytes, sum = %.4f GB, %.4fMB/s, count=%.2f million %.2f k/s\n",
               stat_payload_length_tcp.mean(),
               stat_payload_length_tcp.stddev(),
               stat_payload_length_tcp.total()/(1024.0*1024.0*1024.0),
               stat_payload_length_tcp.total()/(time_range * 1024.0*1024.0),
               (double)stat_payload_length_tcp.count()/(1000000.0),
               (double)stat_payload_length_tcp.count()/(time_range * 1000.0));
        printf("End-%s\n",__PRETTY_FUNCTION__);
    }

    DataSeriesModule &source;
    ExtentSeries s;
    Int64Field packet_at;
    Int32Field payload_length;
    BoolField is_udp;
    ByteField op_id;
    Stats stat_payload_length, stat_payload_length_udp, stat_payload_length_tcp;
    int64_t min_time, max_time;
};

NFSDSModule *
NFSDSAnalysisMod::newPayloadInfo(DataSeriesModule &prev)
{
    return new PayloadInfo(prev);
}

class UnbalancedOps : public NFSDSModule {
  public:
    UnbalancedOps(DataSeriesModule &_source)
    : source(_source), s(ExtentSeries::typeExact),
      reqtime(s,"packet-at"),
      sourceip(s,"source"),destip(s,"dest"),
      is_udp(s,"is-udp"),
      is_request(s,"is-request"),
      transaction_id(s, "transaction-id"),
      op_id(s,"op-id",Field::flag_nullable),
      operation(s,"operation"),
      request_count(0),
      lastrotate(0),
      rotate_interval_ns((ExtentType::int64)3600*1000*1000*1000), // 1 hour
      missing_response_rotated_count(0)
    {
        pending_prev = new HashTable<tidData, tidHash, tidEqual>();
        pending_cur = new HashTable<tidData, tidHash, tidEqual>();
    }

    virtual ~UnbalancedOps() { }

    static const bool print_detail = false;

    DataSeriesModule &source;
    ExtentSeries s;
    Int64Field reqtime;
    Int32Field sourceip, destip;
    BoolField is_udp;
    BoolField is_request;
    Int32Field transaction_id;
    ByteField op_id;
    Variable32Field operation;

    long long request_count;

    // data structure to keep requests that do not yet have a matching response
    struct tidData {
        unsigned clientip, serverip;
        int is_udp;
        unsigned transaction_id;
        int op_id;
        string operation;
        long long reqtime;

        tidData() { }
        tidData(unsigned tid_in, unsigned c_in, unsigned s_in)
                : clientip(c_in), serverip(s_in), is_udp(0),
                  transaction_id(tid_in), op_id(0),
                  reqtime(0) {}
    };
    class tidHash {
      public: unsigned int operator()(const tidData &t) const {
          return lintel::BobJenkinsHashMix3(t.transaction_id,t.clientip,2004); // transaction id assigned by client
      }};
    class tidEqual {
      public: bool operator()(const tidData &t1, const tidData &t2) const {
          return t1.transaction_id == t2.transaction_id && t1.clientip == t2.clientip;
      }};

    // rotated hash tables to prevent requests with missing replies
    // from accumulating forever.
    HashTable<tidData, tidHash, tidEqual> *pending_prev, *pending_cur;
    ExtentType::int64 lastrotate;
    const ExtentType::int64 rotate_interval_ns;
    ExtentType::int64 missing_response_rotated_count;
    vector<tidData *> retransmit;
    vector<tidData *> duplicateresponse;

    virtual Extent::Ptr getSharedExtent() {
        Extent::Ptr e = source.getSharedExtent();
        if (e == NULL)
            return e;
        if (e->getTypePtr()->getName() != "NFS trace: common")
            return e;
        for (s.setExtent(e);s.morerecords();++s) {
            if (op_id.isNull())
                continue;
            if ((reqtime.val() - lastrotate) > rotate_interval_ns) {
                missing_response_rotated_count += pending_prev->size();
                delete pending_prev;
                pending_prev = pending_cur;
                pending_cur = new HashTable<tidData, tidHash, tidEqual>();
                lastrotate = reqtime.val();
            }
            if (is_request.val()) {
                ++request_count;
                tidData dummy(transaction_id.val(), sourceip.val(), // request source is client
                              destip.val());

                tidData *t = pending_cur->lookup(dummy); // expect to find (if at all) in most recent
                if (t == NULL) {
                    t = pending_prev->lookup(dummy); // have to check both for correctness
                }
                if (t == NULL) {
                    // add request to list of pending requests
                    dummy.reqtime = reqtime.val();
                    dummy.is_udp = is_udp.val();
                    dummy.op_id = op_id.val();
                    dummy.operation = operation.stringval();
                    t = pending_cur->add(dummy);
                } else {
                    // add request to list of retransmitted requests
                    tidData *tcopy = new tidData(*t);
                    retransmit.push_back(tcopy);
                }
            } else { // request is a response
                tidData dummy(transaction_id.val(), destip.val(), sourceip.val()); // response source is server
                tidData *t = pending_cur->lookup(dummy);
                if (t != NULL) {
                    pending_cur->remove(*t);
                } else {
                    t = pending_prev->lookup(dummy);
                    if (t != NULL) {
                        pending_prev->remove(*t);
                    } else {
                        t = new tidData(dummy);
                        t->reqtime = reqtime.val();
                        t->is_udp = is_udp.val();
                        t->op_id = op_id.val();
                        t->operation = operation.stringval();
                        duplicateresponse.push_back(t);
                    }
                }
            }
        }
        return e;
    }

    class sortByTime {
      public: bool operator()(tidData *a, tidData *b) const {
          return a->reqtime < b->reqtime;
      }};

    virtual void printResult() {
        printf("Begin-%s\n",__PRETTY_FUNCTION__);

        vector<tidData *> vals;
        for (HashTable<tidData, tidHash, tidEqual>::iterator i =
                    pending_cur->begin();
            i != pending_cur->end(); ++i) {
            vals.push_back(&(*i));
        }
        for (HashTable<tidData, tidHash, tidEqual>::iterator i =
                    pending_prev->begin();
            i != pending_prev->end(); ++i) {
            vals.push_back(&(*i));
        }
        sort(vals.begin(),vals.end(),sortByTime());
        cout << format("Summary: %lld/%lld (%.2f%%) requests without responses, %d shown\n")
                % (missing_response_rotated_count + vals.size())
                % request_count
                % ((double)(missing_response_rotated_count + vals.size())/(double)request_count)
                % vals.size();
        if (print_detail) {
            printf("%-13s %-15s %-15s %-4s %-8s %-2s %-12s\n",
                   "time",
                   "client",
                   "server",
                   "prot",
                   "xid",
                   "op",
                   "operation");

            printf("%-13s %-15s %-15s %-4s %-8s %-2s %-12s\n",
                   "-------------", "---------------", "---------------",
                   "----", "--------", "--", "------------");
            for (vector<tidData *>::iterator i = vals.begin();
                i != vals.end();++i) {
                tidData *j = *i;
                printf("%13lld %-15s %-15s %-4s %08x %2d %-12s\n",
                       j->reqtime / (1000000),
                       ipv4tostring(j->clientip).c_str(),
                       ipv4tostring(j->serverip).c_str(),
                       j->is_udp ? "UDP" : "TCP",
                       j->transaction_id,
                       j->op_id,
                       j->operation.c_str());
            }
            printf("\n");
        }

        cout << format("Summary: %d/%lld duplicate requests\n")
                % retransmit.size() % request_count;
        if (print_detail) {
            printf("%-13s %-15s %-15s %-4s %-8s %-2s %-12s\n",
                   "time", "client", "server", "prot", "xid", "op", "operation");
            printf("%-13s %-15s %-15s %-4s %-8s %-2s %-12s\n",
                   "-------------", "---------------", "---------------",
                   "----", "--------", "--", "------------");
            for (vector<tidData *>::iterator p = retransmit.begin();
                p != retransmit.end();++p) {
                tidData *j = *p;
                printf("%13lld %-15s %-15s %-4s %08x %2d %-12s\n",
                       j->reqtime / (1000000),
                       ipv4tostring(j->clientip).c_str(),
                       ipv4tostring(j->serverip).c_str(),
                       j->is_udp ? "UDP" : "TCP",
                       j->transaction_id,
                       j->op_id,
                       j->operation.c_str());
            }
            printf("\n");
        }

        cout << format("Summary: %d duplicate responses\n") % duplicateresponse.size();
        if (print_detail) {
            printf("%-13s %-15s %-15s %-4s %-8s %-2s %-12s\n",
                   "time", "client", "server", "prot", "xid", "op", "operation");
            printf("%-13s %-15s %-15s %-4s %-8s %-2s %-12s\n",
                   "-------------", "---------------", "---------------",
                   "----", "--------", "--", "------------");
            for (vector<tidData *>::iterator p = duplicateresponse.begin();
                p != duplicateresponse.end();++p) {
                tidData *j = *p;
                printf("%13lld %-15s %-15s %-4s %08x %2d %-12s\n",
                       j->reqtime / (1000000),
                       ipv4tostring(j->clientip).c_str(),
                       ipv4tostring(j->serverip).c_str(),
                       j->is_udp ? "UDP" : "TCP",
                       j->transaction_id,
                       j->op_id,
                       j->operation.c_str());
            }
        }

        printf("End-%s\n",__PRETTY_FUNCTION__);
    }
};

NFSDSModule *
NFSDSAnalysisMod::newUnbalancedOps(DataSeriesModule &prev)
{
    return new UnbalancedOps(prev);
}

double NFSDSAnalysisMod::gap_parm = 5000.0; // in ms

class NFSTimeGaps : public NFSDSModule {
  public:
    NFSTimeGaps(DataSeriesModule &_source)
    : source(_source), s(ExtentSeries::typeExact),
      reqtime(s,"packet-at"), currstart(0), last(0)
    {}

    virtual ~NFSTimeGaps() { }

    DataSeriesModule &source;
    ExtentSeries s;
    Int64Field reqtime;

    // data structure to keep requests that do not yet have a matching response
    struct RangeData {
        long long start, end;
        RangeData(unsigned long long s_in, unsigned long long e_in) :
                start(s_in), end(e_in) {}
    };
    list<RangeData> ranges;
    long long currstart, last, curr;
    long long starttime;

    virtual Extent::Ptr getSharedExtent() {

        Extent::Ptr e = source.getSharedExtent();
        if (e == NULL)
            return e;
        if (e->getTypePtr()->getName() != "NFS trace: common")
            return e;
        for (s.setExtent(e);s.morerecords();++s) {
            curr = reqtime.val();
            if (currstart == 0) {
                starttime = curr;
                currstart = curr;
            }
            else
            {
                if ((double) (curr - last) / 1000000 > NFSDSAnalysisMod::gap_parm)
                {
                    // new range found
                    ranges.push_back(RangeData(currstart, last));
                    currstart = curr;
                }
            }
            last = curr;
        }
        return e;
    }

    virtual void printResult() {
        long long next;

        // This code should be executed after all extents are processed.
        // This is probably not the best place to put it.
        if (currstart != 0)
        {
            ranges.push_back(RangeData(currstart, last));
            currstart = 0;
        }
        printf("Begin-%s\n",__PRETTY_FUNCTION__);
        printf("%-17s %-17s %-15s %-15s %-8s\n", "start", "end", "start from beg", "end from beg", "gap");
        printf("%-17s %-17s %-15s %-15s %-8s\n", "-----------------", "-----------------",
               "---------------", "---------------", "--------");

        for (list<RangeData>::iterator p = ranges.begin();
             p != ranges.end();
             )
        {
            RangeData j = *p++;
            if (p != ranges.end())
                next = p->start;
            else
                next = j.end;

            printf("%17.3f %17.3f %15.3f %15.3f %8.3f\n",
                   (double) j.start / 1000000,
                   (double) j.end / 1000000,
                   (double) (j.start - starttime) / 1000000,
                   (double) (j.end - starttime) / 1000000,
                   (double) (next - j.end) / 1000000);
        }

        printf("End-%s\n",__PRETTY_FUNCTION__);
    }
};

NFSDSModule *
NFSDSAnalysisMod::newNFSTimeGaps(DataSeriesModule &prev)
{
    return new NFSTimeGaps(prev);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <boost/functional/hash.hpp>  // IWYU pragma: keep
#include <cstddef>
#include <memory>
#include <pup.h>
#include <string>
#include <utility>

#include "DataStructures/DataBox/DataBox.hpp"
#include "DataStructures/DataBox/DataBoxTag.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/CoordinateMaps/Affine.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.hpp"
#include "Domain/CoordinateMaps/ProductMaps.hpp"
#include "Domain/Element.hpp"
#include "Domain/ElementId.hpp"
#include "Domain/ElementIndex.hpp"
#include "Domain/ElementMap.hpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/Mesh.hpp"
#include "Domain/SizeOfElement.hpp"  // IWYU pragma: keep
#include "Domain/Tags.hpp"
#include "Evolution/DiscontinuousGalerkin/SlopeLimiters/LimiterActions.hpp"  // IWYU pragma: keep
#include "Evolution/DiscontinuousGalerkin/SlopeLimiters/Minmod.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"
#include "tests/Unit/ActionTesting.hpp"

namespace {
struct TemporalId : db::SimpleTag {
  static std::string name() noexcept { return "TemporalId"; }
  using type = int;
};

struct Var : db::SimpleTag {
  static std::string name() noexcept { return "Var"; }
  using type = Scalar<DataVector>;
};

template <size_t Dim>
struct System {
  static constexpr const size_t volume_dim = Dim;
  using variables_tag = Tags::Variables<tmpl::list<Var>>;
};

struct LimiterTag {
  using type = SlopeLimiters::Minmod<2, tmpl::list<Var>>;
};

template <size_t Dim, typename Metavariables>
struct component {
  using metavariables = Metavariables;
  using chare_type = ActionTesting::MockArrayChare;
  using array_index = ElementIndex<Dim>;
  using const_global_cache_tag_list = tmpl::list<LimiterTag>;
  using action_list =
      tmpl::list<SlopeLimiters::Actions::SendData<Metavariables>,
                 SlopeLimiters::Actions::Limit<Metavariables>>;
  using simple_tags =
      db::AddSimpleTags<TemporalId, Tags::Mesh<Dim>, Tags::Element<Dim>,
                        Tags::ElementMap<Dim>,
                        Tags::Coordinates<Dim, Frame::Logical>,
                        Tags::Coordinates<Dim, Frame::Inertial>, Var>;
  using compute_tags = db::AddComputeTags<Tags::SizeOfElement<Dim>>;
  using initial_databox =
      db::compute_databox_type<tmpl::append<simple_tags, compute_tags>>;
};

template <size_t Dim>
struct Metavariables {
  using component_list = tmpl::list<component<Dim, Metavariables>>;
  using const_global_cache_tag_list = tmpl::list<>;
  using limiter = LimiterTag;
  using system = System<Dim>;
  using temporal_id = TemporalId;
  static constexpr bool local_time_stepping = false;
};
}  // namespace

// This test checks that the Minmod limiter's interfaces and type aliases
// succesfully integrate with the limiter actions. It does this by compiling
// together the Minmod limiter and the actions, then making calls to the
// SendData and the Limit actions. No checks are performed here that the limiter
// and/or actions produce correct output: that is done in other tests.
SPECTRE_TEST_CASE("Unit.Evolution.DG.SlopeLimiters.LimiterActions.Minmod",
                  "[Unit][NumericalAlgorithms][Actions]") {
  using metavariables = Metavariables<2>;
  using my_component = component<2, metavariables>;

  const Mesh<2> mesh{3, Spectral::Basis::Legendre,
                     Spectral::Quadrature::GaussLobatto};
  const ElementId<2> self_id(1, {{{2, 0}, {1, 0}}});
  const Element<2> element(self_id, {});

  const CoordinateMaps::Affine xi_map{-1., 1., 3., 7.};
  const CoordinateMaps::Affine eta_map{-1., 1., 7., 3.};
  auto map = ElementMap<2, Frame::Inertial>(
      self_id, make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
                   CoordinateMaps::ProductOf2Maps<CoordinateMaps::Affine,
                                                  CoordinateMaps::Affine>(
                       xi_map, eta_map)));

  auto logical_coords = logical_coordinates(mesh);
  auto inertial_coords = map(logical_coords);
  auto var = Scalar<DataVector>(mesh.number_of_grid_points(), 1234.);

  using MockRuntimeSystem = ActionTesting::MockRuntimeSystem<metavariables>;
  using MockDistributedObjectsTag =
      MockRuntimeSystem::MockDistributedObjectsTag<my_component>;
  MockRuntimeSystem::TupleOfMockDistributedObjects dist_objects{};
  tuples::get<MockDistributedObjectsTag>(dist_objects)
      .emplace(
          self_id,
          db::create<my_component::simple_tags, my_component::compute_tags>(
              0, mesh, element, std::move(map), std::move(logical_coords),
              std::move(inertial_coords), std::move(var)));

  ActionTesting::MockRuntimeSystem<metavariables> runner{
      SlopeLimiters::Minmod<2, tmpl::list<Var>>(
          SlopeLimiters::MinmodType::LambdaPi1),
      std::move(dist_objects)};

  // SendData
  runner.next_action<my_component>(self_id);
  CHECK(runner.is_ready<my_component>(self_id));
  // Limit
  runner.next_action<my_component>(self_id);
}


/**
 * @Copyright 2021, Wuhan University of Technology
 * @Author: Pengwei Zhou
 * @Date: 2021/2/14 上午10:47
 * @FileName: segmentation.hpp
 * @Description: Fast and precise point cloud segmentation including multi-region ground extraction and dynamic curved-voxel clustering
 * @License: See LICENSE for the license information
 */
#ifndef TLOAM_SEGMENTATION_HPP
#define TLOAM_SEGMENTATION_HPP

// for std
#include <iostream>
#include <cmath>
#include <vector>
#include <thread>
#include <mutex>
#include <future>
#include <utility>
#include <algorithm>
// for Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
// for ros
#include <omp.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <yaml-cpp/yaml.h>
// for Open3D
#include <open3d/Open3D.h>
#include "tloam/open3d/PointCloud2.hpp"
#include "tloam/open3d/open3d_to_ros.hpp"
// for cloud data
#include "tloam/models/utils/sensor_data.hpp"
// for subscriber
#include "tloam/subscriber/cloud_subscriber.hpp"
// for publisher
#include "tloam/publisher/boundingbox_publisher.hpp"
#include "tloam/publisher/cloud_publisher.hpp"
// for listener
#include "tloam/subscriber/tf_listener.hpp"
// for OpenMp
#include <omp.h>
// for ros
#include <ros/ros.h>

#include "tloam/models/utils/utils.hpp"
#include "tloam/models/utils/work_space_path.h"

namespace tloam{
class Segmentation{
public:
  enum LiDAR{
    HDL_64E = 64,
    VLP_16 = 16
  };
  Segmentation() = default;
  Segmentation(ros::NodeHandle& nh, std::string& topic_name);
  ~Segmentation();

  void spinOnce();

  bool initWithConfig();

protected:

  void initVeldyneConfig(const YAML::Node& node);

  void initGroundSegConfig(const YAML::Node& node);

  void initDCVCConfig(const YAML::Node& node);

  bool readData();

  bool hasData();

  bool validData();

  /**
   * @brief initialize ring radius table and section bounds
   * @param void
   * @return true if success otherwise false
   */
  bool initSections(void);

  /**
   * @brief determine the section number according to the polar diameter.
   * @param radius, polar diameter data of the point cloud
   * @return section number
   */
  int getSection(const double& radius);

  /**
   * @brief
   * @param cloud_in_, the point cloud to be estimated
   * @param cloud_out_, output the beams and collected time information of each point and stored in the intensity channel.
   * @return true if success otherwise false
   */
  bool estimateRingsAndTimes(
    CloudData& cloud_in_,
    CloudData& cloud_out_
  ) const;

  /**
   * @brief estimation of LiDAR beam and time information
   * @param cloud_in_, input point cloud
   * @return mean_height of point cloud
   */
  double estimateRingsAndTimes2(
    CloudData& cloud_in_
  ) const;

  /**
   * @brief filter out part of the vertical point cloud according to the height value
   * @param cloud_in_
   * @param mean_height_
   * @param cloud_out_
   * @return true if success otherwise false
   */
  bool filterByHeight(
    CloudData& cloud_in_,
    double& mean_height_,
    CloudData& cloud_out_
  );

  /**
   * @brief Remove some close points and invalid points
   * @param cloud_in_
   * @param dis_th
   * @param remove_nan
   * @param remove_infinite
   */
  void RemoveClosedNonFinitePoints(
     CloudData& cloud_in_,
     double dis_th = 3.0,
     bool remove_nan = true,
     bool remove_infinite = true
 );

  /**
   * @brief calculate the region index of each point
   * @param cloud_in_, input point cloud
   * @return true if success otherwise false
   */
  bool fillSectionIndex(CloudData& cloud_in_);

  /**
   * @brief Find best plane based on matrix-axis linear regression
   * @param cloud_in_, input point cloud data
   * @param out_plane_, output plane model
   * @return void
   */
  void findBestPlane(CloudData& cloud_in_, Eigen::Vector4d& out_plane_);

  /**
   * @brief thread function of ground extraction
   * @param q, quadrant index
   * @param cloud_in_, input point cloud
   * @param out_no_ground, output non-ground point cloud
   * @param out_ground, output ground point cloud
   * @return true if success otherwise false
   */
  bool segmentGroundThread(
    int q,
    const CloudData& cloud_in_,
    CloudData& out_no_ground,
    CloudData& out_ground
  );

  /**
   * @brief ground extraction
   * @param void
   * @return true if success otherwise false
   */
  bool groundRemove(void);

  /**
   * @brief get the index value in the polar radial direction
   * @param radius, polar diameter
   * @return polar diameter index
   */
  int getPolarIndex(double& radius);

  /**
   * @brief converting rectangular coordinate to polar coordinates
   * @param cloud_in_, input point cloud
   * @return void
   */
  void convertToPolar(const CloudData& cloud_in_);

  /**
   * @brief Create a hash table
   * @param void
   * @return true if success otherwise false
   */
  bool createHashTable(void);

  /**
   * @brief search for neighboring voxels
   * @param polar_index, polar diameter index
   * @param pitch_index, pitch angular index
   * @param azimuth_index, azimuth angular index
   * @param out_neighIndex, output adjacent voxel index set
   * @return void
   */
  void searchKNN(
    int& polar_index,
    int& pitch_index,
    int& azimuth_index,
    std::vector<int>& out_neighIndex
  ) const;

  /**
   * @brief the Dynamic Curved-Voxle Clustering algoithm for fast and precise point cloud segmentaiton
   * @param label_info, output the category information of each point
   * @return true if success otherwise false
   */
  bool DCVC(std::vector<int>& label_info);

  /**
   * @brief obtain the point cloud data of each category
   * @param label_info, input category information
   * @return void
   */
  void labelAnalysis(std::vector<int>& label_info);

  /**
   * @brief statistics label information and render the colors for better visualization
   * @param void
   * @return true if success otherwise false
   */
  bool colorSegmentation(void);

  /**
   * @brief category segmentation for non-ground points
   * @brief void
   * @return true if success otherwise false
   */
  bool objectSegmentation(void);

  /**
   * @brief reset related parameters for the next step
   * @param void
   * @return void
   */
  void resetParams(void);

  /**
   * @brief edge features extraction based on the smoothness of the local surface
   * @param cloud_in_, input non-ground point
   * @param out_edge_point, output edge features
   * @param non_edge_point, output non-edge features
   * @return true if success otherwise false
   */
  bool extractEdgePoint(
    const CloudData& cloud_in_,
    CloudData& out_edge_point_,
    CloudData& non_edge_point_
  );

  void extractFromSection(
    CloudData& cloud_in_,
    std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>& curvature_,
    CloudData& out_edge_cloud_,
    CloudData& non_edge_cloud_
  );

private:
  class Voxel{
  public:
    int label{-1};
    std::vector<int> index{};
  };

  class segInfo{
  public:
    int label{-1};
    std::vector<int> index{};
  };

  // subscriber
  std::unique_ptr<CloudSubscriber> cloud_sub_ptr_;
  // publisher
  std::unique_ptr<CloudPublisher> cloud_pub_ground_ptr_;
  std::unique_ptr<CloudPublisher> cloud_pub_object_ptr_;
  std::unique_ptr<CloudPublisher> cloud_pub_edge_ptr_;
  std::unique_ptr<CloudPublisher> cloud_pub_general_ptr_;
  std::unique_ptr<BoundingBoxPublisher> box_pub_ptr_;
  // listener
  std::unique_ptr<TFListener> velo_link_to_map_ptr_;
  Eigen::Matrix4d velo_link_to_map = Eigen::Matrix4d::Identity();

  // 3D point cloud data
  CloudData current_scan{};
  CloudData non_ground_scan{};
  CloudData ground_scan{};
  CloudData object_scan{};
  CloudData segmented_scan{};
  CloudData edge_scan{};
  CloudData general_scan{};

  // LiDAR params
  int sensorModel{64};
  double scanPeriod{0.0};
  double verticalRes{0.0};
  double initAngle{0.0};
  double sensorHeight{0.0};
  double sensorMinRange{0.0};
  double sensorMaxRange{0.0};
  double near_dis{3.0};

  // multi-ground extraction params
  double planeDis{0.0};
  int numSec{0};
  int quadrant{0};
  int groundSeedNum{0};
  int maxIter{0};
  int ringMinNum{0};

  Eigen::VectorXd radiusTableDiff;
  Eigen::VectorXd radiusTable;
  std::vector<size_t>** regionIndex{};
  std::vector<double> sectionBounds{};

  // DCVC params
  double minPitch{0.0};
  double maxPitch{0.0};
  double minPolar{5.0};
  double maxPolar{5.0};
  double startR{0.0};
  double deltaR{0.0};
  double deltaP{0.0};
  double deltaA{0.0};
  int width{0};
  int height{0};
  int minSeg{0};
  int polarNum{0};
  std::vector<double> polarBounds{};
  std::vector<jsk_recognition_msgs::BoundingBox> boxInfo{};

  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> polarCor;
  //std::unordered_map<int, Voxel> voxelMap{};
  std::unordered_map<int, std::vector<int>> voxelMap{};
  std::vector<std::pair<int, segInfo>> labelRecords;

  std::deque<CloudData> cloud_data_buff_;

  int num_threads_;
  std::mutex regionMutex{};
  std::mutex bufferMutex{};

  ros::Time current_time;

  double rgb_table[37][3] = {
    {255, 218, 185},    // PeachPuff
    {255, 245, 238},    // Seashell
    {100, 149, 237},    // ComflowerBlue
    {132, 112, 255},    // LightSlateBlue
    {0, 0, 255},        // Blue
    {30, 144, 255},     // DogerBlue
    {0, 191, 255},      // DeepSkyBlue
    {0, 206, 209},      // DarkTurquoise
    {127, 255, 212},    // Aquamarine
    {0, 255, 127},      // SpringGreen
    {173, 255, 47},     // GreenYellow
    {255, 255, 0},      // Yellow
    {255, 215, 0},      // Gold
    {255, 140, 0},      // DarkOrange
    {255, 105, 180},    // HotPink
    {255, 20, 147},     // DeepPink
    {255, 0, 255},      // Magenta
    {186, 85, 211},     // MediumOrchild
    {138, 43, 226},     // BlueViolet
    {131, 111, 255},    // SlateBlue1
    {72, 118, 255},     // RoyalBlue1
    {0, 245, 255},      // Turquoise1
    {127, 255, 212},    // Aquamarine1
    {84, 255, 159},     // SeaGreen1
    {0, 255, 0},        // Green1
    {127, 255, 0},      // Chartreuse1
    {255, 255, 0},      // Yellow1
    {238, 121, 66},     // Sienna1
    {255, 193, 193},    // RosyBrown1
    {255, 140, 105},    // Salmon1
    {255, 52, 179},     // Maroon1
    {255, 62, 150},     // VioletRed1
    {255, 131, 250},    // Orchid1
    {255, 0, 255},      // Magenta1
    {191, 62, 255},     // DarkOrchild1
    {144, 238, 144},    // LightGreen
    {155, 48, 255}      // Purple1
  };
};

}

#endif //TLOAM_SEGMENTATION_HPP