/**
 * \ file SinusGeneratorFilter.cpp
 */

#include <ATK/config.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>
#include <ATK/Tools/SinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (1024*1024)

BOOST_AUTO_TEST_CASE( SinusGeneratorFilter_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<float> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(-1);
  generator.set_frequency(1000);

  ATK::SinusGeneratorFilter<float> filter;
  filter.set_output_sampling_rate(48000);
  filter.set_frequency(1000);

  ATK::SumFilter<float> sumfilter;
  sumfilter.set_input_sampling_rate(48000);

  ATK::TriangleCheckerFilter<float> checker;
  checker.set_input_sampling_rate(48000);
  checker.set_amplitude(0);
  checker.set_frequency(1000);

  sumfilter.set_input_port(0, &generator, 0);
  sumfilter.set_input_port(1, &filter, 1);

  checker.set_input_port(0, &sumfilter, 0);

  checker.process(PROCESSSIZE);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_VEC_ERF_INCLUDE
#define MTL_VEC_ERF_INCLUDE

#include <boost/numeric/mtl/vector/map_view.hpp>

namespace mtl { namespace vec {

# ifdef MTL_WITH_MATH_ELEVEN
    /// Error function of \a v element-wise
    template <typename Vector>
    erf_view<Vector> erf(const Vector& v)
    {
        return erf_view<Vector>(v);
    }

    /// Inverse error function of \a v element-wise
    template <typename Vector>
    erfc_view<Vector> erfc(const Vector& v)
    {
        return erfc_view<Vector>(v);
    }
# endif

}} // namespace mtl::vec

#endif // MTL_VEC_ERF_INCLUDE


/*
 * File:   NoArvore.hpp
 * Author: arthur
 *
 * Created on 11 de Setembro de 2015, 09:30
 */

#ifndef NOARVORE_HPP
#define	NOARVORE_HPP

#include <armadillo>

#include "Ambient.hpp"
#include "Light.hpp"
#include "Hittable.hpp"
#include "Box.hpp"
#include "Object.hpp"

using namespace arma;
using namespace std;

class NoArvore :public Hittable {

private:
    Box bounds;
    NoArvore *esq, *dir;
    int eixo;
    double split;
    vector<Object *> objetos;

public:
    vec getCentro() {
        return 0;
    }

    vec getMin() {
        return 0;
    }

    vec getMax() {
        return 0;
    }

    static NoArvore * construirArvore(vector <Object *> &objetos, unsigned int &porc, int nivel=0);
    static NoArvore * percorrerArvore(NoArvore * &No, Ray &r, bool &colide, double &Tmin, double &T, int &indice);

    static bool Folha(NoArvore * &No);

    void setObjetos(vector<Object*> objetos) {
        this->objetos = objetos;
    }

    vector<Object*> getObjetos() {
        return objetos;
    }

    void setSplit(double split) {
        this->split = split;
    }

    double getSplit() {
        return split;
    }

    void setEixo(int eixo) {
        this->eixo = eixo;
    }

    int getEixo() {
        return eixo;
    }

    void setDir(NoArvore* dir) {
        this->dir = dir;
    }

    NoArvore* getDir() {
        return dir;
    }

    void setEsq(NoArvore* esq) {
        this->esq = esq;
    }

    NoArvore* getEsq() {
        return esq;
    }

    void setBounds(Box bounds) {
        this->bounds = bounds;
    }

    Box getBounds() {
        return bounds;
    }
};

#endif	/* NOARVORE_HPP */


#ifndef PANACEA_PRIVATE_VECTOREIGEN_H
#define PANACEA_PRIVATE_VECTOREIGEN_H
#pragma once

// Local PANACEA includes
#include "vector.hpp"

#include "data_settings.hpp"

// Third party includes
#include <Eigen/Dense>

// Third party includes
#include <memory>

namespace panacea {

/**
 * Here the underlying vector is owned by the memory manager so we only have a
 * pointer here.
 */
class VectorEigen : public Vector {
private:
  Direction direction_ = Direction::AlongRows;
  std::unique_ptr<Eigen::VectorXd> vector_;
  virtual void direction(const Direction &direction) final {
    direction_ = direction;
  }

public:
  VectorEigen();
  virtual ~VectorEigen() final{};
  virtual const VectorType type() const final;
  virtual VectorEigen &operator=(const Vector &vec) final;
  virtual double &operator()(const int index) final;
  virtual double operator()(const int index) const final;
  virtual Direction direction() const final;
  virtual void resize(const int size) final;
  virtual void setZero() final;
  virtual int rows() const final;
  virtual int cols() const final;
  virtual void print() const final;
};
} // namespace panacea

#endif // PANACEA_PRIVATE_VECTOREIGEN_H


#ifndef TEST_PERIOR_TREE_AABB_TYPE_HPP
#define TEST_PERIOR_TREE_AABB_TYPE_HPP
#include <periortree/aabb_traits.hpp>
#include <test/point_type.hpp>
#include <boost/array.hpp>
#include <boost/mpl/int.hpp>

namespace perior
{
namespace test
{

struct aabb
{
    aabb(const xyz& lw, const xyz& up)
      : lower(lw), upper(up)
    {}

    aabb() {}
    ~aabb(){}

    aabb(const aabb& rhs) : lower(rhs.lower), upper(rhs.upper){}
    aabb& operator=(const aabb& rhs)
    {
        upper = rhs.upper;
        lower = rhs.lower;
        return *this;
    }

    xyz lower, upper;
};

}// test
}// perior

namespace perior
{
namespace traits
{
template<> struct tag<perior::test::aabb> {typedef aabb_tag type;};
template<> struct dimension_of<perior::test::aabb>: boost::mpl::int_<3>{};
template<> struct point_type_of<perior::test::aabb>{typedef perior::test::xyz type;};
template<> struct coordinate_type_of<perior::test::aabb>{typedef double type;};

template<std::size_t D>
struct box_access<perior::test::aabb, max_corner, D>
{
    typedef perior::test::aabb box_type;
    typedef typename point_type_of<box_type>::type point_type;
    typedef typename coordinate_type_of<box_type>::type coordinate_type;

    BOOST_FORCEINLINE
    static coordinate_type get(const box_type& b)
        BOOST_NOEXCEPT_IF(
            noexcept(point_access<point_type, D>::get(b.upper))
        )
    {
        return point_access<point_type, D>::get(b.upper);
    }

    BOOST_FORCEINLINE
    static void set(box_type& b, coordinate_type x)
        BOOST_NOEXCEPT_IF(
            noexcept(point_access<point_type, D>::set(b.upper, x))
        )
    {
        return point_access<point_type, D>::set(b.upper, x);
    }
};

template<std::size_t D>
struct box_access<perior::test::aabb, min_corner, D>
{
    typedef perior::test::aabb box_type;
    typedef typename point_type_of<box_type>::type point_type;
    typedef typename coordinate_type_of<box_type>::type coordinate_type;

    BOOST_FORCEINLINE
    static coordinate_type get(const box_type& b)
        BOOST_NOEXCEPT_IF(
            noexcept(point_access<point_type, D>::get(b.lower))
        )
    {
        return point_access<point_type, D>::get(b.lower);
    }

    BOOST_FORCEINLINE
    static void set(box_type& b, coordinate_type x)
        BOOST_NOEXCEPT_IF(
            noexcept(point_access<point_type, D>::set(b.lower, x))
        )
    {
        return point_access<point_type, D>::set(b.lower, x);
    }
};

template<std::size_t D>
struct box_range_access<perior::test::aabb, D>
{
    typedef perior::test::aabb box_type;
    typedef typename point_type_of<box_type>::type point_type;
    typedef typename coordinate_type_of<box_type>::type coordinate_type;

    BOOST_FORCEINLINE
    static coordinate_type get(const box_type& b)
        BOOST_NOEXCEPT_IF(
            noexcept(point_access<point_type, D>::get(b.lower))
        )
    {
        return point_access<point_type, D>::get(b.upper) -
               point_access<point_type, D>::get(b.lower);
    }
};

}// traits
}// perior
#endif//TEST_PERIOR_TREE_AABB_TYPE_HPP


#include <boost/units/systems/si/wavenumber.hpp>


#include <boost/geometry/multi/strategies/cartesian/centroid_average.hpp>


/*
 * Copyright 2019 OmniSci, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <gtest/gtest.h>
#include <boost/filesystem.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/program_options.hpp>
#include <csignal>
#include <exception>
#include <memory>
#include <vector>
#include "Catalog/Catalog.h"
#include "Catalog/DBObject.h"
#include "DataMgr/DataMgr.h"
#include "QueryEngine/Execute.h"
#include "QueryEngine/ExtensionFunctionsWhitelist.h"
#include "QueryEngine/OverlapsJoinHashTable.h"
#include "QueryEngine/ResultSet.h"
#include "QueryEngine/UDFCompiler.h"
#include "QueryRunner/QueryRunner.h"
#include "Shared/Logger.h"
#include "Shared/MapDParameters.h"
#include "TestHelpers.h"

namespace po = boost::program_options;

#ifndef BASE_PATH
#define BASE_PATH "./tmp"
#endif

constexpr size_t c_calcite_port = 36279;

using namespace Catalog_Namespace;
using namespace TestHelpers;

using QR = QueryRunner::QueryRunner;

namespace {
ExecutorDeviceType g_device_type;
}

inline auto sql(const std::string& sql_stmts) {
  return QR::get()->runMultipleStatements(sql_stmts, g_device_type);
}

int deviceCount(const Catalog_Namespace::Catalog* catalog,
                const ExecutorDeviceType device_type) {
  if (device_type == ExecutorDeviceType::GPU) {
    const auto cuda_mgr = catalog->getDataMgr().getCudaMgr();
    CHECK(cuda_mgr);
    return cuda_mgr->getDeviceCount();
  } else {
    return 1;
  }
}

TEST(Memcmp, OverlapsJoinHashTable) {
  g_device_type = ExecutorDeviceType::CPU;

  sql(R"(
    drop table if exists my_points;
    drop table if exists my_grid;
  )");

  sql(R"(
    create table my_points (locations geometry(point, 4326) encoding none);
    create table my_grid (cells geometry(multipolygon, 4326) encoding none);

    insert into my_points values ('point(5 5)');
    insert into my_points values ('point(5 25)');
    insert into my_points values ('point(10 5)');

    insert into my_grid values ('multipolygon(((0 0,10 0,10 10,0 10,0 0)))');
    insert into my_grid values ('multipolygon(((10 0,20 0,20 10,10 10,10 0)))');
    insert into my_grid values ('multipolygon(((0 10,10 10,10 20,0 20,0 10)))');
    insert into my_grid values ('multipolygon(((10 10,20 10,20 20,10 20,10 10)))');
  )");

  auto catalog = QR::get()->getCatalog();
  auto executor = Executor::getExecutor(catalog->getCurrentDB().dbId);

  auto tmeta1 = catalog->getMetadataForTable("my_points");
  auto tmeta2 = catalog->getMetadataForTable("my_grid");

  SQLTypeInfo ti1{kPOINT, 4326, 4326, false, kENCODING_NONE, 64, kGEOMETRY};
  SQLTypeInfo ti2{kARRAY, 0, 0, true, kENCODING_NONE, 0, kDOUBLE};
  ti2.set_size(32);

  auto a1 = std::make_shared<Analyzer::ColumnVar>(ti1, tmeta1->tableId, 1, 0);
  auto a2 = std::make_shared<Analyzer::ColumnVar>(ti2, tmeta2->tableId, 5, 1);

  auto op = std::make_shared<Analyzer::BinOper>(kBOOLEAN, kOVERLAPS, kONE, a1, a2);

  size_t number_of_join_tables{2};
  std::vector<InputTableInfo> viti(number_of_join_tables);
  viti[0].table_id = tmeta1->tableId;
  viti[0].info = tmeta1->fragmenter->getFragmentsForQuery();
  viti[1].table_id = tmeta2->tableId;
  viti[1].info = tmeta2->fragmenter->getFragmentsForQuery();

  auto memory_level = Data_Namespace::CPU_LEVEL;

  auto device_count = deviceCount(catalog.get(), g_device_type);

  ColumnCacheMap ccm;

  auto hash_table = OverlapsJoinHashTable::getInstance(
      op, viti, memory_level, device_count, ccm, executor.get());

  auto ptr1 =
      reinterpret_cast<const int32_t*>(hash_table->getJoinHashBuffer(g_device_type, 0));
  auto ptr2 = reinterpret_cast<const int32_t*>(
      hash_table->getJoinHashBuffer(g_device_type, 0) + hash_table->offsetBufferOff());
  CHECK_EQ(*ptr1, -1);
  CHECK_EQ(*ptr2, -1);
}

int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);

  logger::LogOptions log_options(argv[0]);
  log_options.severity_ = logger::Severity::DEBUG1;
  logger::init(log_options);

  QR::init(BASE_PATH);

  int err{0};
  try {
    err = RUN_ALL_TESTS();
  } catch (const std::exception& e) {
    LOG(ERROR) << e.what();
  }
  QR::reset();
  return err;
}


#include "iteration/subroutine/two_grid_acceleration.hpp"

#include <deal.II/lac/vector.h>

#include "acceleration/two_grid/tests/flux_corrector_mock.hpp"
#include "calculator/residual/tests/domain_isotropic_residual_mock.hpp"
#include "framework/tests/framework_mock.hpp"
#include "system/moments/tests/spherical_harmonic_mock.h"
#include "system/system.hpp"
#include "test_helpers/test_helper_functions.h"
#include "test_helpers/test_assertions.hpp"

#include "test_helpers/gmock_wrapper.h"

namespace  {

using namespace bart;
using ::testing::NiceMock, ::testing::DoDefault, ::testing::Return, ::testing::ReturnRef, ::testing::_;
using ::testing::Ref, ::testing::AtLeast;

class IterationSubroutineTwoGridAccelerationTest : public ::testing::Test {
 public:
  using RHSVector = dealii::Vector<double>;
  using FluxCorrectorMock = acceleration::two_grid::FluxCorrectorMock;
  using FrameworkMock = framework::FrameworkMock;
  using IsotropicResidualCalculatorMock = calculator::residual::DomainIsotropicResidualMock;
  using Moments = system::moments::SphericalHarmonicMock;
  using TwoGridSubroutine = iteration::subroutine::TwoGridAcceleration;
  using System = system::System;

  // Dependency pointers
  std::shared_ptr<RHSVector> rhs_vector_ptr_;
  FluxCorrectorMock* flux_corrector_mock_obs_ptr_;
  FrameworkMock* framework_mock_obs_ptr_;
  IsotropicResidualCalculatorMock* isotropic_residual_calculator_mock_obs_ptr_;
  Moments* current_moments_obs_ptr_;
  Moments* previous_moments_obs_ptr_;
  Moments* framework_system_current_moments_obs_ptr_;

  std::unique_ptr<TwoGridSubroutine> test_subroutine_;
  System test_system_;
  std::shared_ptr<System> framework_system_{ std::make_shared<System>() };
  system::moments::MomentsMap current_moments_map_{};

  dealii::Vector<double> total_isotropic_residual_;

  static constexpr int vector_size_{ 10 };
  static constexpr int total_groups_{ 3 };
  auto SetUp() -> void override;
};

auto DealiiVector(std::vector<double> to_convert) {
  dealii::Vector<double> return_vector(to_convert.size());
  for (std::vector<double>::size_type i = 0; i < to_convert.size(); ++i)
    return_vector(i) = to_convert.at(i);
  return return_vector;
}

auto IterationSubroutineTwoGridAccelerationTest::SetUp() -> void {
  rhs_vector_ptr_ = std::make_shared<RHSVector>(vector_size_);

  auto flux_corrector_ptr = std::make_unique<FluxCorrectorMock>();
  flux_corrector_mock_obs_ptr_ = flux_corrector_ptr.get();
  auto isotropic_residual_calculator_ptr = std::make_unique<IsotropicResidualCalculatorMock>();
  isotropic_residual_calculator_mock_obs_ptr_ = isotropic_residual_calculator_ptr.get();
  auto framework_ptr = std::make_unique<FrameworkMock>();
  framework_mock_obs_ptr_ = framework_ptr.get();

  test_subroutine_ = std::make_unique<TwoGridSubroutine>(std::move(flux_corrector_ptr),
                                                         std::move(framework_ptr),
                                                         std::move(isotropic_residual_calculator_ptr),
                                                         rhs_vector_ptr_);

  test_system_.total_groups = this->total_groups_;
  test_system_.current_moments = std::make_shared<Moments>();
  test_system_.previous_moments = std::make_unique<Moments>();
  current_moments_obs_ptr_ = dynamic_cast<Moments*>(test_system_.current_moments.get());
  previous_moments_obs_ptr_ = dynamic_cast<Moments*>(test_system_.previous_moments.get());

  framework_system_->total_groups = 1;
  framework_system_->current_moments = std::make_shared<Moments>();
  framework_system_current_moments_obs_ptr_ = dynamic_cast<Moments*>(framework_system_->current_moments.get());

  total_isotropic_residual_ = DealiiVector(test_helpers::RandomVector(vector_size_, 0, 100));

  for (int group = 0; group < total_groups_; ++group)
    current_moments_map_[{group, 0, 0}] = DealiiVector(test_helpers::RandomVector(vector_size_, 0, 100));

  ON_CALL(*isotropic_residual_calculator_mock_obs_ptr_, CalculateDomainResidual(_,_))
      .WillByDefault(Return(total_isotropic_residual_));
  ON_CALL(*current_moments_obs_ptr_, begin()).WillByDefault(Return(current_moments_map_.begin()));
}

// Getters for depdendencies should return correct pointers to dependencies
TEST_F(IterationSubroutineTwoGridAccelerationTest, DependencyGetters) {
  EXPECT_EQ(test_subroutine_->framework_ptr(), framework_mock_obs_ptr_);
  EXPECT_EQ(test_subroutine_->flux_corrector_ptr(), flux_corrector_mock_obs_ptr_);
  EXPECT_EQ(test_subroutine_->residual_calculator_ptr(), isotropic_residual_calculator_mock_obs_ptr_);
  EXPECT_EQ(test_subroutine_->isotropic_residual_ptr(), rhs_vector_ptr_);
}

// Passing a null dependency should throw an error
TEST_F(IterationSubroutineTwoGridAccelerationTest, NullDependenciesThrow) {
  constexpr int n_dependencies{ 4 };
  for (int i = 0; i < n_dependencies; ++i) {
    EXPECT_ANY_THROW({
      TwoGridSubroutine(i == 0 ? nullptr : std::make_unique<FluxCorrectorMock>(),
                        i == 1 ? nullptr : std::make_unique<FrameworkMock>(),
                        i == 2 ? nullptr : std::make_unique<IsotropicResidualCalculatorMock>(),
                        i == 3 ? nullptr : rhs_vector_ptr_);
    });
  }
}

TEST_F(IterationSubroutineTwoGridAccelerationTest, Execute) {
  EXPECT_CALL(*this->isotropic_residual_calculator_mock_obs_ptr_, CalculateDomainResidual(current_moments_obs_ptr_, _))
      .WillOnce(DoDefault());
  EXPECT_CALL(*this->current_moments_obs_ptr_, total_groups()).WillOnce(Return(total_groups_));
  EXPECT_CALL(*this->current_moments_obs_ptr_, begin()).Times(total_groups_).WillRepeatedly(DoDefault());
  EXPECT_CALL(*this->framework_mock_obs_ptr_, SolveSystem());
  EXPECT_CALL(*this->framework_mock_obs_ptr_, system())
      .Times(AtLeast(1))
      .WillRepeatedly(Return(this->framework_system_.get()));
  dealii::Vector<double> error_vector(DealiiVector(test_helpers::RandomVector(this->vector_size_, 0, 100)));

  EXPECT_CALL(*this->framework_system_current_moments_obs_ptr_, GetMoment(std::array<int, 3>{0, 0, 0}))
      .WillOnce(ReturnRef(error_vector));
  for (int group = 0; group < this->total_groups_; ++group) {
    dealii::Vector<double> flux_to_correct(DealiiVector(test_helpers::RandomVector(this->vector_size_, 0, 100)));
    EXPECT_CALL(*this->current_moments_obs_ptr_, GetMoment(std::array<int, 3>{group, 0, 0}))
        .Times(2)
        .WillRepeatedly(ReturnRef(flux_to_correct));
    EXPECT_CALL(*this->flux_corrector_mock_obs_ptr_, CorrectFlux(Ref(flux_to_correct), error_vector, group));
  }

  this->test_subroutine_->Execute(this->test_system_);

  EXPECT_TRUE(test_helpers::AreEqual(this->total_isotropic_residual_, *this->rhs_vector_ptr_));
}

} // namespace


#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <opencv2/ml/ml.hpp>

#include <iostream>
#include <experimental/filesystem>
#include "common.hpp"

namespace fs = std::experimental::filesystem;

using namespace dlib;
using namespace std;

void performSVM(std::string csvFile)
{
    ifstream file(csvFile);
    std::string line;
    int number_of_lines = 0;
    while (std::getline(file, line))
      ++number_of_lines;

    file.close();

    int features = 68 + 1;
    cv::Mat training_mat(number_of_lines, features - 1, CV_32FC1);
    cv::Mat labels(number_of_lines, 1, CV_32SC1);

    ifstream f(csvFile);
    int line_num = 0;

    while (std::getline(f, line))
    {
        std::istringstream iss(line);
        float i;
        std::vector<float> vect;
        while (iss >> i)
        {
            vect.push_back(i);
            if (iss.peek() == ',')
                iss.ignore();
        }
        for (int j = 0; j < vect.size(); j++) {
            if (j == 68) {
                int tt = (int)vect.at(j);
                labels.at<int>(line_num, 0) = tt;
            } else{
                training_mat.at<float>(line_num, j) = vect.at(j);
            }
        }
        line_num++;
    }

    f.close();

    cv::Ptr<cv::ml::SVM> svm = cv::ml::SVM::create();

    svm->setType(cv::ml::SVM::C_SVC);
    svm->setKernel(cv::ml::SVM::RBF);

    try {
        svm->trainAuto(training_mat, cv::ml::ROW_SAMPLE, labels);
        svm->save("liveness.svm");
    } catch(cv::Exception& e) {
        cout << e.what() << endl;
    }
    return;
}

int main(int argc, char** argv )
{
    if ( argc != 2 )
    {
        printf("usage2: csvFile>\n");
        return -1;
    }

    std::string csvFile = argv[1];
    performSVM(csvFile);//~/Work/face_liveness_detection_data/combine.csv

    return 0;
}


#include <WebServer/base/LogStream.h>

#include <boost/static_assert.hpp>

#include <limits>
#include <assert.h>
#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include <algorithm>

const char digits[] = "9876543210123456789";
const char* zero = digits + 9;
BOOST_STATIC_ASSERT(sizeof(digits) == 20);

const char digitsHex[] = "0123456789ABCDEF";
BOOST_STATIC_ASSERT(sizeof(digitsHex) == 17);

template<typename T>
size_t convert(char buf[], T value)
{
    T i = value;
    char* p = buf;

    do
    {
        int lsd = static_cast<int> (i % 10);
        i /= 10;
        *p++ = zero[lsd];
    } while (i != 0);

    if (value < 0)
    {
        *p++ = '-';
    }
    *p = '\0';
    std::reverse(buf, p);

    return p - buf;
}

size_t convertHex(char buf[], uintptr_t value)
{
    uintptr_t i = value;
    char* p = buf;

    do
    {
        int lsd = i % 16;
        i /= 16;
        *p++ = digitsHex[lsd];
    } while(i != 0);

    *p = '\0';
    std::reverse(buf, p);

    return p - buf;
}

using namespace ywl;
template class ywl::FixedBuffer<KSmallBuffer>;
template class ywl::FixedBuffer<KLargeBuffer>;

template<typename T>
void LogStream::formatInteger(T v)
{
    //buffer容不下KMaxNumericSize则丢弃
    if (buffer_.writeableBytes() >= KMaxNumericSize)
    {
        size_t len = convert(buffer_.current(), v);
        buffer_.retrieve(len);
    }
}

LogStream& LogStream::operator<<(short v)

{
    *this << static_cast<int>(v);
    return *this;
}

LogStream& LogStream::operator<<(unsigned short v)
{
    *this << static_cast<unsigned int>(v);
    return *this;
}

LogStream& LogStream::operator<<(int v)
{
    formatInteger(v);
    return *this;
}

LogStream& LogStream::operator<<(unsigned int v)
{
    formatInteger(v);
    return *this;
}

LogStream& LogStream::operator<<(long v)
{
    formatInteger(v);
    return *this;
}

LogStream& LogStream::operator<<(unsigned long v)
{
    formatInteger(v);
    return *this;
}

LogStream& LogStream::operator<<(long long v)
{
    formatInteger(v);
    return *this;
}

LogStream& LogStream::operator<<(unsigned long long v)
{
    formatInteger(v);
    return *this;
}

LogStream& LogStream::operator<<(const void* p)
{
    uintptr_t v = reinterpret_cast<uintptr_t>(p);
    if (buffer_.writeableBytes() >= KMaxNumericSize)
    {
        char* buf = buffer_.current();
        buf[0] = '0';
        buf[1] = 'x';
        size_t len = convertHex(buf+2, v);
        buffer_.retrieve(len+2);
    }
    return *this;
}

LogStream& LogStream::operator<<(double v)
{
    if (buffer_.writeableBytes() >= KMaxNumericSize)
    {
        int len = snprintf(buffer_.current(), KMaxNumericSize, "%.12g", v);
        buffer_.retrieve(len);
    }
    return *this;
}

LogStream& LogStream::operator<<(long double v)
{
    if (buffer_.writeableBytes() >= KMaxNumericSize)
    {
        int len = snprintf(buffer_.current(), KMaxNumericSize, "%.12Lg", v);
        buffer_.retrieve(len);
    }
    return *this;
}


// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_DYN_LINK
#define SPATIAL_ENABLE_ASSERT // detect interal issues that should not occur

#include <boost/test/unit_test.hpp>
#include "../../src/point_multiset.hpp"
#include "spatial_test_types.hpp"

using namespace spatial;

BOOST_AUTO_TEST_CASE( test_point_constructors )
{
  point_multiset<2, int2> points;
  point_multiset<0, int2> runtime_points;
}

BOOST_AUTO_TEST_CASE( test_point_copy_assignment )
{
  point_multiset<2, int2> points;
  points.insert(zeros);
  points.insert(ones);
  points.insert(twos);
  point_multiset<2, int2> copy(points);
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
  points = copy;
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
}

BOOST_AUTO_TEST_CASE( test_zero_point_copy_assignment )
{
  point_multiset<0, int2> points;
  points.insert(zeros);
  points.insert(ones);
  points.insert(twos);
  point_multiset<0, int2> copy(points);
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
  points = copy;
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_IS_NLE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NLE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_nle capabilities

    Returns @ref True  or @ref False depending on whether x is
    not less or equal to y or is.

    This is a convenient alias of @ref is_not_less_equal
  **/
  const boost::dispatch::functor<tag::is_nle_> is_nle = {};
} }
#endif

#include <boost/simd/function/scalar/is_not_less_equal.hpp>
#include <boost/simd/function/simd/is_nle.hpp>

#endif


//==================================================================================================
/**
  Copyright 2016 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_ARCH_COMMON_SIMD_FUNCTION_BROADCAST_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_BROADCAST_HPP_INCLUDED

#include <boost/simd/function/extract.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  BOOST_DISPATCH_OVERLOAD ( broadcast_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::unspecified_<A0>,X>
                          , bd::constant_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      return do_(a0,brigand::bool_< A1::value >= 0 && A1::value < A0::static_size>{});
    }

    static BOOST_FORCEINLINE A0 do_(A0 const& a0, brigand::bool_<true> const&) BOOST_NOEXCEPT
    {
      return A0(extract<A1::value>(a0));
    }

    static BOOST_FORCEINLINE A0 do_(A0 const&, brigand::bool_<false> const&) BOOST_NOEXCEPT
    {
      return Zero<A0>();
    }
  };
} } }

#endif


/*
 * LinearSolver.cpp
 *
 *  Created on: Sep 24, 2015
 *      Author: lriha
 */
//#include <Driver/DissectionSolver.hpp>
#include "../../basis/utilities/utils.h"
#include "FETISolver.h"

//#include <Eigen/Dense>
//using Eigen::MatrixXd;

#ifdef READEX_LEVEL_1
#include "readex.h"
#include "readex_regions.h"
#endif

using namespace espreso;

FETISolver::FETISolver(Instance *instance, const ESPRESOSolver &configuration)
: instance(instance),
  configuration(configuration),
  timeEvalMain("ESPRESO Solver Overal Timing"),
  cluster(NULL),
  solver(NULL)
{
}

FETISolver::~FETISolver() {

	if (cluster != NULL) {
		delete cluster;
	}

	if (solver != NULL) {
		delete solver;
	}
}

// make full initialization of solver
void FETISolver::init()
{
	if (cluster != NULL) {
		delete cluster;
	}

	if (solver != NULL) {
		delete solver;
	}

	//instance->computeKernels(configuration.regularization, configuration.SC_SIZE);
	if (configuration.method == ESPRESO_METHOD::HYBRID_FETI) {
		instance->assembleB0(configuration.B0_type, instance->N1);
	}
	solver  = new IterSolver	(configuration);
	cluster = new SuperCluster	(configuration, instance);

	init(instance->neighbours);
}

// make partial initialization according to updated matrices
void FETISolver::update(Matrices matrices)
{
	// TODO update appropriate solver objects and stop steeling matrices! :)

	if (matrices & (Matrices::K | Matrices::N)) {
		// factorization and preconditioners and HFETI preprocessing

		delete cluster;
		delete solver;

		//instance->computeKernels(configuration.regularization, configuration.SC_SIZE);
		cluster = new SuperCluster(configuration, instance);
		solver  = new IterSolver(configuration);

		init(instance->neighbours);

	} else {

		if (matrices & Matrices::B1) { // N is kernel of matrix K
			// updateGGt();
			setup_CreateG_GGt_CompressG();
		}

		if (matrices & (Matrices::B1c | Matrices::f)) {
			// update dual RHS
			//for f:
			// - updated by solve

			//for B1c:
			setup_SetDirichletBoundaryConditions();
		}

		if (matrices & Matrices::B0) {
			// HFETI preprocessing
			if (configuration.method == ESPRESO_METHOD::HYBRID_FETI) {
				instance->assembleB0(configuration.B0_type, instance->N1);
			}
			setup_HTFETI();
		}

		if (matrices & Matrices::B1duplicity) {
			// update duplicity vector
			#pragma omp parallel for
			for (size_t d = 0; d < cluster->domains.size(); d++) {
				cluster->domains[d]->B1_scale_vec = instance->B1duplicity[d];
			}
		}

	}

	// TODO: remove full re-initialization
	//	delete cluster;
	//	delete solver;
	//	cluster = new Cluster(configuration);
	//	solver = new IterSolver(configuration);
	//	setup();
	//	init(instance->neighbours);
}

// run solver and store primal and dual solution
void FETISolver::solve()
{
	if (
			std::any_of(instance->K.begin(), instance->K.end(), [] (const SparseMatrix &K) { return K.mtype == MatrixType::REAL_UNSYMMETRIC; }) &&
			configuration.solver != ESPRESO_ITERATIVE_SOLVER::GMRES &&
			configuration.solver != ESPRESO_ITERATIVE_SOLVER::BICGSTAB) {

		ESINFO(ERROR) << "Invalid Linear Solver configuration: Only GMRES and BICGSTAB can solve unsymmetric system.";
	}

	Solve(instance->f, instance->primalSolution, instance->dualSolution);
}

void FETISolver::setup_HTFETI() {
// Setup Hybrid FETI part of the solver

	if (cluster->USE_HFETI == 1) {
		 TimeEvent timeHFETIprec(string("Solver - HFETI preprocessing"));
		 timeHFETIprec.start();

		cluster->SetClusterHFETI();

		 timeHFETIprec.endWithBarrier();
		 timeEvalMain.addEvent(timeHFETIprec);

		ESLOG(MEMORY) << "After HFETI preprocessing process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";
	}
}

void FETISolver::setup_LocalSchurComplement() {
// Computation of the Local Schur Complements
//TODO: update for multiple clusters

	if (cluster->USE_KINV == 1) {
		 TimeEvent KSCMem(string("Solver - SC asm. w PARDISO-SC mem [MB]")); KSCMem.startWithoutBarrier(GetProcessMemory_u());
		 TimeEvent timeSolSC2(string("Solver - Schur Complement asm. - using PARDISO-SC"));timeSolSC2.start();
		bool USE_FLOAT = false;
		if (configuration.schur_precision == FLOAT_PRECISION::SINGLE) {
			USE_FLOAT = true;
		}
		cluster->Create_SC_perDomain(USE_FLOAT);
		 timeSolSC2.endWithBarrier(); timeEvalMain.addEvent(timeSolSC2);
		 KSCMem.endWithoutBarrier(GetProcessMemory_u()); //KSCMem.printLastStatMPIPerNode();
		 ESLOG(MEMORY) << "After K inv. process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";
	} else {
		for (size_t d = 0; d < cluster->domains.size(); d++) {
			cluster->domains[d]->isOnACC = 0;
		}
	}

}

void FETISolver::createCMat() {
/*

	//int d = 0;

	cluster->SetupPreconditioner();

	SEQ_VECTOR<SparseMatrix> Ml_per_dom (cluster->domains.size());
	SEQ_VECTOR<SparseMatrix> Ml_fin_pd  (cluster->domains.size());

	SEQ_VECTOR<SparseMatrix> Bdec   (cluster->domains.size());
	SEQ_VECTOR<SparseMatrix> Btdec  (cluster->domains.size());

	for (size_t d = 0; d < cluster->domains.size(); d++) {

		cluster->domains[d].B1 = instance->B1[d];

		SparseMatrix B1full = instance->B1[d];
		B1full.ConvertToCSR(0);

		SparseMatrix B1tfull = instance->B1[d];
		B1tfull.ConvertToCSR(0);
		B1tfull.MatTranspose();

		SparseMatrix PrecFull = cluster->domains[d].Prec;

		PrecFull.ConvertDenseToCSR(1);

		SparseMatrix Bt = cluster->domains[d].B1t_DirPr;
		SparseMatrix B;
		Bt.MatTranspose(B);

		Bdec[d] = B;
		Bdec[d].ConvertToCOO(1);

		Btdec[d] = Bt;
		Btdec[d].ConvertToCOO(1);

		SparseMatrix Tmp  = PrecFull;
		Tmp.SetDiagonalOfSymmetricMatrix(0.0);
		Tmp.MatTranspose();
		PrecFull.MatAddInPlace(Tmp,'N',1.0);
		PrecFull.ConvertToCOO(1);
		PrecFull.type='G';

		SEQ_VECTOR <eslocal> prec_map_vec;
		prec_map_vec = cluster->domains[d].B1t_Dir_perm_vec;

		for (size_t i = 0; i < PrecFull.I_row_indices.size(); i++) {
			PrecFull.I_row_indices[i] = prec_map_vec[PrecFull.I_row_indices[i]-1];
			PrecFull.J_col_indices[i] = prec_map_vec[PrecFull.J_col_indices[i]-1];
		}

		PrecFull.rows = cluster->domains[d].B1.cols;
		PrecFull.cols = cluster->domains[d].B1.cols;
		PrecFull.ConvertToCSR(0);

		SparseMatrix TmpBt, Ml;

		TmpBt.MatMat(PrecFull,'N',B1tfull);
		Ml.MatMat(B1full,'N',TmpBt);
		//std::cout << Ml.SpyText();

		//Decompress matrix
		Ml.ConvertToCOO(0);

//		eslocal dl_size = cluster->my_lamdas_indices.size();
//		Ml.rows = dl_size;
//		Ml.cols = dl_size;
//
//
//		for (eslocal i = 0; i < Ml.I_row_indices.size(); i++) {
//			eslocal I 		= Ml.I_row_indices[i] - 1;
//			eslocal J 		= Ml.J_col_indices[i] - 1;
//			eslocal I_dec   = cluster->domains[d].lambda_map_sub_local[I] + 1;
//			eslocal J_dec   = cluster->domains[d].lambda_map_sub_local[J] + 1;
//			Ml.I_row_indices[i] = I_dec;
//			Ml.J_col_indices[i] = J_dec;
//
//		}
//
//		Ml.ConvertToCSR(1);
		Ml_per_dom[d].swap(Ml);
//
//		Bdec[d].rows = dl_size;
//		Btdec[d].cols = dl_size;
//		for (eslocal i = 0; i < Bdec[d].I_row_indices.size(); i++) {
//			eslocal I     = Bdec[d].I_row_indices[i] - 1;
//			eslocal I_dec = cluster->domains[d].lambda_map_sub_local[I] + 1;
//
//			Bdec[d].I_row_indices[i]  = I_dec;
//			Btdec[d].J_col_indices[i] = I_dec;
//		}
//		Bdec[d].ConvertToCSR(1);
//		Btdec[d].ConvertToCSR(1);

	}

	//TODO: can be done as log N
	SparseMatrix Ml_per_cluster;
	Ml_per_cluster = Ml_per_dom[0];
	for (size_t d = 1; d < cluster->domains.size(); d++) {
		Ml_per_cluster.MatAddInPlace(Ml_per_dom[d],'N',1.0);
	}

	SparseMatrix C_per_cluster;

	std::cout << Ml_per_cluster.SpyText();

	for (size_t d = 0; d < cluster->domains.size(); d++) {

		SparseMatrix B1tfull = instance->B1[d];
		SparseMatrix B1full;

		B1tfull.ConvertToCSR(1);
		B1tfull.MatTranspose();
		Esutils::removeDuplicity(B1tfull.CSR_I_row_indices);
		B1tfull.rows = B1tfull.CSR_I_row_indices.size() - 1;

		B1full = B1tfull;
		B1tfull.ConvertToCOO(0);
		B1full.MatTranspose();
		B1full.ConvertToCOO(0);

		SparseMatrix tmp;
		tmp.MatMat(Ml_per_cluster,'N',B1full);
		Ml_fin_pd[d].MatMat(B1tfull,'N',tmp);
		std::cout << Ml_fin_pd[d].SpyText();
		Ml_fin_pd[d].ConvertToCOO(0);

		SparseMatrix Prec = cluster->domains[d].Prec;
		Prec.ConvertDenseToCSR(1);
		SparseMatrix Tmp2 = Prec;
		Tmp2.SetDiagonalOfSymmetricMatrix(0.0);
		Tmp2.MatTranspose();
		Prec.MatAddInPlace(Tmp2,'N',1.0);
		Prec.type='G';

		SparseMatrix A = Prec;
		A.type = 'G';
		SparseMatrix B = Ml_fin_pd[d];
		B.type = 'G';

		A.ConvertCSRToDense(0);
		B.ConvertCSRToDense(0);

//		SparseMatrix Ab = A;
//		SparseMatrix Bb = B;
//
//		int info = 0;
//		info = LAPACKE_dsyev (
//				LAPACK_COL_MAJOR, //int matrix_layout,
//				'N', // char jobz,
//				'U', // char uplo,
//				n, // lapack_int n,
//				&Ab.dense_values[0], //float* a,
//				n, //lapack_int lda,
//				&w[0]); //float* w);
//
//
//		info = 0;
//		info = LAPACKE_dsyev (
//				LAPACK_COL_MAJOR, //int matrix_layout,
//				'N', // char jobz,
//				'U', // char uplo,
//				n, // lapack_int n,
//				&Bb.dense_values[0], //float* a,
//				n, //lapack_int lda,
//				&w[0]); //float* w);
//
//		SparseMatrix Ac = A;
//		SparseMatrix Bc = B;
//
//		info = 0;
//		info = LAPACKE_dsygv (
//				LAPACK_COL_MAJOR, //int matrix_layout,
//				1, //lapack_int itype - itype = 1, the problem type is A*x = lambda*B*x;
//				'V', //char jobz, - If jobz = 'V', then compute eigenvalues and eigenvectors.
//				'U', // char uplo,
//				n, // lapack_int n,
//				&Ac.dense_values[0], // float* a,
//				n, // lapack_int lda,
//				&Bc.dense_values[0], // float* b,
//				n, // lapack_int ldb,
//				&w[0]); //float* w);
//
//		std:fill(w.begin(), w.end(), 0.0);

		int il = 1; // first eigen value to be calculated
		int iv = 4; // last  eigen value to be calculated

		int eig_n = (iv-il+1);
		eslocal n = A.rows;
		SEQ_VECTOR <double> w (eig_n);				// eigen values storage
		SEQ_VECTOR <double> eig_vectors (eig_n*n);	// eigen vectors storage
		SEQ_VECTOR <eslocal> ifail (n);				// dummy
		SEQ_VECTOR <eslocal> m (n);					// dummy
		double tmpd = 0;							// dummy
		double abstol = 0.0;						// dummy

		LAPACKE_dsygvx (
				LAPACK_COL_MAJOR, 	// int matrix_layout,
				1, 					//lapack_int itype,
				'V', 				//char jobz,
				'I', 				//char range,
				'U', 				//char uplo,
				n, 					//lapack_int n,
				&A.dense_values[0], //double* a,
				n, 					//lapack_int lda,
				&B.dense_values[0], //double* b,
				n, 					//lapack_int ldb,
				tmpd, 				//double vl,
				tmpd, 				//double vu,
				il, 				//lapack_int il,
				iv, 				//lapack_int iu,
				abstol, 			//double abstol,
				&m[0],				//lapack_int* m,
				&w[0],				//double* w,
				&eig_vectors[0], 	//double* z,
				n, 					//lapack_int ldz,
				&ifail[0]); 		//lapack_int* ifail);

            int copy_ind_begin = 0;
            for (int i = 0; i < eig_n; i++) {
        		if ( w[i] > 1e-10 ) {
        			copy_ind_begin = n*(i);
        			break;
        		}
        	}

            SparseMatrix Vi;
            Vi.dense_values = std::vector<double> (eig_vectors.begin() + copy_ind_begin, eig_vectors.end() );
            Vi.type = 'G';
            Vi.nnz  = Vi.dense_values.size();
            Vi.rows = n;
			Vi.cols = eig_n - (copy_ind_begin/n);
			Vi.ConvertDenseToCSR(0);

			SparseMatrix BtVi;
			BtVi.MatMat(B1full,'N',Vi);
			BtVi.MatTranspose();

			C_per_cluster.MatAppend(BtVi);

	}

	C_per_cluster.MatTranspose();
	C_per_cluster.ConvertCSRToDense(0);

	SparseMatrix CP_per_cluster;
	SparseMatrix CPt_per_cluster;
	SparseMatrix ACP_per_cluster;

	solver->Projector_l_inv_compG(solver->timeEvalProj, *cluster, C_per_cluster, CP_per_cluster );

	solver->apply_A_l_Mat (solver->timeEvalAppa, *cluster, CP_per_cluster, ACP_per_cluster);

	SparseMatrix Proj_per_cluster;
	SparseMatrix Proj_per_cluster2;

	SparseMatrix Proj_per_cluster_sp;

	CPt_per_cluster = CP_per_cluster;
	CPt_per_cluster.ConvertDenseToCSR(1);
	CPt_per_cluster.MatTranspose();
	CPt_per_cluster.ConvertCSRToDense(1);

	Proj_per_cluster.DenseMatMat(CPt_per_cluster,'N',ACP_per_cluster,'N');

	Proj_per_cluster2.DenseMatMat(CP_per_cluster,'T',ACP_per_cluster,'N'); // TODO : DGEMM nefunguje s transpose turned on - needs to be fixed

	ACP_per_cluster.ConvertDenseToCSR(0);
	CPt_per_cluster.ConvertDenseToCSR(0);

	Proj_per_cluster_sp.MatMat(CPt_per_cluster,'N',ACP_per_cluster);
	Proj_per_cluster_sp.ConvertCSRToDense(0);

	Proj_per_cluster.RemoveLowerDense();
	Proj_per_cluster.mtype = espreso::MatrixType::REAL_SYMMETRIC_POSITIVE_DEFINITE;

	DenseSolverMKL ProjSolver;

	ProjSolver.ImportMatrix(Proj_per_cluster);

	ProjSolver.Factorization("Geneo projector factorization");

	exit(0);

	SEQ_VECTOR<double> y_out, x_in;

	#pragma omp parallel for
	for (size_t d = 0; d < cluster->domains.size(); d++) {

		SEQ_VECTOR < double > x_in_tmp ( cluster->domains[d].B1_comp_dom.rows, 0.0 );

		for (size_t i = 0; i < cluster->domains[d].lambda_map_sub_local.size(); i++)
			x_in_tmp[i] = x_in[ cluster->domains[d].lambda_map_sub_local[i]] * cluster->domains[d].B1_scale_vec[i]; // includes B1 scaling

		cluster->domains[d].B1t_DirPr.MatVec (x_in_tmp, cluster->x_prim_cluster1[d], 'N');
		//cluster.domains[d].Prec.MatVec(cluster.x_prim_cluster1[d], cluster.x_prim_cluster2[d],'N');
		cluster->domains[d].Prec.DenseMatVec(cluster->x_prim_cluster1[d], cluster->x_prim_cluster2[d],'N');

	}

	std::fill( cluster->compressed_tmp.begin(), cluster->compressed_tmp.end(), 0.0);
	SEQ_VECTOR < double > y_out_tmp;
	for (size_t d = 0; d < cluster->domains.size(); d++) {
		y_out_tmp.resize( cluster->domains[d].B1_comp_dom.rows );

		cluster->domains[d].B1t_DirPr.MatVec (cluster->x_prim_cluster2[d], y_out_tmp, 'T', 0, 0, 0.0); // will add (summation per elements) all partial results into y_out

	}

	//solver->All_Reduce_lambdas_compB(cluster, cluster->compressed_tmp, y_out);

*/
}

void FETISolver::setup_Preconditioner() {
// Load Matrix K, Regularization
		 TimeEvent timeRegKproc(string("Solver - Setup preconditioners")); timeRegKproc.start();
		 ESLOG(MEMORY) << "Before - Setup preconditioners - process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";

		 TimeEvent KregMem(string("Solver - Setup preconditioners mem. [MB]")); KregMem.startWithoutBarrier(GetProcessMemory_u());
		 ESINFO(PROGRESS3) << "Setup preconditioners";

		cluster->SetupPreconditioner();

		 KregMem.endWithoutBarrier(GetProcessMemory_u()); //KregMem.printLastStatMPIPerNode();

		 ESLOG(MEMORY) << "After - Setup preconditioners " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";
		 timeRegKproc.endWithBarrier();
		 timeEvalMain.addEvent(timeRegKproc);
}

void FETISolver::setup_FactorizationOfStiffnessMatrices() {
// K Factorization
		 TimeEvent timeSolKproc(string("Solver - K factorization")); timeSolKproc.start();
		 TimeEvent KFactMem(string("Solver - K factorization mem. [MB]")); KFactMem.startWithoutBarrier(GetProcessMemory_u());
		 ESINFO(PROGRESS3) << "Factorize K";

		cluster->SetupKsolvers();

		 KFactMem.endWithoutBarrier(GetProcessMemory_u()); //KFactMem.printLastStatMPIPerNode();
		 ESLOG(MEMORY) << "After K solver setup process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";
		 timeSolKproc.endWithBarrier();
		 timeEvalMain.addEvent(timeSolKproc);
}

void FETISolver::setup_SetDirichletBoundaryConditions() {
// Set Dirichlet Boundary Condition

		 TimeEvent timeSetInitialCondition(string("Solver - Set Dirichlet Boundary Condition"));
		 timeSetInitialCondition.start();

		#pragma omp parallel for
		for (size_t d = 0; d < cluster->domains.size(); d++) {
			cluster->domains[d]->vec_c  = instance->B1c[d];
			cluster->domains[d]->vec_lb = instance->LB[d];
		}

		 timeSetInitialCondition.endWithBarrier();
		 timeEvalMain.addEvent(timeSetInitialCondition);
}

void FETISolver::setup_CreateG_GGt_CompressG() {

		 TimeEvent timeSolPrec(string("Solver - FETI Preprocessing")); timeSolPrec.start();

		 ESLOG(MEMORY) << "Solver Preprocessing - HFETI with regularization from K matrix";
		 ESLOG(MEMORY) << "process " << environment->MPIrank << " uses "<< Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";

		 TimeEvent G1_perCluster_time("Setup G1 per Cluster time   - preprocessing"); G1_perCluster_time.start();

		 TimeEvent G1_perCluster_mem("Setup G1 per Cluster memory - preprocessing"); G1_perCluster_mem.startWithoutBarrier(GetProcessMemory_u());
		cluster->Create_G_perCluster();

		 G1_perCluster_time.end(); G1_perCluster_time.printStatMPI();
		 G1_perCluster_mem.endWithoutBarrier(GetProcessMemory_u()); G1_perCluster_mem.printStatMPI();

		 ESLOG(MEMORY) << "Created G1 per cluster";
		 ESLOG(MEMORY) << "Before HFETI create GGt process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";

		 TimeEvent solver_Preprocessing_time("Setup GGt time   - preprocessing"); solver_Preprocessing_time.start();
		 TimeEvent solver_Preprocessing_mem("Setup GGt memory - preprocessing"); solver_Preprocessing_mem.start();

		solver->Preprocessing(*cluster);

		 solver_Preprocessing_time.end(); solver_Preprocessing_time.printStatMPI();
		 solver_Preprocessing_mem.end();  solver_Preprocessing_mem.printStatMPI();

		 ESLOG(MEMORY) << "Create GGtInv";
		 ESLOG(MEMORY) << "process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";

		 TimeEvent solver_G1comp_time("Setup G1 compression time   - preprocessing"); solver_G1comp_time.start();
		 TimeEvent solver_G1comp_mem("Setup G1 compression memory - preprocessing");  solver_G1comp_mem.start();

		cluster->Compress_G1(); // Compression of Matrix G1 to work with compressed lambda vectors

		 solver_G1comp_time.end(); solver_G1comp_time.printStatMPI();
		 solver_G1comp_mem.end();  solver_G1comp_mem.printStatMPI();
		 ESLOG(MEMORY) << "G1 compression";
		 ESLOG(MEMORY) << "process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
		 ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";

		 timeSolPrec.endWithBarrier(); timeEvalMain.addEvent(timeSolPrec);

}

void FETISolver::setup_InitClusterAndSolver( )
{

	cluster->SetupCommunicationLayer();

	// *** Iter Solver Set-up ***************************************************************************************************************************
	solver->CG_max_iter 		= configuration.iterations;
	solver->USE_GGtINV  		= 1;
	solver->epsilon 			= configuration.epsilon;
	solver->USE_PREC 			= configuration.preconditioner;

	solver->USE_KINV 			= configuration.use_schur_complement ? 1 : 0;
	solver->PAR_NUM_THREADS 	= environment->PAR_NUM_THREADS;
	solver->SOLVER_NUM_THREADS  = environment->SOLVER_NUM_THREADS;

	switch (configuration.method) {
	case ESPRESO_METHOD::TOTAL_FETI:
		solver->USE_HFETI = false;
		break;
	case ESPRESO_METHOD::HYBRID_FETI:
		solver->USE_HFETI = true;
		break;
	default:
		ESINFO(GLOBAL_ERROR) << "Unsupported FETI METHOD";
	}

}

// TODO: const parameters
void FETISolver::init(const std::vector<int> &neighbours)
{
#ifdef READEX_LEVEL_1
	READEX_REGION_START(REG_FETISolver_Init, "FETISolver--Init", SCOREP_USER_REGION_TYPE_COMMON);
#endif

	//mkl_cbwr_set(MKL_CBWR_COMPATIBLE);

	// Overall Linear Solver Time measurement structure
	timeEvalMain.totalTime.startWithBarrier();

	// *** Initialize the Cluster and Solver structures
	setup_InitClusterAndSolver();

	// *** Setup B0 matrix
	// setup_B0Matrices();

	// *** Setup B1 matrix
	// setup_B1Matrices();

	// *** Setup R matrix
	// setup_KernelMatrices();

	// *** Set Dirichlet Boundary Condition
	// setup_SetDirichletBoundaryConditions();

	// *** if TFETI is used or if HTFETI and analytical kernel are used we can compute GGt here - between solution in terms of peak memory
	if (  !(cluster->USE_HFETI == 1 && configuration.regularization == REGULARIZATION::NULL_PIVOTS)  ) {
		setup_CreateG_GGt_CompressG();
	}

#ifdef READEX_LEVEL_1
	READEX_REGION_START(REG_FETISolver_Create_Dirichlet_Prec, "FETISolver--Create_Dirichlet_Prec", SCOREP_USER_REGION_TYPE_COMMON);
#endif

	// *** setup all preconditioners
	setup_Preconditioner();

	// *** Computation of the Schur Complement
	setup_LocalSchurComplement();

#ifdef READEX_LEVEL_1
	READEX_REGION_STOP(REG_FETISolver_Create_Dirichlet_Prec);
#endif

	// *** K Factorization
	setup_FactorizationOfStiffnessMatrices();

	// *** Setup Hybrid FETI part of the solver
	setup_HTFETI();

	// *** For algebraic kernels, GGt needs to be computed after HTFETI preprocessing
	if (cluster->USE_HFETI == 1 && configuration.regularization == REGULARIZATION::NULL_PIVOTS) {
		setup_CreateG_GGt_CompressG();
	}

	// Setup Conj Projector

	if ( configuration.conj_projector == CONJ_PROJECTOR::GENEO ) {
		// solver->CreateConjProjector(*cluster);
	}

	// Cleanup of unnecessary objects
	// TODO: This can be a problem in some cases - need to be verified
	cluster->my_lamdas_map_indices.clear();

	#pragma omp parallel for
	for (size_t d = 0; d < cluster->domains.size(); d++) {
		cluster->domains[d]->B1.Clear();
	}

	ESLOG(MEMORY) << "End of preprocessing - process " << environment->MPIrank << " uses " << Measure::processMemory() << " MB";
	ESLOG(MEMORY) << "Total used RAM " << Measure::usedRAM() << "/" << Measure::availableRAM() << " [MB]";

#ifdef READEX_LEVEL_1
	READEX_REGION_STOP(REG_FETISolver_Init);
#endif

}

void FETISolver::Solve( std::vector < std::vector < double > >  & f_vec,
		                  std::vector < std::vector < double > >  & prim_solution)
{

	std::vector < std::vector < double > > dual_solution;
	Solve(f_vec, prim_solution, dual_solution);
}

void FETISolver::Solve( std::vector < std::vector < double > >  & f_vec,
		                  std::vector < std::vector < double > >  & prim_solution,
		                  std::vector < std::vector < double > > & dual_solution) {
#ifdef READEX_LEVEL_1
	READEX_REGION_START(REG_FETISolver_Solve, "FETISolver--Solve", SCOREP_USER_REGION_TYPE_COMMON);
#endif

	 	 TimeEvent timeSolCG(string("Solver - CG Solver runtime"));
	 	 timeSolCG.start();

	 solver->Solve    ( *cluster, f_vec, prim_solution, dual_solution );

	 	 timeSolCG.endWithBarrier();
	 	 timeEvalMain.addEvent(timeSolCG);

#ifdef READEX_LEVEL_1
	READEX_REGION_STOP(REG_FETISolver_Solve);
#endif
}

void FETISolver::Postprocessing( ) {

}

void FETISolver::finalize() {

	// Show Linear Solver Runtime Evaluation
	solver->preproc_timing.printStatsMPI();
	solver->timing.printStatsMPI();
	solver->postproc_timing.printStatsMPI();
	solver->timeEvalAppa.printStatsMPI();
	solver->timeEvalProj.printStatsMPI();

	if ( solver->USE_PREC != ESPRESO_PRECONDITIONER::NONE ) solver->timeEvalPrec.printStatsMPI();

	//TODO: Fix timing:  if ( cluster->USE_HFETI == 1 ) cluster->ShowTiming();

	 timeEvalMain.totalTime.endWithBarrier();
	 timeEvalMain.printStatsMPI();
}

void FETISolver::CheckSolution( vector < vector < double > > & prim_solution ) {
    // *** Solutin correctnes test **********************************************************************************************

	//	double max_v = 0.0;
//		for (eslocal i = 0; i < number_of_subdomains_per_cluster; i++)
//			for (size_t j = 0; j < prim_solution[i].size(); j++)
//				if ( fabs ( prim_solution[i][j] ) > max_v) max_v = fabs( prim_solution[i][j] );
//
//	TimeEvent max_sol_ev ("Max solution value "); max_sol_ev.startWithoutBarrier(0.0); max_sol_ev.endWithoutBarrier(max_v);
//
//	double max_vg;
//	MPI_Reduce(&max_v, &max_vg, 1, MPI_DOUBLE, MPI_MAX, 0, environment->MPICommunicator );
//	ESINFO(DETAILS) << "Maxvalue in solution = " << std::setprecision(12) << max_vg;

	//max_sol_ev.printLastStatMPIPerNode(max_vg);
	// *** END - Solutin correctnes test ******************************************************************************************

}

void FETISolver::Preprocessing( std::vector < std::vector < eslocal > > & lambda_map_sub) {
/*
#ifdef READEX_LEVEL_1
	READEX_REGION_START(REG_FETISolver_Preprocessing, "FETISolver--Preprocessing", SCOREP_USER_REGION_TYPE_COMMON);
#endif

#ifdef READEX_LEVEL_1
	READEX_REGION_STOP(REG_FETISolver_Preprocessing);
#endif
*/
}


#include <frovedis.hpp>
#include <frovedis/ml/fpm/fp_growth.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

int main(int argc, char* argv[]){
    frovedis::use_frovedis use(argc, argv);
    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "produce help message")
        ("input,i" , value<std::string>(), "input data path containing transaction dataframe.")
        ("test-input,t" , value<std::string>(), "test input data path containing transaction dataframe.")
        ("output,o" , value<std::string>(), "output path for saving model.")
        ("min-support,s", value<double>(), "minimal support level of the frequent pattern. [default: 0.2]")
        ("min-confidence,c", value<double>(), "minimum confidence value for rule mining [default: 0.5]")
        ("tid-dtype", value<std::string>(), "how to load tid column (0th) from input data (as int, long etc.) [default: int]")
        ("item-dtype", value<std::string>(), "how to load item column (1st) from input data (as int, long, string etc.) [default: int]")
        ("tree-depth", value<int>(), "required depth till which tree needs to be constructed (>=1) [default: INT_MAX]")
        ("compression-point", value<int>(), "ith point from which trees will be compressed (>=2) [default: 4]")
        ("mem-opt-level", value<int>(), "memory opt level to use (either 0 or 1) [default: 0]")
        ("verbose", "set loglevel to DEBUG")
        ("verbose2", "set loglevel to TRACE");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().
          run(), argmap);
    notify(argmap);

    std::string data_p, out_p, test_p, tid_dtype = "int", item_dtype = "int";
    double min_support = 0.2, conf = 0.5;
    int tree_depth = std::numeric_limits<int>::max();
    int compression_point = 4;
    int mem_opt_level = 0;

    if(argmap.count("help")){
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("input")){
      data_p = argmap["input"].as<std::string>();
    }
    else {
      cerr << "input is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }

    if(argmap.count("test-input")){
      test_p = argmap["test-input"].as<std::string>();
    }
    else {
      cerr << "test input is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }

    if(argmap.count("output")){
      out_p = argmap["output"].as<std::string>();
    }
    else {
      cerr << "output is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }

    if(argmap.count("min-support")){
       min_support = argmap["min-support"].as<double>();
    }

    if(argmap.count("min-confidence")){
       conf = argmap["min-confidence"].as<double>();
    }

    if(argmap.count("tid-dtype")){
      tid_dtype = argmap["tid-dtype"].as<std::string>();
    }

    if(argmap.count("item-dtype")){
      item_dtype = argmap["item-dtype"].as<std::string>();
    }

    if(argmap.count("tree-depth")){
       tree_depth = argmap["tree-depth"].as<int>();
    }

    if(argmap.count("compression-point")){
       compression_point = argmap["compression-point"].as<int>();
    }

    if(argmap.count("mem-opt-level")){
       mem_opt_level = argmap["mem-opt-level"].as<int>();
    }

    if(argmap.count("verbose")){
      set_loglevel(DEBUG);
    }

    if(argmap.count("verbose2")){
      set_loglevel(TRACE);
    }

    try {
      auto t = make_dftable_loadtext(data_p,
                                 {tid_dtype, item_dtype},
                                 {"trans_id", "item"});
      auto test_t = make_dftable_loadtext(test_p,
                                 {tid_dtype, item_dtype},
                                 {"trans_id", "item"});
      time_spent t_tree(INFO), t_rule(INFO), t_trans(INFO);
      t_tree.lap_start();
      auto model = grow_fp_tree(t, min_support, tree_depth,
                                compression_point, mem_opt_level);
      t_tree.lap_stop();
      t_tree.show_lap("generate freq-itemsets: ");
      auto xt = model.get_frequent_itemset();
      for ( auto i : xt) i.show();
      model.debug_print();
      std::cout << "tree-depth: " << model.get_depth()
                << "; FIS-count: " << model.get_count() << std::endl;
      t_rule.lap_start();
      auto rule = model.generate_rules(conf);
      t_rule.lap_stop();
      t_rule.show_lap("generate_rules: ");
      rule.debug_print();
      std::cout << "rule-count: " << rule.get_count() << std::endl;

      t_trans.lap_start();
      auto trans = model.transform(test_t);
      t_trans.lap_stop();
      t_trans.show_lap("Transform: ");
      trans.show_all();

      make_directory(out_p);
      rule.save(out_p + "/rule");
      model.save(out_p + "/model");
      rule.load(out_p + "/rule");
      model.load(out_p + "/model");
      return 0;
    }
    catch (std::exception& e) {
      std::cout << "exception caught: " << e.what() << std::endl;
    }
    return 0;
}


// Copyright (c) 2010, Lawrence Livermore National Security, LLC. Produced at
// the Lawrence Livermore National Laboratory. LLNL-CODE-443211. All Rights
// reserved. See file COPYRIGHT for details.
//
// This file is part of the MFEM library. For more information and source code
// availability see http://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the GNU Lesser General Public License (as published by the Free
// Software Foundation) version 2.1 dated February 1999.

/// This HYPRE library interface has been taken originally from MFEM and modified
/// to suit the needs for the MOAB library.
/// Modified by: Vijay Mahadevan

#ifndef MOAB_HYPREPARMATRIX
#define MOAB_HYPREPARMATRIX

#include "moab/MOABConfig.h"
#include "moab/Core.hpp"

#ifdef MOAB_HAVE_EIGEN
#include <Eigen/Core>
#include <Eigen/Sparse>
#else
#error Configure with Eigen3 enabled
#endif

#ifdef MOAB_HAVE_MPI
#include "moab/ParallelComm.hpp"

// hypre header files
// #include "HYPRE.h"
// #include "HYPRE_parcsr_ls.h"
// #include "HYPRE_MatvecFunctions.h"

#include "seq_mv.h"
#include "HypreParVector.hpp"
#include "_hypre_parcsr_mv.h"
#include "_hypre_parcsr_ls.h"
#include "interpreter.h"
#include "HYPRE_MatvecFunctions.h"

#include "temp_multivector.h"

#ifdef HYPRE_COMPLEX
#error "MOAB does not work with HYPRE's complex numbers support"
#endif

#include "hypre_parcsr.hpp"

namespace moab
{

  namespace internal
  {

// Convert a HYPRE_Int to int
    inline int to_int(HYPRE_Int i)
    {
#ifdef HYPRE_BIGINT
      MFEM_ASSERT(HYPRE_Int(int(i)) == i, "overflow converting HYPRE_Int to int");
#endif
      return int(i);
    }

  }

/// Wrapper for hypre's ParCSR matrix class
  class HypreParMatrix
  {
    private:
      /// The actual object
      HYPRE_IJMatrix A;
      hypre_ParCSRMatrix *A_parcsr;

      mv_InterfaceInterpreter *interpreter;

      // Does the object own the pointer A?
      char ParCSROwner;

      // Store the dimensions of the matrix
      int height, gnrows;
      int width, gncols;

      moab::ParallelComm *pcomm;
      char initialized;

      // Initialize with defaults. Does not initialize inherited members.
      void Init();

      // Delete all owned data. Does not perform re-initialization with defaults.
      void Destroy();

      friend class HypreSolver;

    public:
      typedef Eigen::VectorXd Vector;
      typedef Eigen::SparseMatrix<double, Eigen::RowMajor> MOABSparseMatrix;
      //typedef template Eigen::ArrayXX<T> Array2D<T>;

    public:
      /// An empty matrix to be used as a reference to an existing matrix
      HypreParMatrix(moab::ParallelComm *p_comm);

      /// Converts hypre's format to HypreParMatrix
      HypreParMatrix(HYPRE_IJMatrix a)
      {
        Init();
        A = a;
        height = GetNumRows();
        width = GetNumCols();
      }

      /** Creates block-diagonal square parallel matrix. Diagonal is given by diag
          which must be in CSR format (finalized). The new HypreParMatrix does not
          take ownership of any of the input arrays. */
      HypreParMatrix(moab::ParallelComm *p_comm, HYPRE_Int glob_size, HYPRE_Int *row_starts,
                     HYPRE_Int nnz_pr_diag = 0);

      /** Creates block-diagonal rectangular parallel matrix. Diagonal is given by
          diag which must be in CSR format (finalized). The new HypreParMatrix does
          not take ownership of any of the input arrays. */
      HypreParMatrix(moab::ParallelComm *p_comm,
                     HYPRE_Int global_num_rows,
                     HYPRE_Int global_num_cols,
                     HYPRE_Int *row_starts,
                     HYPRE_Int *col_starts,
                     HYPRE_Int nnz_pr_diag = 0,
                     HYPRE_Int onz_pr_diag = 0,
                     HYPRE_Int nnz_pr_offdiag = 0);

      /** Creates a general parallel matrix from a local CSR matrix on each
          processor described by the I, J and data arrays. The local matrix should
          be of size (local) nrows by (global) glob_ncols. The new parallel matrix
          contains copies of all input arrays (so they can be deleted). */
      // HypreParMatrix(MPI_Comm comm, int nrows, HYPRE_Int glob_nrows,
      //                HYPRE_Int glob_ncols, int *I, HYPRE_Int *J,
      //                double *data, HYPRE_Int *rows, HYPRE_Int *cols);

      /// Make this HypreParMatrix a reference to 'master'
      void MakeRef(const HypreParMatrix &master);

      /// MPI communicator
      moab::ParallelComm* GetParallelCommunicator() const { return pcomm; }

      /// Typecasting to hypre's HYPRE_IJMatrix*
      operator HYPRE_IJMatrix() { return A; }
      /// Typecasting to hypre's HYPRE_ParCSRMatrix, a.k.a. void *
      operator HYPRE_ParCSRMatrix() { return (HYPRE_ParCSRMatrix) A_parcsr; }

      /// Changes the ownership of the matrix
      HYPRE_IJMatrix StealData();
      /// Changes the ownership of the matrix to A
      void StealData(HypreParMatrix &A);

      /** If the HypreParMatrix does not own the row-starts array, make a copy of
          it that the HypreParMatrix will own. If the col-starts array is the same
          as the row-starts array, col-starts is also replaced. */
      // void CopyRowStarts();
      /** If the HypreParMatrix does not own the col-starts array, make a copy of
          it that the HypreParMatrix will own. If the row-starts array is the same
          as the col-starts array, row-starts is also replaced. */
      // void CopyColStarts();

      /// Returns the global number of nonzeros
      inline HYPRE_Int NNZ() { return A_parcsr->num_nonzeros; }
      /// Returns the row partitioning
      inline HYPRE_Int *RowPart() { return A->row_partitioning; }
      /// Returns the column partitioning
      inline HYPRE_Int *ColPart() { return A->col_partitioning; }
      /// Returns the global number of rows
      inline HYPRE_Int M() { return A->global_num_rows; }
      /// Returns the global number of columns
      inline HYPRE_Int N() { return A->global_num_cols; }
      /// Returns the global number of rows
      inline HYPRE_Int FirstRow() { return A->global_first_row; }
      /// Returns the global number of columns
      inline HYPRE_Int FirstCol() { return A->global_first_col; }

      /// Get the local diagonal of the matrix.
      void GetDiag(Vector &diag) const;
      /// Get the local diagonal block. NOTE: 'diag' will not own any data.
      void GetDiag(MOABSparseMatrix &diag) const;
      /// Get the local off-diagonal block. NOTE: 'offd' will not own any data.
      void GetOffd(MOABSparseMatrix &offd, HYPRE_Int *&cmap) const;

      /** Split the matrix into M x N equally sized blocks of parallel matrices.
          The size of 'blocks' must already be set to M x N. */
      // void GetBlocks(Eigen::Array<HypreParMatrix*,Eigen::Dynamic,Eigen::Dynamic> &blocks,
      //                bool interleaved_rows = false,
      //                bool interleaved_cols = false) const;

      /// Returns the transpose of *this
      // HypreParMatrix * Transpose();

      /** Destroy and resize block-diagonal square parallel matrix. Diagonal is given by diag
          which must be in CSR format (finalized). The new HypreParMatrix does not
          take ownership of any of the input arrays. */
      void resize(HYPRE_Int glob_size,
                  HYPRE_Int *row_starts,
                  HYPRE_Int nnz_pr_diag = 0,
                  HYPRE_Int nnz_pr_offdiag = 0);

      /** Destroy and resize block-diagonal rectangular parallel matrix. Diagonal is given by
          diag which must be in CSR format (finalized). The new HypreParMatrix does
          not take ownership of any of the input arrays. */
      void resize(HYPRE_Int global_num_rows,
                  HYPRE_Int global_num_cols,
                  HYPRE_Int *row_starts,
                  HYPRE_Int *col_starts,
                  HYPRE_Int *nnz_pr_diag = NULL,
                  HYPRE_Int *onz_pr_diag = NULL,
                  HYPRE_Int nnz_pr_offdiag = 0);

      /// Returns the number of rows in the diagonal block of the ParCSRMatrix
      int GetNumRows() const
      {
        return internal::to_int(
                 hypre_CSRMatrixNumRows(hypre_ParCSRMatrixDiag(A_parcsr)));
      }

      /// Returns the number of columns in the diagonal block of the ParCSRMatrix
      int GetNumCols() const
      {
        return internal::to_int(
                 hypre_CSRMatrixNumCols(hypre_ParCSRMatrixDiag(A_parcsr)));
      }

      /// Computes y = alpha * A * x + beta * y
      HYPRE_Int Mult(HypreParVector &x, HypreParVector &y,
                     double alpha = 1.0, double beta = 0.0);
      /// Computes y = alpha * A * x + beta * y
      HYPRE_Int Mult(HYPRE_ParVector x, HYPRE_ParVector y,
                     double alpha = 1.0, double beta = 0.0);
      /// Computes y = alpha * A^t * x + beta * y
      HYPRE_Int MultTranspose(HypreParVector &x, HypreParVector &y,
                              double alpha = 1.0, double beta = 0.0);

      void Mult(double a, const HypreParVector &x, double b, HypreParVector &y) const;
      void MultTranspose(double a, const HypreParVector &x, double b, HypreParVector &y) const;

      // virtual void Mult(const Vector &x, Vector &y) const
      // { Mult(1.0, x, 0.0, y); }
      // virtual void MultTranspose(const Vector &x, Vector &y) const
      // { MultTranspose(1.0, x, 0.0, y); }

      /** The "Boolean" analog of y = alpha * A * x + beta * y, where elements in
          the sparsity pattern of the matrix are treated as "true". */
      // void BooleanMult(int alpha, int *x, int beta, int *y)
      // {
      //    internal::hypre_ParCSRMatrixBooleanMatvec(A_parcsr, alpha, x, beta, y);
      // }

      /** Multiply A on the left by a block-diagonal parallel matrix D. Return
          a new parallel matrix, D*A. If D has a different number of rows than A,
          D's row starts array needs to be given (as returned by the methods
          GetDofOffsets/GetTrueDofOffsets of ParFiniteElementSpace). The new matrix
          D*A uses copies of the row-, column-starts arrays, so "this" matrix and
          "row_starts" can be deleted.
          NOTE: this operation is local and does not require communication. */
      // HypreParMatrix* LeftDiagMult(const MOABSparseMatrix &D,
      //                              HYPRE_Int* row_starts = NULL) const;

      /// Scale the local row i by s(i).
      void ScaleRows(const Vector &s);
      /// Scale the local row i by 1./s(i)
      void InvScaleRows(const Vector &s);
      /// Scale all entries by s: A_scaled = s*A.
      void operator*=(double s);

      /// Remove values smaller in absolute value than some threshold
      void Threshold(double threshold = 0.0);

      /// If a row contains only zeros, set its diagonal to 1.
      void EliminateZeroRows() { hypre_ParCSRMatrixFixZeroRows(A_parcsr); }

      /** Eliminate rows and columns from the matrix, and rows from the vector B.
          Modify B with the BC values in X. */
      void EliminateRowsCols(const std::vector<HYPRE_Int> &rows_cols, const HypreParVector &X,
                             HypreParVector &B);

      /** Eliminate rows and columns from the matrix and store the eliminated
          elements in a new matrix Ae (returned), so that the modified matrix and
          Ae sum to the original matrix. */
      HypreParMatrix *EliminateRowsCols(const std::vector<HYPRE_Int> &rows_cols);

      HYPRE_Int GetValues(const HYPRE_Int nrows, HYPRE_Int *ncols, HYPRE_Int *rows, HYPRE_Int *cols,
                          HYPRE_Complex *values);
      HYPRE_Int SetValues(const HYPRE_Int nrows, HYPRE_Int *ncols, const HYPRE_Int *rows,
                          const HYPRE_Int *cols, const HYPRE_Complex *values);
      HYPRE_Int AddToValues(const HYPRE_Int nrows, HYPRE_Int *ncols, const HYPRE_Int *rows,
                            const HYPRE_Int *cols, const HYPRE_Complex *values);

      HYPRE_Int FinalizeAssembly();

      HYPRE_Int verbosity(const HYPRE_Int level);

      /// Prints the locally owned rows in parallel
      void Print(const char *fname, HYPRE_Int offi = 0, HYPRE_Int offj = 0);
      /// Reads the matrix from a file
      void Read(const char *fname);

      /// Calls hypre's destroy function
      virtual ~HypreParMatrix() { Destroy(); }

      /// Returns the matrix A * B
      friend HypreParMatrix *ParMult(HypreParMatrix *A, HypreParMatrix *B);

      /// Returns the matrix P^t * A * P
      friend HypreParMatrix *RAP(HypreParMatrix *A, HypreParMatrix *P);
      /// Returns the matrix Rt^t * A * P
      friend HypreParMatrix *RAP(HypreParMatrix *Rt, HypreParMatrix *A, HypreParMatrix *P);

      /** Eliminate essential BC specified by 'ess_dof_list' from the solution X to
          the r.h.s. B. Here A is a matrix with eliminated BC, while Ae is such that
          (A+Ae) is the original (Neumann) matrix before elimination. */
      friend void EliminateBC(HypreParMatrix &A, HypreParMatrix &Ae,
                              const std::vector<int> &ess_dof_list,
                              const HypreParVector &X, HypreParVector &B);

      friend class HypreSolver;

  };

}

#endif // MOAB_HAVE_MPI

#endif


#include "cMain.hpp"
#include <boost/range/irange.hpp>

cMain::cMain() : wxFrame(nullptr, wxID_ANY, "Mineweeper on wxWidgets!", wxPoint(30, 30), wxSize(800, 600))
{
    btn = new wxButton*[nFieldWidth * nFieldHeight];
    wxGridSizer *grid = new wxGridSizer(nFieldWidth, nFieldHeight, 0, 0);

    nField = new int[nFieldWidth * nFieldHeight];

    wxFont font(24, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD, false);

    for (auto x : boost::irange(0,nFieldWidth))
        for (auto y : boost::irange(0, nFieldHeight))
        {
            btn[y*nFieldWidth + x] = new wxButton(this, 10000 + (y * nFieldWidth +x));
            btn[y*nFieldWidth + x]->SetFont(font);
            btn[y*nFieldWidth + x]->SetForegroundColour(*wxBLUE);
            btn[y*nFieldWidth + x]->SetBackgroundColour(*wxWHITE);
            grid->Add(btn[y*nFieldWidth + x], 1, wxEXPAND | wxALL);

            // bind this button to an event (in this case they are all binded to the same events)
            btn[y*nFieldWidth + x]->Bind(wxEVT_COMMAND_BUTTON_CLICKED, &cMain::OnButtonClicked, this);
            // set the default value for each location in the mine field array
            nField[y*nFieldWidth + x] = 0;
        }

    this->SetSizer(grid);
    grid->Layout();
}

cMain::~cMain()
{
    /* I don't want memory leaks */
    delete[]btn;
}

void cMain::OnButtonClicked(wxCommandEvent &evt)
{
    // Get coordinate of button in field array
    int x = (evt.GetId() - 10000) % nFieldWidth;
    int y = (evt.GetId() - 10000) / nFieldWidth;

    // generate the mine field on the very first click of the user
    if (bFirstClick)
    {
        // total number of mines
        int mines = 30;

        while (mines)
        {
            int rx = rand() % nFieldWidth;
            int ry = rand() % nFieldHeight;

            if (nField[ry * nFieldWidth + rx] == 0 && rx != x && ry != y)
            {
                nField[ry * nFieldWidth + rx] = -1;
                mines--;
            }
        }

        // do not generate the mine field again
        bFirstClick = false;
    }

    // disable button, preventing it being pressed again
    btn[y*nFieldWidth + x]->Enable(false);
    btn[y*nFieldWidth + x]->SetBackgroundColour(*wxLIGHT_GREY);

    // check if player hit a mine
    if (nField[y*nFieldWidth + x] == -1)
    {
        btn[y*nFieldWidth + x]->SetBackgroundColour(*wxBLACK);
        btn[y*nFieldWidth + x]->SetForegroundColour(*wxRED);
        btn[y*nFieldWidth + x]->SetLabel("X");

        wxMessageBox("Bomb went off - Game Over !!");

        // reset game
        bFirstClick = true;
        for (auto x : boost::irange(0,nFieldWidth))
            for (auto y : boost::irange(0, nFieldHeight))
            {
                nField[y*nFieldWidth + x] = 0;
                btn[y*nFieldWidth + x]->SetLabel("");
                btn[y*nFieldWidth + x]->Enable(true);
                btn[y*nFieldWidth + x]->SetBackgroundColour(*wxWHITE);
                btn[y*nFieldWidth + x]->SetForegroundColour(*wxBLUE);
            }
    }
    else
    {
        // count neighbouring mines
        int mine_cntr = 0;
        for (auto i : boost::irange(-1, 2))
            for (auto j : boost::irange(-1, 2))
            {
                if (x + i >= 0 && x + i < nFieldWidth && y + j >= 0 && y + j < nFieldHeight)
                    if (nField[(y + j)*nFieldWidth + (x + i)] == -1)
                        mine_cntr++;
            }

        // update button's label to show mine count if > 0
        if (mine_cntr > 0)
            btn[y*nFieldWidth + x]->SetLabel(std::to_string(mine_cntr));
    }

    evt.Skip();
}


#include <algorithm>

#include <boost/polygon/polygon.hpp>

#include <cbag/common/box_t_adapt.h>
#include <cbag/common/box_t_util.h>
#include <cbag/layout/geo_iterator.h>
#include <cbag/util/overload.h>

using namespace boost::polygon::operators;

namespace cbag {
namespace layout {

struct geo_iterator::helper {

    struct xform_visitor {
      public:
        const cbag::transformation &xform;

        xform_visitor(const cbag::transformation &xform) : xform(xform) {}

        template <typename T> void operator()(T &v) { bp::transform(v, xform); }
    };

    struct geo_visitor {
      public:
        geo_iterator &self;

        geo_visitor(geo_iterator &self) : self(self) {}

        bool operator()(const geo_instance &v) {
            self.inner =
                std::make_shared<geo_iterator>(v.begin_intersect(self.box, self.spx, self.spy));
            return true;
        }

        template <typename T> bool operator()(const T &v) {
            auto test_box = get_expand(self.box, std::max(self.spx, self.cur->spx),
                                       std::max(self.spy, self.cur->spy));
            if (bp::empty(v & test_box)) {
                ++self.cur;
                return true;
            }
            self.cur_val = geo_union(v);
            std::visit(xform_visitor(self.xform), self.cur_val.val);
            return false;
        }
    };

    // advance the iterator to the next point that
    static void get_val_reference(geo_iterator &self) {
        for (auto repeat = true; repeat;) {
            if (self.inner == nullptr) {
                // no inner loop
                if (self.cur == self.end) {
                    return;
                } else {
                    // still has item, check intersection
                    repeat = std::visit(geo_visitor(self), self.cur->val);
                }
            } else {
                // has inner loop
                if (self.inner->has_next()) {
                    // get inner item
                    self.cur_val = *(*self.inner);
                    std::visit(xform_visitor(self.xform), self.cur_val.val);
                    return;
                } else {
                    // no more inner items
                    self.inner.reset();
                    ++self.cur;
                }
            }
        }
    }
};

geo_iterator::geo_iterator() = default;

geo_iterator::geo_iterator(const box_t &box, offset_t spx, offset_t spy, geo_query_iter &&cur,
                           geo_query_iter &&end, const cbag::transformation &xform)
    : box(box), spx(spx), spy(spy), cur(std::move(cur)), end(std::move(end)), xform(xform) {
    helper::get_val_reference(*this);
}

bool geo_iterator::has_next() const { return cur != end; }

geo_iterator &geo_iterator::operator++() {
    if (inner == nullptr) {
        ++cur;
    } else {
        ++(*inner);
    }
    helper::get_val_reference(*this);
    return *this;
}

geo_iterator geo_iterator::operator++(int) {
    auto ans = *this;
    ++(*this);
    return ans;
}

geo_iterator::reference geo_iterator::operator*() const { return cur_val; }

bool geo_iterator::operator==(const geo_iterator &rhs) const {
    auto a = has_next();
    auto b = rhs.has_next();
    return (!a && !b) ||
           (a && b && box == rhs.box && spx == rhs.spx && spy == rhs.spy && cur == rhs.cur &&
            end == rhs.end && xform == rhs.xform && inner == rhs.inner);
}

bool geo_iterator::operator!=(const geo_iterator &rhs) const { return !(*this == rhs); }

} // namespace layout
} // namespace cbag


// This example program shows how to use dlib's implementation of the paper:
//    One Millisecond Face Alignment with an Ensemble of Regression Trees by
//    Vahid Kazemi and Josephine Sullivan, CVPR 2014
//
// http://www.csc.kth.se/~vahidk/papers/KazemiCVPR14.pdf
//

#include "drishti/core/drishti_stdlib_string.h" // FIRST!

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <dlib/data_io.h>
#include <dlib/image_transforms/assign_image.h>
#include <dlib/statistics/statistics.h>
#include <dlib/image_processing/shape_predictor.h>
#include <dlib/opencv/cv_image.h>
#include <dlib/vectorstream.h>
#include <dlib/serialize.h>
#include <dlib/data_io/load_image_dataset.h>
#include <dlib/opencv.h>

#include "drishti/core/Line.h"
#include "drishti/core/string_utils.h"
#include "drishti/ml/shape_predictor_archive.h"
#include "drishti/testlib/drishti_cli.h"

#include "drishti/core/drishti_cereal_pba.h"
#include "drishti/core/drishti_cv_cereal.h"

#include "cxxopts.hpp"

#include <iostream>

std::vector<std::vector<double>>
get_interocular_distances(const std::vector<std::vector<dlib::full_object_detection>>& objects);

cv::Point cv_point(const dlib::point& p)
{
    return cv::Point(p.x(), p.y());
}

cv::Rect cv_rect(const dlib::rectangle& r)
{
    return cv::Rect(cv_point(r.tl_corner()), cv_point(r.br_corner()));
}

dlib::rectangle dlib_rect(const cv::Rect& r)
{
    return {r.x, r.y, r.br().x, r.br().y};
}

dlib::point dlib_point(const cv::Point& p)
{
    return dlib::point(p.x, p.y);
}

int mine(int argc, char* argv[])
{
    const auto argumentCount = argc;

    bool doPreview = false;
    bool doThreads = false;
    bool doVerbose = false;
    bool doHelp = false;
    std::string sInput;
    std::string sModel;

    cxxopts::Options options("train_shape_predictor", "Command line interface for dlib shape_predictor training");

    // clang-format off
    options.add_options()
        ( "input", "Input filename list", cxxopts::value<std::string>(sInput))
        ( "model", "Model filename", cxxopts::value<std::string>(sModel))
        ( "preview", "Use preview window", cxxopts::value<bool>(doPreview))
        ( "threads", "Use worker threads when possible", cxxopts::value<bool>(doThreads))
        ( "verbose", "Print verbose diagnostics", cxxopts::value<bool>(doVerbose))
        ( "help", "Print the help message", cxxopts::value<bool>(doHelp));
    // clang-format on
    auto parseResult = options.parse(argc, argv);

    if((argumentCount <= 1) || parseResult.count("help"))
    {
        std::cout << options.help({""}) << std::endl;
        return 0;
    }

    if(sModel.empty())
    {
        std::cerr << "Must specify output *.dat model file."  << std::endl;
        exit(1);
    }

    if(sInput.empty())
    {
        std::cerr << "Must specify valid input file list."  << std::endl;
        exit(1);
    }

    const auto filenames = drishti::cli::expand(sInput);

    // Default deserialization:
    //dlib::shape_predictor sp;
    //dlib::deserialize( model.c_str() ) >> sp;

    drishti::ml::shape_predictor sp;
    load_cpb(sModel, sp);
    //int ellipse_count = sp.m_ellipse_count;

#if 0
    for(auto &f : filenames)
    {
        cv::Mat crop = cv::imread(f, cv::IMREAD_GRAYSCALE);

        auto img = dlib::cv_image<uint8_t>(crop);
        dlib::full_object_detection shape = sp(img, dlib::rectangle(0,0,crop.cols,crop.rows));

        cv::Mat canvas;
        cv::cvtColor(crop, canvas, cv::COLOR_GRAY2BGR);

        int count = shape.num_parts();
        count -= (ellipse_count * 5);
        std::vector<cv::Point> points;
        for(int j = 0; j < count; j++)
        {
            points.push_back(cv_point(shape.part(j)));
            cv::circle(canvas, points.back(), 4, {0,255,0}, -1, 8);
        }

        std::vector<cv::RotatedRect> ellipses;
        for(int j = count; j < shape.num_parts(); j+=5)
        {
            cv::RotatedRect e1;
            e1.center.x =    shape.part(j+0).x();
            e1.center.y =    shape.part(j+1).x();
            e1.size.width =  shape.part(j+2).x();
            e1.size.height = shape.part(j+3).x();
            e1.angle =       shape.part(j+4).x();
            ellipses.push_back(e1);

            std::cout << e1.center << " " << e1.size << " " << e1.angle << std::endl;

            cv::ellipse(canvas, e1, {0,255,0}, 1, 8);
        }

        cv::imshow("canvas", canvas);
        cv::waitKey(0);
    }

#else
    std::vector<std::vector<dlib::full_object_detection> > faces_train;
    dlib::array<dlib::array2d<uint8_t>> images_train;
    dlib::image_dataset_file source(sInput);
    source.skip_empty_images();
    load_image_dataset(images_train, faces_train, source);

    double elapsed = 0.0;
    for(int i = 0; i < images_train.size(); i++)
    {
        for(const auto &face : faces_train[i])
        {
            int64 tic = cv::getTickCount();
            dlib::full_object_detection shape = sp(images_train[i], face.get_rect());
            int64 toc = cv::getTickCount();
            elapsed += (toc - tic) / cv::getTickFrequency();

            cv::Mat image;
            if(doPreview)
            {
                image = dlib::toMat( images_train[i] );
                cv::cvtColor(image, image, cv::COLOR_GRAY2BGR);
                cv::rectangle(image, cv_rect(face.get_rect()), {0,255,0}, 1, 8);
            }

            std::vector<cv::Point> points;
            for(int j = 0; j < shape.num_parts(); j++)
            {
                points.push_back(cv_point(shape.part(j)));

                if(doPreview)
                {
                    cv::circle(image, points.back(), 2, {0,255,0}, -1, 8);
                }
            }

            if(doPreview)
            {
                cv::imshow("image", image), cv::waitKey(0);
            }
        }
    }

    std::cout << "elapsed: " << elapsed << std::endl;
#endif

    return 0;
}

int main(int argc, char *argv[])
{
    try
    {
        mine(argc, argv);
    }
    catch (std::exception& e)
    {
        std::cout << "\nexception thrown!" << std::endl;
        std::cout << e.what() << std::endl;
    }
}


/*
# Copyright 2018 HyphaROS Workshop
# Original Author: HaoChih, LIN (f44006076@gmail.com)
# Maintainer: HyphaROS Workshop (hypha.ros@gmail)
# Maintainer: PouChun, Kung (k3083518729@gmail.com)
#
# Website: https://hypharosworkshop.wordpress.com/
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
*/

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <Eigen/Core>
#include <Eigen/LU>
#include <tf/transform_broadcaster.h>

//---kbhit() header---
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>

#define pos_x 0
#define pos_y 1
#define pos_z 2
#define ori_x 3
#define ori_y 4
#define ori_z 5
#define ori_w 6

//--------------kbhit function model in Linux----------------
int kbhit(void)
{
  struct termios oldt, newt;
  int ch;
  int oldf;
  tcgetattr(STDIN_FILENO, &oldt);
  newt = oldt;
  newt.c_lflag &= ~(ICANON | ECHO);
  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
  oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
  fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
  ch = getchar();
  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
  fcntl(STDIN_FILENO, F_SETFL, oldf);
  if(ch != EOF)
  {
    ungetc(ch, stdin);
    return 1;
  }
  return 0;
}

using namespace Eigen;

class Pose_Conversion
{
  public:
    Pose_Conversion();

  private:
    int peirod;
    int publish_index;  // The index for detecting the PC2_callback function
    int publish_choice; // The index for deciding whether the PC2_scaled is published or not
    int ptam_loop;
    int scale_lock;
    float ptam_pose_cur[7];
    double slam_time_cur;
    double ptam_time_cur;
    double looprate;

    ros::NodeHandle n;
    ros::Subscriber lsdslam_pose_sub;
    ros::Subscriber tumptam_pose_sub;
    ros::Subscriber pointcloud_sub;
    ros::Publisher  pointcloud2_pub;

    Matrix4f P;
    Matrix4f R;
    Matrix3f dq2M;
    Vector4f Q;
    Vector4f S;
    Vector4f dx;
    Vector4f dq;

    void lsdslam_pose_CB(const geometry_msgs::PoseStampedConstPtr slam_msg);
    void tumptam_pose_CB(const geometry_msgs::PoseStampedConstPtr ptam_msg);
    void pointcloud_CB(const sensor_msgs::PointCloudConstPtr PC_msg);
};

Pose_Conversion::Pose_Conversion()
{
  pointcloud2_pub = n.advertise<sensor_msgs::PointCloud2>("pointcloud2_scaled", 1, true);
  pointcloud_sub = n.subscribe("/pointcloud", 1, &Pose_Conversion::pointcloud_CB, this);
  lsdslam_pose_sub = n.subscribe("/lsd_slam/pose", 1, &Pose_Conversion::lsdslam_pose_CB, this);
  tumptam_pose_sub = n.subscribe("/tum_ardrone/pose", 1, &Pose_Conversion::tumptam_pose_CB, this);

  ptam_pose_cur[0]=ptam_pose_cur[1]=ptam_pose_cur[2]=ptam_pose_cur[3]=ptam_pose_cur[4]=ptam_pose_cur[5]=ptam_pose_cur[6]=0.0;

  publish_index = 0;
  publish_choice = 0;
  ptam_loop = 0;
  peirod = 0.500;
  scale_lock = 0;
  slam_time_cur = 0.0;
  ptam_time_cur = 0.0;
  looprate = ros::Time::now().toSec();

  P << 0,0,0,0,
       0,0,0,0,
       0,0,0,0,
       0,0,0,0;
  R = P;

  dq2M << 0,0,0,
          0,0,0,
          0,0,0;

  Q << 0,0,0,0;
  S = dx = dq = Q;
}

void Pose_Conversion::lsdslam_pose_CB(const geometry_msgs::PoseStampedConstPtr slam_msg)
{
	slam_time_cur = slam_msg->header.stamp.toSec();

	//----Copy data from slam_pose and ptam_pose---
	float ptam_pose[7];
	ptam_pose[pos_x] = ptam_pose_cur[pos_x];
	ptam_pose[pos_y] = ptam_pose_cur[pos_y];
	ptam_pose[pos_z] = ptam_pose_cur[pos_z];
	ptam_pose[ori_x] = ptam_pose_cur[ori_x];
	ptam_pose[ori_y] = ptam_pose_cur[ori_y];
	ptam_pose[ori_z] = ptam_pose_cur[ori_z];
	ptam_pose[ori_w] = ptam_pose_cur[ori_w];

	float slam_pose[7];
	slam_pose[pos_x] = slam_msg->pose.position.x;
	slam_pose[pos_y] = slam_msg->pose.position.y;
	slam_pose[pos_z] = slam_msg->pose.position.z;
	slam_pose[ori_x] = slam_msg->pose.orientation.x;
	slam_pose[ori_y] = slam_msg->pose.orientation.y;
	slam_pose[ori_z] = slam_msg->pose.orientation.z;
	slam_pose[ori_w] = slam_msg->pose.orientation.w;

	//---Declare matrix and vector for least square calculating---
	Matrix4f temp_p; // q_lsd matrix
	MatrixXf temp_r(3,4);
	Vector3f xlsd_temp(0,0,0);

	//---Transform the tum_pose_ori to new tum_pose_ori---
	Vector4f qtf1(0.70710678, 0.70710678,0 ,0);
	Vector4f qtf1_inv(-0.70710678, -0.70710678,0 ,0);
	Matrix4f M_dq;
	Vector4f qtum(ptam_pose[ori_x],ptam_pose[ori_y],ptam_pose[ori_z],ptam_pose[ori_w]);
	Vector3f xtum(ptam_pose[pos_x],ptam_pose[pos_y],ptam_pose[pos_z]);

	M_dq << qtf1_inv(3), qtf1_inv(2),-qtf1_inv(1), qtf1_inv(0),
	       -qtf1_inv(2), qtf1_inv(3), qtf1_inv(0), qtf1_inv(1),
                qtf1_inv(1),-qtf1_inv(0), qtf1_inv(3), qtf1_inv(2),
               -qtf1_inv(0),-qtf1_inv(1),-qtf1_inv(2), qtf1_inv(3);

	qtum = M_dq*qtum;

	M_dq << qtum(3), qtum(2),-qtum(1), qtum(0),
	       -qtum(2), qtum(3), qtum(0), qtum(1),
                qtum(1),-qtum(0), qtum(3), qtum(2),
               -qtum(0),-qtum(1),-qtum(2), qtum(3);

	qtum = M_dq*qtf1; // Set up new tum_pose_ori !!

	//---Transform the lsd_pose to new lsd_pose---
	Vector4f qtf2(0.70710678, 0, 0, 0.70710678);		// x-axis 90 degree
	Vector4f qtf2_inv(-0.70710678, 0, 0, 0.70710678);
	Matrix4f M_dq2;
	Vector4f qlsd(slam_pose[ori_x],slam_pose[ori_y],slam_pose[ori_z],slam_pose[ori_w]);
	Vector3f xlsd(slam_pose[pos_x],slam_pose[pos_y],slam_pose[pos_z]);

	M_dq2 << qtf2(3), qtf2(2),-qtf2(1), qtf2(0),
	        -qtf2(2), qtf2(3), qtf2(0), qtf2(1),
                 qtf2(1),-qtf2(0), qtf2(3), qtf2(2),
                -qtf2(0),-qtf2(1),-qtf2(2), qtf2(3);

	qlsd = M_dq2*qlsd;

	M_dq2 << qlsd(3), qlsd(2),-qlsd(1), qlsd(0),
	        -qlsd(2), qlsd(3), qlsd(0), qlsd(1),
                 qlsd(1),-qlsd(0), qlsd(3), qlsd(2),
                -qlsd(0),-qlsd(1),-qlsd(2), qlsd(3);

	qlsd = M_dq2*qtf2_inv; // Set up new lsd_pose_ori !!

	Matrix3f R_qtf2;

	R_qtf2 <<  -2*qtf2(1)*qtf2(1)-2*qtf2(2)*qtf2(2)+1,    2*qtf2(0)*qtf2(1)-2*qtf2(2)*qtf2(3),    2*qtf2(0)*qtf2(2)+2*qtf2(1)*qtf2(3),
		      2*qtf2(0)*qtf2(1)+2*qtf2(2)*qtf2(3), -2*qtf2(0)*qtf2(0)-2*qtf2(2)*qtf2(2)+1,    2*qtf2(1)*qtf2(2)-2*qtf2(0)*qtf2(3),
		      2*qtf2(0)*qtf2(2)-2*qtf2(1)*qtf2(3),    2*qtf2(0)*qtf2(3)+2*qtf2(1)*qtf2(2), -2*qtf2(0)*qtf2(0)-2*qtf2(1)*qtf2(1)+1;

	xlsd = (R_qtf2.transpose())*xlsd;    // Set up new lsd_pose_pos !!

	if( scale_lock == 0)
	{
		//---Loop rate is 2 Hz---
		if( (ros::Time::now().toSec() - looprate) >= peirod && ptam_loop == 1)
		{
			//---Do the least square method---
			temp_p <<  qlsd(3), qlsd(2),-qlsd(1), qlsd(0),
				  -qlsd(2), qlsd(3), qlsd(0), qlsd(1),
		                   qlsd(1),-qlsd(0), qlsd(3), qlsd(2),
		                  -qlsd(0),-qlsd(1),-qlsd(2), qlsd(3);

			P = P + (temp_p.transpose())*temp_p;
			Q = Q + (temp_p.transpose())*qtum;

			if(P.determinant() > 0.000001)
			{
				dq = (P.inverse())*Q;
				dq = dq/(sqrt(dq(0)*dq(0) + dq(1)*dq(1) + dq(2)*dq(2) + dq(3)*dq(3) ));
			}

			dq2M <<  -2*dq(1)*dq(1)-2*dq(2)*dq(2)+1,    2*dq(0)*dq(1)-2*dq(2)*dq(3),    2*dq(0)*dq(2)+2*dq(1)*dq(3),
		                    2*dq(0)*dq(1)+2*dq(2)*dq(3), -2*dq(0)*dq(0)-2*dq(2)*dq(2)+1,    2*dq(1)*dq(2)-2*dq(0)*dq(3),
		                    2*dq(0)*dq(2)-2*dq(1)*dq(3),    2*dq(0)*dq(3)+2*dq(1)*dq(2), -2*dq(0)*dq(0)-2*dq(1)*dq(1)+1;

			temp_r << 1,0,0,0,
				  0,1,0,0,
	       			  0,0,1,0;

		        xlsd_temp = (dq2M.transpose())*xlsd;
			temp_r(0,3) = xlsd_temp(0);
	 		temp_r(1,3) = xlsd_temp(1);
			temp_r(2,3) = xlsd_temp(2);

			Matrix4f Rtemp;

			Rtemp = R+(temp_r.transpose())*temp_r;

			if( R(0,0) == 0)
			{
				R = Rtemp;
				S = S+(temp_r.transpose())*xtum;
			}
			else if(  (Rtemp/ (Rtemp(0,0)) ).determinant() > (R/ (R(0,0))).determinant() )
			{
				R = Rtemp;
				S = S+(temp_r.transpose())*xtum;
			}

			if( (R/ (R(0,0))).determinant() > 0.000001)
			{
				dx = (R.inverse())*S;
				publish_index = 1;
				ROS_INFO("Least-square successful !!");
			}

			std::stringstream ss;
			ss <<"\ndx:" << dx.transpose() << "\ndq:" << dq.transpose() << " \nx_lsd:" << xlsd.transpose() << "\nx_tum:" << xtum.transpose()<<"  \nTime:" << ros::Time::now() ;
			//ss <<"\ndx:" << dx.transpose() << "\nR:" << R << " \nS:" <<S.transpose();
			std::string s = ss.str();
			ROS_INFO(s.c_str());
			ptam_loop = 0;
			looprate = looprate + peirod;
		}
	}

	if( kbhit() )
	{
		int ch = getchar();
		if( ch == 'l')
		{
			scale_lock = 1;
			ROS_INFO("Scale Lock!! (Enter 'r' to unlock)");
		}
		if( ch == 'r')
			scale_lock = 0;
		if( ch == 'p' )
		{
			publish_choice = 1;
			ROS_INFO("Start to publish the PC2_scaled !!");
		}
		if( ch == 's' )
		{
			publish_choice = 0;
			ROS_INFO("Stop publishing the PC2_scaled !!");
		}
	}
	//---Broadcasting the TF (from map to shadow) by using lsd_pose---
	Matrix4f Mqlsd;
	Vector4f qlsd_tf_temp(0,0,0,0);
	Mqlsd << qlsd(3), qlsd(2),-qlsd(1), qlsd(0),
		-qlsd(2), qlsd(3), qlsd(0), qlsd(1),
                 qlsd(1),-qlsd(0), qlsd(3), qlsd(2),
                -qlsd(0),-qlsd(1),-qlsd(2), qlsd(3);

	qlsd_tf_temp = Mqlsd*dq;

	Vector3f xlsd_tf_temp(0,0,0);
	MatrixXf temp_tf(3,4);
	temp_tf << 1,0,0,0,
		   0,1,0,0,
       		   0,0,1,0;
	xlsd_tf_temp = (dq2M.transpose())*xlsd;
	temp_tf(0,3) = xlsd_tf_temp(0);
	temp_tf(1,3) = xlsd_tf_temp(1);
	temp_tf(2,3) = xlsd_tf_temp(2);

	xlsd_tf_temp = temp_tf*dx;

	static tf::TransformBroadcaster br;
	tf::Transform transform;
	transform.setOrigin(tf::Vector3(xlsd_tf_temp(0), xlsd_tf_temp(1), xlsd_tf_temp(2)));
	transform.setRotation(tf::Quaternion( qlsd_tf_temp(0), qlsd_tf_temp(1), qlsd_tf_temp(2), qlsd_tf_temp(3)));
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "shadow"));
}

void Pose_Conversion::tumptam_pose_CB(const geometry_msgs::PoseStampedConstPtr ptam_msg)
{
	ptam_loop = 1;
	ptam_time_cur = ptam_msg->header.stamp.toSec();
	ptam_pose_cur[pos_x] = ptam_msg->pose.position.x;
	ptam_pose_cur[pos_y] = ptam_msg->pose.position.y;
	ptam_pose_cur[pos_z] = ptam_msg->pose.position.z;
	ptam_pose_cur[ori_x] = ptam_msg->pose.orientation.x;
	ptam_pose_cur[ori_y] = ptam_msg->pose.orientation.y;
	ptam_pose_cur[ori_z] = ptam_msg->pose.orientation.z;
	ptam_pose_cur[ori_w] = ptam_msg->pose.orientation.w;
}

void Pose_Conversion::pointcloud_CB(const sensor_msgs::PointCloudConstPtr PC_msg)
{
	ROS_INFO("pointcloud_CB");
	if(publish_index == 1 && publish_choice == 1)
        {
		sensor_msgs::PointCloud cloud;
		cloud.header.stamp = ros::Time::now();
		cloud.header.frame_id = "map";
		cloud.points.resize(PC_msg->points.size());

		Matrix3f R_qtf2;
		Vector4f qtf2(0.70710678, 0, 0, 0.70710678);
		Vector3f x_temp(0,0,0);
		Vector3f x_ori(0,0,0);
		Vector3f x_new(0,0,0);
		MatrixXf temp_r(3,4);
		Matrix3f dq2M_cur;
		Vector4f dx_cur(0,0,0,0);

		dq2M_cur = dq2M;
		dx_cur = dx;

		for(int total=0; total < (PC_msg->points.size()) ; total++)
		{
			// Transform lsd_pose_pointcloud
			x_ori << PC_msg->points[total].x, PC_msg->points[total].y, PC_msg->points[total].z;

			R_qtf2 <<  -2*qtf2(1)*qtf2(1)-2*qtf2(2)*qtf2(2)+1,    2*qtf2(0)*qtf2(1)-2*qtf2(2)*qtf2(3),    2*qtf2(0)*qtf2(2)+2*qtf2(1)*qtf2(3),
				      2*qtf2(0)*qtf2(1)+2*qtf2(2)*qtf2(3), -2*qtf2(0)*qtf2(0)-2*qtf2(2)*qtf2(2)+1,    2*qtf2(1)*qtf2(2)-2*qtf2(0)*qtf2(3),
				      2*qtf2(0)*qtf2(2)-2*qtf2(1)*qtf2(3),    2*qtf2(0)*qtf2(3)+2*qtf2(1)*qtf2(2), -2*qtf2(0)*qtf2(0)-2*qtf2(1)*qtf2(1)+1;

			x_ori = (R_qtf2.transpose())*x_ori;    // Set up new lsd_pose_pointcloud !!

			// Transform lsd_pose_pointcloud from lsd_world to map
			temp_r << 1,0,0,0,
				  0,1,0,0,
	       			  0,0,1,0;

			x_temp = (dq2M_cur.transpose())*x_ori;
			temp_r(0,3) = x_temp(0);
	 		temp_r(1,3) = x_temp(1);
			temp_r(2,3) = x_temp(2);

			x_new = temp_r*dx_cur;

			cloud.points[total].x = x_new(0);
			cloud.points[total].y = x_new(1);
			cloud.points[total].z = x_new(2);
		}
		sensor_msgs::PointCloud2 cloud2;
		convertPointCloudToPointCloud2(cloud,cloud2);
		pointcloud2_pub.publish(cloud2);
		ROS_INFO("Publishing Scaled Point_Cloud2");
	}
	else
		ROS_INFO("Still un-invertible");
}

//------------Main function-----------------
int main(int argc, char **argv)
{
  ros::init(argc, argv, "conversion");
  ROS_INFO("START");
  ROS_INFO("l:lock the scale, r:unlock, p:publish PC2_scale, s:Stop publishing  :");
  Pose_Conversion pose_conversion;

  ros::AsyncSpinner spinner(3); // Use 3 threads
  spinner.start();
  ros::waitForShutdown();

  return 0;
}