#include <frovedis.hpp>
#include <frovedis/ml/recommendation/als.hpp>

#include <boost/program_options.hpp>
#include <stdlib.h>

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

// to let dvector<T>::saveline know the operator...
namespace frovedis {
ostream& operator<<(ostream& os, const pair<size_t, double>& d) {
  return os << d.first << ", " << d.second;
}
}

void do_train(const string& input, const string& output, int num_iteration,
              int factor, double alpha, double regParam, int seed,
              bool binary, double sf) {
  if(seed == -1) seed = abs(rand());
  crs_matrix<double> data;
  time_spent t(INFO);
  if(binary) data = make_crs_matrix_loadbinary<double>(input);
  else data = make_crs_matrix_load<double>(input);
  t.show("data load time: ");
  auto mfm = matrix_factorization_using_als::train(data, factor, num_iteration,
                                                   alpha, regParam, seed, sf);
  t.show("train time: ");
  if(binary) mfm.savebinary(output);
  else mfm.save(output);
  t.show("model save time: ");
}

void do_predict(const string& input, const string& output,
                int user_for_recommend, int product_for_recommend,
                int num_recommend, bool binary) {
  matrix_factorization_model<double> mfm;
  if(binary) mfm.loadbinary(input);
  else mfm.load(input);
  if(user_for_recommend >= 0 && product_for_recommend >= 0) {
    double r = mfm.predict(user_for_recommend, product_for_recommend);
    cout << r << endl;
  } else {
    if(user_for_recommend >= 0) {
      auto r = mfm.recommend_products(user_for_recommend, num_recommend);
      make_dvector_scatter(r).saveline(output);
    } else if(product_for_recommend >= 0) {
      auto r = mfm.recommend_users(product_for_recommend, num_recommend);
      make_dvector_scatter(r).saveline(output);
    } else {
      cerr << "either user or product is not specified for predict" << endl;
      exit(1);
    }
  }
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("predict,p", "predict mode")
    ("input,i", value<string>(), "input matrix / model")
    ("output,o", value<string>(), "output model or predict result (if both product and user is specified for prediction, output to stdout)")
    ("factor,f", value<int>(), "number of factor")
    ("num-iteration,n", value<int>(), "number of iteration")
    ("alpha,a", value<double>(), "learning rate")
    ("regularization-parameter,e", value<double>(), "regularization parameter")
    ("seed,s", value<int>(), "random seed (default 0; if -1, random)")
    ("similarity-factor", value<double>(), "similarity factor (default 1.0)")
    ("recommend-products,r", value<int>(),
     "recommend products for specified user")
    ("recommend-users,u", value<int>(),
     "recommend users for specified product")
    ("num-recommend,k", value<int>(), "number of products/users for recommend")
    ("verbose", "set loglevel to DEBUG")
    ("verbose2", "set loglevel to TRACE")
    ("binary,b", "use binary input (output is stdout or text)");

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

  bool ispredict = false;
  string input, output;
  int num_iteration = 100;
  int factor = 100;
  double alpha = 0.01;
  double regParam = 0.01;
  int seed = 0;
  int product_for_recommend = -1;
  int user_for_recommend = -1;
  int num_recommend = 10;
  bool binary = false;
  double sf = 0.1;

  if(argmap.count("help")){
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("predict")){
    ispredict = true;
  }

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

  if(argmap.count("factor")){
    factor = argmap["factor"].as<int>();
  } else {
    if(!ispredict) {
      cerr << "factor is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }

  if(argmap.count("num-iteration")){
    num_iteration = argmap["num-iteration"].as<int>();
  }

  if(argmap.count("alpha")){
    alpha = argmap["alpha"].as<double>();
  }

  if(argmap.count("regularization-parameter")){
    regParam = argmap["regularization-parameter"].as<double>();
  }

  if(argmap.count("similarity-factor")){
    sf = argmap["similarity-factor"].as<double>();
  }

  if(argmap.count("seed")){
    seed = argmap["seed"].as<int>();
  }

  // if both recommend-products and recommend-users are specified,
  // predict is called.
  // otherwise, predictProducts or predictUsers are called for prediction
  if(argmap.count("recommend-products")){
    user_for_recommend = argmap["recommend-products"].as<int>();
  }

  if(argmap.count("recommend-users")){
    product_for_recommend = argmap["recommend-users"].as<int>();
  }

  if(argmap.count("output")){
    output = argmap["output"].as<string>();
  } else {
    if(!(ispredict && user_for_recommend >= 0 && product_for_recommend >=0)) {
      cerr << "output is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }

  if(argmap.count("num-recommend")){
    num_recommend = argmap["num-recommend"].as<int>();
  }

  if(argmap.count("binary")){
    binary = true;
  }

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

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

  if(ispredict) {
    do_predict(input, output, user_for_recommend, product_for_recommend,
               num_recommend, binary);
  } else {
    do_train(input, output, num_iteration, factor, alpha,
             regParam, seed, binary, sf);
  }
}


/*
 * Copyright 2018, LAAS-CNRS
 * Author: Steve Tonneau
 */

#ifndef BEZIER_COM_TRAJ_DEFINITIONS_H
#define BEZIER_COM_TRAJ_DEFINITIONS_H

#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <ndcurves/bezier_curve.h>
#include <Eigen/Dense>

namespace bezier_com_traj {

typedef double value_type;
typedef Eigen::Matrix<value_type, 3, 3> Matrix3;
typedef Eigen::Matrix<value_type, 6, 3> Matrix63;
typedef Eigen::Matrix<value_type, 3, 9> Matrix39;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3> MatrixX3;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> MatrixXX;
typedef centroidal_dynamics::Vector3 Vector3;
typedef centroidal_dynamics::Vector6 Vector6;
typedef centroidal_dynamics::VectorX VectorX;

typedef Eigen::Ref<Vector3> Ref_vector3;
typedef Eigen::Ref<VectorX> Ref_vectorX;
typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
typedef Eigen::Ref<MatrixXX> Ref_matrixXX;

typedef const Eigen::Ref<const Vector3>& Cref_vector3;
typedef const Eigen::Ref<const Vector6>& Cref_vector6;
typedef const Eigen::Ref<const VectorX>& Cref_vectorX;
typedef const Eigen::Ref<const MatrixXX>& Cref_matrixXX;
typedef const Eigen::Ref<const MatrixX3>& Cref_matrixX3;

typedef Matrix63 matrix6_t;
typedef Vector6 point6_t;
typedef Matrix3 matrix3_t;
typedef Vector3 point3_t;

typedef Eigen::Vector3d point_t;
typedef const Eigen::Ref<const point_t>& point_t_tC;

/**
 * @brief waypoint_t a waypoint is composed of a  6*3 matrix that depend
 * on the variable x, and of a 6d vector independent of x, such that
 * each control point of the target bezier curve is given by pi = wix * x + wis
 */
typedef std::pair<matrix6_t, point6_t> waypoint6_t;
typedef std::pair<matrix3_t, point3_t> waypoint3_t;
typedef std::pair<Matrix39, point3_t> waypoint9_t;
struct waypoint_t;  // forward declaration

typedef ndcurves::bezier_curve<double, double, true, point_t> bezier_t;
typedef ndcurves::bezier_curve<double, double, true, waypoint_t> bezier_wp_t;
typedef ndcurves::bezier_curve<double, double, true, point6_t> bezier6_t;

typedef std::vector<std::pair<double, int> > T_time;
typedef T_time::const_iterator CIT_time;

typedef std::pair<double, point3_t> coefs_t;

}  // end namespace bezier_com_traj

#endif


/*
 *  Uuid.cpp
 *  Copyright 2021 ItJustWorksTM
 *
 *  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 <SMCE/Uuid.hpp>

#include <array>
#include <cstring>
#include <mutex>
#include <random>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>

namespace {

// Derived from https://codereview.stackexchange.com/a/109266
template <class T = std::mt19937_64, std::size_t N = T::state_size * sizeof(typename T::result_type)>
T ProperlySeededRandomEngine() noexcept {
    std::random_device source;
    auto make_iter = [&](std::size_t n) {
        return boost::make_transform_iterator(boost::counting_iterator<std::size_t>(n),
                                              [&](std::size_t) { return source(); });
    };
    std::seed_seq seeds(make_iter(0), make_iter((N - 1) / sizeof(source()) + 1));
    return T{seeds};
}

} // namespace

namespace smce {

std::string Uuid::to_hex() const noexcept {
    constexpr auto hexstr = "0123456789ABCDEF";
    std::string ret;
    ret.reserve(bytes.size() * 2);
    for (const std::byte v : bytes) {
        ret.push_back(hexstr[std::to_integer<int>(v >> 4)]);
        ret.push_back(hexstr[std::to_integer<int>(v & std::byte{0xF})]);
    }
    return ret;
}

Uuid Uuid::generate() noexcept {
    static std::mt19937_64 gen = ProperlySeededRandomEngine<>();
    static std::mutex gen_mtx;
    [[maybe_unused]] std::lock_guard lk{gen_mtx};
    Uuid ret;
    const uint64_t data[2]{gen(), gen()};
    std::memcpy(ret.bytes.data(), data, ret.bytes.size());
    return ret;
}

} // namespace smce


#define PY_ARRAY_UNIQUE_SYMBOL phist_PyArray_API
#define NO_IMPORT_ARRAY

#include <string>
#include <cmath>

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <boost/array.hpp>

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/histogram/histogram.hxx"
#include "seglib/histogram/histogram_python.hxx"

namespace python = boost::python;

namespace histogram {

    vigra::NumpyAnyArray labelHistogram(
        vigra::NumpyArray<3, vigra::Multiband<LabelType>  >         img,
        const LabelType                                             nLabels,
        const size_t                                                r,
        //output
        vigra::NumpyArray<4, float >    res = vigra::NumpyArray<4, float >()
    ){
        const size_t nChannels=img.shape(2);
        // allocate output
        typedef typename vigra::NumpyArray<4, float >::difference_type Shape4;
        Shape4 shape(img.shape(0),img.shape(1),nChannels,nLabels);
        res.reshapeIfEmpty(shape);
        std::fill(res.begin(),res.end(),0.0);

        // coordinate in the res array (pixel wise histogram)
        // (x,y,c,bin)
        Shape4 histCoord;
        const vigra::TinyVector<float, 2>  radius1(r+1,r+1);

        vigra::TinyVector<int,2>  start,end,c;

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            for(int d=0;d<2;++d){
                start[d]   = std::max(int(0),            int(histCoord[d]) - int(r));
                end[d]     = std::min(int(img.shape(d)), int(histCoord[d] + (r+1) ));
            }

            for(c[0]=start[0];c[0]<end[0];++c[0])
            for(c[1]=start[1];c[1]<end[1];++c[1]){

                // iterate over all channels
                for(histCoord[2]=0;histCoord[2]<nChannels;++histCoord[2] ){

                    const LabelType label = img(c[0],c[1],histCoord[2]);

                    histCoord[3] = label;

                    /*
                    std::cout<<"\n\n” channel "<<histCoord[2]<<"\n";
                    std::cout<<"value "<< value<<"\n";
                    std::cout<<"mi " << min(histCoord[2])<<"\n";
                    std::cout<<"ma " << max(histCoord[2])<<"\n";
                    std::cout<<"fa " << fac[histCoord[2]]<<"\n";
                    */

                    PHIST_ASSERT_OP(histCoord[3],<,nLabels);
                    // increment hist
                    res(histCoord[0],histCoord[1],histCoord[2],histCoord[3])+=1.0;
                }
            }
        }

        // normalize

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1])
        for(histCoord[2]=0;histCoord[2]<img.shape(2);++histCoord[2]){

            float sum=0.0;
            for(histCoord[3]=0;histCoord[3]<nLabels;++histCoord[3]){
                sum+=res(histCoord[0],histCoord[1],histCoord[2],histCoord[3]);
            }
            for(histCoord[3]=0;histCoord[3]<nLabels;++histCoord[3]){
                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3])/=sum;
            }
        }
        return res;
    }

    vigra::NumpyAnyArray labelSimHistogram(
        vigra::NumpyArray<3, vigra::Multiband<LabelType>  >         img,
        vigra::NumpyArray<3, float  >                               labelSim,
        const LabelType                                             nLabels,
        const size_t                                                r,
        //output
        vigra::NumpyArray<4, float >    res = vigra::NumpyArray<4, float >()
    ){
        const size_t nChannels=img.shape(2);
        // allocate output
        typedef typename vigra::NumpyArray<4, float >::difference_type Shape4;
        Shape4 shape(img.shape(0),img.shape(1),nChannels,nLabels);
        res.reshapeIfEmpty(shape);
        std::fill(res.begin(),res.end(),0.0);

        PHIST_ASSERT_OP(labelSim.shape(0),==,labelSim.shape(1));
        PHIST_ASSERT_OP(labelSim.shape(0),==,nLabels);
        PHIST_ASSERT_OP(labelSim.shape(2),==,img.shape(2));
        // coordinate in the res array (pixel wise histogram)
        // (x,y,c,bin)
        Shape4 histCoord;
        const vigra::TinyVector<float, 2>  radius1(r+1,r+1);

        vigra::TinyVector<int,2>  start,end,c;

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            for(int d=0;d<2;++d){
                start[d]   = std::max(int(0),            int(histCoord[d]) - int(r));
                end[d]     = std::min(int(img.shape(d)), int(histCoord[d] + (r+1) ));
            }

            for(c[0]=start[0];c[0]<end[0];++c[0])
            for(c[1]=start[1];c[1]<end[1];++c[1]){

                // iterate over all channels
                for(histCoord[2]=0;histCoord[2]<nChannels;++histCoord[2] ){

                    const LabelType label = img(c[0],c[1],histCoord[2]);

                    histCoord[3] = label;

                    /*
                    std::cout<<"\n\n” channel "<<histCoord[2]<<"\n";
                    std::cout<<"value "<< value<<"\n";
                    std::cout<<"mi " << min(histCoord[2])<<"\n";
                    std::cout<<"ma " << max(histCoord[2])<<"\n";
                    std::cout<<"fa " << fac[histCoord[2]]<<"\n";
                    */

                    PHIST_ASSERT_OP(histCoord[3],<,nLabels);
                    // increment hist

                    for(size_t ll = 0 ;ll<nLabels;++ll){
                        const float sim = labelSim(label,ll,histCoord[2]);
                        res(histCoord[0],histCoord[1],histCoord[2],ll)+=sim;
                    }

                }
            }
        }

        // normalize

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1])
        for(histCoord[2]=0;histCoord[2]<img.shape(2);++histCoord[2]){

            float sum=0.0;
            for(histCoord[3]=0;histCoord[3]<nLabels;++histCoord[3]){
                sum+=res(histCoord[0],histCoord[1],histCoord[2],histCoord[3]);
            }
            for(histCoord[3]=0;histCoord[3]<nLabels;++histCoord[3]){
                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3])/=sum;
            }
        }
        return res;
    }

    void moveMe(
        vigra::NumpyArray<2, float  >           globalFeatures,
        vigra::NumpyArray<1, vigra::Int64  >    batchIndex,
        vigra::NumpyArray<1, vigra::Int64  >    minCenterIndex,
        vigra::NumpyArray<1, float  >           centerCount,
        vigra::NumpyArray<2, float  >           centers
    ){

        const size_t batchSize = batchIndex.shape(0);
        const size_t nFeatures = globalFeatures.shape(1);
        for(size_t bi=0;bi<batchSize;++bi){
            const size_t ci = minCenterIndex(bi);
            centerCount(ci)+=1.0;
            const float rate = 1.0/centerCount(ci);
            for(size_t f=0;f<nFeatures;++f){
                centers(ci,f)*=(1.0-rate);
                centers(ci,f)+=rate*globalFeatures(batchIndex(bi),f);
            }
        }
    }

    void histDist(
        vigra::NumpyArray<2, float  > samples,
        vigra::NumpyArray<2, float  > centers,
        vigra::NumpyArray<2, float  > distances,
        const std::string distType
    ){

        // renormalize centers

        const size_t nSamples  = samples.shape(0);
        const size_t nFeatures = samples.shape(1);
        const size_t nCenters  = centers.shape(0);

        if(distType==std::string("bhattacharyya")){

            // bhattacharyya dist may shift centers
            for(size_t c=0;c<nCenters;++c){

                float minV=std::numeric_limits<float>::infinity();
                for(size_t f=0;f<nFeatures;++f){
                    minV=std::min(minV,centers(c,f));
                }
                if (minV<0.000001f){
                    for(size_t f=0;f<nFeatures;++f){
                        centers(c,f)-=minV;
                    }
                }
                float sum=0;
                for(size_t f=0;f<nFeatures;++f){
                    sum+=centers(c,f);
                }
                if(sum>0.999999){
                    for(size_t f=0;f<nFeatures;++f){
                        centers(c,f)/=sum;
                    }
                }

            }

            for(size_t s=0;s<nSamples;++s)
            for(size_t c=0;c<nCenters;++c){
                // compute distace

                float sum=0.0;
                for(size_t f=0;f<nFeatures;++f){

                    const float sv=samples(s,f);
                    const float cv=samples(c,f);
                    PHIST_ASSERT_OP(sv , > , -0.0000001);
                    PHIST_ASSERT_OP(cv , > , -0.0000001);
                    PHIST_ASSERT_OP(sv , < , 1.0000001);
                    PHIST_ASSERT_OP(cv , < , 1.0000001);
                    sum+=std::sqrt(samples(s,f)*centers(c,f));
                }
                PHIST_ASSERT_OP(sum , <= , 1.0001);
                sum=std::min(sum,0.9999999f);
                distances(s,c)=(1.0f - sum);
                //std::cout<<"distances "<<distances(s,c)<<"\n";
            }
        }
        else if(distType==std::string("chi2")){
            for(size_t s=0;s<nSamples;++s)
            for(size_t c=0;c<nCenters;++c){
                // compute distace

                float sum=0.0;
                for(size_t f=0;f<nFeatures;++f){
                    const float sv=samples(s,f);
                    const float cv=samples(c,f);

                    sum+=std::pow(sv-cv,2)/(sv+cv);
                }
                distances(s,c)=0.5*sum;
                //std::cout<<"distances "<<distances(s,c)<<"\n";
            }
        }
        else{
            PHIST_ASSERT_OP(0,==,1);
        }
    }

    void export_label_histogram(){

        python::def("_label_histogram_",vigra::registerConverters(&labelHistogram),
            (
                python::arg("img"),
                python::arg("nLabels"),
                python::arg("r"),
                python::arg("out")=python::object()
            )
        );

        python::def("_label_sim_histogram_",vigra::registerConverters(&labelSimHistogram),
            (
                python::arg("img"),
                python::arg("labelSim"),
                python::arg("nLabels"),
                python::arg("r"),
                python::arg("out")=python::object()
            )
        );

        python::def("moveMe",vigra::registerConverters(&moveMe),
            (
                python::arg("globalFeatures"),
                python::arg("batchIndex"),
                python::arg("minCenterIndex"),
                python::arg("centerCount"),
                python::arg("centers")
            )
        );

        python::def("histDist",vigra::registerConverters(&histDist),
            (
                python::arg("samples"),
                python::arg("centers"),
                python::arg("distances"),
                python::arg("distType")=std::string("bhattacharyya")
            )
        );

    }

}


#include "main.h"
#include "bignum.h"

#include <boost/test/unit_test.hpp>

#include <vector>

namespace {
const CBigNum PROOF_OF_WORK_LIMIT(~uint256(0) >> 20);
}

BOOST_AUTO_TEST_SUITE(consensus_tests)

BOOST_AUTO_TEST_CASE(nextworkrequired_null)
{
    BOOST_CHECK_EQUAL(GetNextWorkRequired(nullptr, nullptr), PROOF_OF_WORK_LIMIT.GetCompact());
}

BOOST_AUTO_TEST_CASE(nextworkrequired_first_block)
{
    CBlockHeader header;
    CBlockIndex index;
    index.nHeight = 0;
    index.nTime = 0;
    index.nBits = 1337;

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&index, &header), 1337);
}

BOOST_AUTO_TEST_CASE(nextworkrequired_retarget_v1)
{
    CBlockHeader header;

    std::vector<CBlockIndex> blockIndexes(1000);
    for (std::size_t i = 0; i < blockIndexes.size(); ++i) {
        blockIndexes[i].nHeight = i;
        if (i > 0) {
            blockIndexes[i-1].pnext = &blockIndexes[i];
            blockIndexes[i].pprev = &blockIndexes[i-1];
        }
    }

    const CBigNum bn(1000);

    for (std::size_t i = 0; i < 240; ++i) {
        blockIndexes[i].nTime = i;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[239], &header), (bn/16).GetCompact());

    for (std::size_t i = 240; i < 480; ++i) {
        blockIndexes[i].nTime = i*10;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[479], &header), (bn*4551/(4*60*60)).GetCompact());

    for (std::size_t i = 480; i < 720; ++i) {
        blockIndexes[i].nTime = i*1000;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[719], &header), (bn*4).GetCompact());
}

BOOST_AUTO_TEST_CASE(nextworkrequired_retarget_v2)
{
    CBlockHeader header;

    std::vector<CBlockIndex> blockIndexes(1000);
    for (std::size_t i = 0; i < blockIndexes.size(); ++i) {
        blockIndexes[i].nHeight = 5039+i;
        if (i > 0) {
            blockIndexes[i-1].pnext = &blockIndexes[i];
            blockIndexes[i].pprev = &blockIndexes[i-1];
        }
    }

    const CBigNum bn(1000);

    for (std::size_t i = 1; i <= 240; ++i) {
        blockIndexes[i].nTime = i;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[240], &header), (bn/8).GetCompact());

    for (std::size_t i = 241; i <= 480; ++i) {
        blockIndexes[i].nTime = i*10;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[480], &header), (bn*4551/(4*60*60)).GetCompact());

    for (std::size_t i = 481; i <= 720; ++i) {
        blockIndexes[i].nTime = i*1000;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[720], &header), (bn*4).GetCompact());
}

BOOST_AUTO_TEST_CASE(nextworkrequired_retarget_v3)
{
    CBlockHeader header;

    std::vector<CBlockIndex> blockIndexes(1000);
    for (std::size_t i = 0; i < blockIndexes.size(); ++i) {
        blockIndexes[i].nHeight = 10079+i;
        if (i > 0) {
            blockIndexes[i-1].pnext = &blockIndexes[i];
            blockIndexes[i].pprev = &blockIndexes[i-1];
        }
    }

    const CBigNum bn(1000);

    for (std::size_t i = 1; i <= 240; ++i) {
        blockIndexes[i].nTime = i;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[240], &header), (bn/4).GetCompact());

    for (std::size_t i = 241; i <= 480; ++i) {
        blockIndexes[i].nTime = i*10;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[480], &header), (bn*4551/(4*60*60)).GetCompact());

    for (std::size_t i = 481; i <= 720; ++i) {
        blockIndexes[i].nTime = i*1000;
        blockIndexes[i].nBits = bn.GetCompact();
    }

    BOOST_CHECK_EQUAL(GetNextWorkRequired(&blockIndexes[720], &header), (bn*4).GetCompact());
}

BOOST_AUTO_TEST_SUITE_END()


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <complex>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <cmath>

#include <boost/timer.hpp>

#include "dune/common/stdstreams.hh"
#include "dune/grid/sgrid.hh"
#include "dune/grid/uggrid.hh"
#include "dune/grid/common/gridinfo.hh"

#include "fem/assemble.hh"
#include "fem/embedded_errorest.hh"
#include "fem/istlinterface.hh"
#include "fem/functional_aux.hh"
#include "fem/hierarchicspace.hh"
#include "fem/lagrangespace.hh"
#include "fem/iterate_grid.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/triplet.hh"
#include "linalg/mumps_solve.hh"

#include "io/vtk.hh"
#include "io/amira.hh"

#include "mg/pcg.hh"
#include "mg/apcg.hh"

#include "utilities/kaskopt.hh"

#include "poisson.hh"
#include "amiramesh.hh"

int problemNo = 1;

bool compareAbs(const double x1, const double  x2)
  {
    return fabs(x1)>fabs(x2);
  }

int main(int argc, char *argv[])
  {
    int verbosity = 1;
    bool dump = false;
    boost::property_tree::ptree *pt = GetKaskadeOptions(argc, argv, verbosity, dump);

    std::cout << "Start cascadic multigrid test program" << std::endl;

    int  direct, refs, order, onlyLowerTriangle = false, maxAdaptSteps;
    SolverType directType;
    IterateType iterateType = IterateType::PCG;
    MatrixProperties property;
    std::string empty, problem, functional, geoFile;

	problem = GetParameter(pt, "problem", empty);
	refs = GetParameter(pt, problem+".refs", 0),
    order =  GetParameter(pt, problem+".order", 1),
    maxAdaptSteps = GetParameter(pt, problem+".maxAdaptSteps", 10);
    geoFile = GetParameter(pt, problem+".geoFile", empty);
    functional = GetParameter(pt, problem+".functional", empty);
    problemNo = GetParameter(pt, "names.functional."+functional, 1);
    std::cout << "selected problem " << problem << ", functional=" <<
                 functional << "(" << problemNo <<  "), geoFile=" <<
                 geoFile << std::endl;

    std::string s("names.type.");
    s += GetParameter(pt, "solver.type", empty);
    direct = GetParameter(pt, s, 0);

    s = "names.direct." + GetParameter(pt, "solver.direct", empty);
    directType = static_cast<SolverType>(GetParameter(pt, s, 2));

	s = "names.iterate." + GetParameter(pt, "solver.iterate", empty);
	iterateType = static_cast<IterateType>(GetParameter(pt, s, 0));

    property = MatrixProperties::POSITIVEDEFINITE;

    if (((property == MatrixProperties::SYMMETRIC)||(property == MatrixProperties::POSITIVEDEFINITE))&&
        ((directType == DirectType::MUMPS)||(directType == DirectType::PARDISO)))
      {
        onlyLowerTriangle = true;
      }

    int const dim = 3;
    int k;
    typedef Dune::UGGrid<dim> Grid;
    Grid *grid = new Grid(20000);
    Dune::GridFactory<Grid> factory(grid);

    DuneAmiraMesh<dim,dim,Grid> mesh(geoFile.c_str());
    mesh.InsertUGGrid(factory);

//    std::auto_ptr<Grid> grid( factory.createGrid() );
    grid = factory.createGrid();
    // the coarse grid will be refined refs times
    for (k=0; k<refs; k++)
      {
        grid->globalRefine(1);
	  }
    // some information on the refined mesh
    std::cout << "Grid: " << grid->size(0) << " tetrahedra, " << std::endl;
    std::cout << "      " << grid->size(1) << " triangles, " << std::endl;
    std::cout << "      " << grid->size(1) << " edges, " << std::endl;
    std::cout << "      " << grid->size(dim) << " points" << std::endl;
    // a gridmanager is constructed
    // as connector between geometric and algebraic information
    GridManager<Grid> gridManager(grid);

    typedef Grid::LeafGridView LeafView;
    // construction of finite element space for the scalar solution T
    typedef FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> > H1Space;

    H1Space temperatureSpace(gridManager,gridManager.grid().leafView(),
                             order);
    typedef boost::fusion::vector<H1Space const*> Spaces;
    Spaces spaces(&temperatureSpace);
    // VariableDescription<int spaceId, int components, int Id>
    // spaceId: number of associated FEFunctionSpace
    // components: number of components in this variable
    // Id: number of this variable
    typedef boost::fusion::vector<VariableDescription<0,1,0> >
      VariableDescriptions;
    std::string varNames[1] = { "T" };
    typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
    VariableSet variableSet(spaces,varNames);

    typedef PoissonFunctional<double,VariableSet> Functional;
    Functional F;
    typedef VariationalFunctionalAssembler<LinearizationAt<Functional> > GOP;
    typedef GOP::TestVariableRepresentation<>::type Rhs;
    GOP gop(gridManager.signals,spaces);

    typedef FEFunctionSpace<ContinuousHierarchicExtensionMapper<double,LeafView> > H1ExSpace;
    H1ExSpace spaceEx(gridManager,gridManager.grid().leafView(), order+1);

    typedef boost::fusion::vector<H1Space const*,H1ExSpace const*> H1ExSpaces;
    H1ExSpaces exSpaces(&temperatureSpace,&spaceEx);

    typedef boost::fusion::vector<VariableDescription<1,1,0> > ExVariableDescriptions;
    typedef VariableSetDescription<H1ExSpaces,ExVariableDescriptions> ExVariableSet;
    std::string exVarNames[2] = { "l", "e"};
    ExVariableSet exVariableSet(exSpaces, exVarNames);

    typedef HierarchicErrorEstimator<LinearizationAt<Functional>,ExVariableSet> ErrorEstimator;
    typedef VariationalFunctionalAssembler<ErrorEstimator> EstGOP;

    EstGOP estGop(gridManager.signals,exSpaces);
	typedef EstGOP::AnsatzVariableRepresentation<> Ansatz;
	typedef EstGOP::TestVariableRepresentation<> Test;

	typedef VariableSet::Grid::Traits::LeafIndexSet IS ;
	IS const& is = gridManager.grid().leafIndexSet();

    VariableSet::VariableSet x(variableSet), dx(variableSet), tmp(variableSet);

    double rTolX = GetParameter(pt, problem+".rTolX", 1.0e-4),
           aTolX = GetParameter(pt, problem+".aTolX", 1.0-2),
           minRefine = GetParameter(pt, problem+".minRefine", 0.0);
    std::vector<std::pair<double,double> > tolX(variableSet.noOfVariables);
    std::vector<std::pair<double,double> > tolXC(variableSet.noOfVariables);
    for (int i=0; i<tolX.size(); ++i)
      {
        tolX[i] = std::make_pair(aTolX,rTolX);
        tolXC[i] = std::make_pair(aTolX/100,rTolX/100);
     }
	std::cout << "rTolX = " << rTolX << ", aTolX = " << aTolX <<
	             ", minRefine = " << minRefine << std::endl ;

	IoOptions options;
	options.outputType = IoOptions::ascii;

    int refSteps = 0;
    bool accurate = false;
	int iteSteps = GetParameter(pt, "solver.iteMax", 1000);
	int verbose = GetParameter(pt, "solver.verbose", 1);
	double iteEps = GetParameter(pt, "solver.iteEps", 1.0e-6);
	typedef GOP::TestVariableRepresentation<>::type LinearSpace;
	Dune::InverseOperatorResult res;
	double errNorm;

	size_t size = variableSet.degreesOfFreedom(0,1);
	double gamma = 1.0, d = dim;
	double beta = 1.0/sqrt(d*gamma);
	double alpha = (d*gamma-1.0)/(d*(1.0+gamma));
	double qk = 1.0, dNk = size, zk = 0.0;
	double requested = sqrt(1-beta*beta)*tolX[0].first;
	double safety = 1.0;
	double yk = pow(dNk,alpha);
printf("gamma=%e, d=%e, beta=%e, alpha=%e, requested=%e, safety=%e\n",
       gamma,d,beta,alpha,requested,safety);
//printf("    %10ld %e   %e   %e   %e\n", size, qk, dNk, yk, zk);
    do
      {
		typedef GOP::AnsatzVariableRepresentation<>::type Sol;
		Sol solution(GOP::AnsatzVariableRepresentation<>::init(gop));
		Sol hilfe(GOP::AnsatzVariableRepresentation<>::init(gop));
		solution = 0;
		hilfe = 0;
		gop.assemble(linearization(F,x));
		Rhs rhs(GOP::TestVariableRepresentation<>::rhs(gop));
		AssembledGalerkinOperator<GOP,0,1,0,1> A(gop, onlyLowerTriangle);
		AssembledGalerkinOperator<GOP,0,1,0,1>::matrix_type tri(A.getmat());

        if (direct)
          {
            directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
		  }
		else
		  {
			switch (iterateType)
			  {
			   case IterateType::CG:
			     {
				   JacobiPreconditioner<GOP,0,1,0> jacobi(gop,1.0);
				   Dune::CGSolver<LinearSpace> cg(A,jacobi,iteEps,iteSteps,verbose);
				   cg.apply(hilfe,rhs,res);
			     }
			     break;
			   case IterateType::PCG:
			     {
			       JacobiPreconditioner<GOP,0,1,0> jacobi(gop,1.0);
			       Kaskade7::NMIIIPCGSolver<LinearSpace> pcg(A,jacobi,iteEps,iteSteps,verbose);
			       pcg.apply(hilfe,rhs,res);
			     }
			     break;
			   case IterateType::APCG:
			     {
    			   int addedIterations = GetParameter(pt, "solver.APCG.addedIterations", 10);
			       JacobiPreconditioner<GOP,0,1,0> jacobiPCG(gop,1.0);
			       Kaskade7::NMIIIAPCGSolver<LinearSpace> apcg(A,jacobiPCG,iteEps,iteSteps,verbose,addedIterations);
			       apcg.apply(hilfe,rhs,res);
			     }
			     break;
			  default:
			    std::cout << "Solver not available" << std::endl;
			    throw -111;
			  }
			solution.axpy(-1,hilfe);
		  }
        dx.data = solution.data;
printf("Solved!\n");

		std::ostringstream fn;
		fn << "graph3d/cmg-grid";
		fn.width(3);
		fn.fill('0');
		fn.setf(std::ios_base::right,std::ios_base::adjustfield);
		fn << refSteps;
		fn.flush();
	    LeafView leafView = gridManager.grid().leafView();
	    writeVTKFile(leafView,variableSet,x,fn.str(),options,order);

        // Do hierarchical error estimation. Remember to provide the very same underlying problem to the
        // error estimator functional as has been used to compute dx (do not modify x!).
        if (!tolX.empty())
          {
	        tmp *= 0 ;
	        estGop.assemble(ErrorEstimator(LinearizationAt<Functional>(F,x),dx));
		    int const estNvars = ErrorEstimator::AnsatzVars::noOfVariables;
		    int const estNeq = ErrorEstimator::TestVars::noOfVariables;
		    size_t  estNnz = estGop.nnz(0,estNeq,0,estNvars,false);
		    size_t  estSize = exVariableSet.degreesOfFreedom(0,estNvars);

		    std::vector<int> estRidx(estNnz), estCidx(estNnz);
		    std::vector<double> estData(estNnz), estRhs(estSize), estSolVec(estSize);
		    estGop.toSequence(0,estNeq,estRhs.begin());

		  // iterative solution of error estimator

		    AssembledGalerkinOperator<EstGOP> E(estGop);
		    Dune::InverseOperatorResult estRes;
		    Test::type estRhside(Test::rhs(estGop) ) ;
		    Ansatz::type estSol(Ansatz::init(estGop) ) ;
		    estSol = 1.0 ;
		    JacobiPreconditioner<EstGOP> jprec(estGop, 1.0);
		    jprec.apply(estSol,estRhside); //single Jacobi iteration
		    estSol.write(estSolVec.begin());

	  // Transfer error indicators to cells.
		    std::vector<double> errorDistribution(is.size(0),0.0);
		    typedef VariableSet::GridView::Codim<0>::Iterator CellIterator ;
		    double maxErr = 0.0;
		    for (CellIterator ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
		      {
				typedef H1ExSpace::Mapper::GlobalIndexRange GIR;
				double err = 0;
				GIR gix = spaceEx.mapper().globalIndices(*ci);
				for (GIR::iterator j=gix.begin(); j!=gix.end(); ++j)
				  err += fabs(boost::fusion::at_c<0>(estSol.data)[*j]);
				errorDistribution[is.index(*ci)] = err;
				if (fabs(err)>maxErr) maxErr = fabs(err);
		      }

		    double errLevel = 0.5*maxErr;

	  	    errLevel = 0.5*maxErr;
		    if (minRefine>0.0)
		      {
		    	std::vector<double> eSort(errorDistribution);
	  	    	std::sort(eSort.begin(),eSort.end(),compareAbs);
	  	    	int minRefineIndex = minRefine*(eSort.size()-1);
	  	    	double minErrLevel = fabs(eSort[minRefineIndex])+1.0e-15;
	  	    	if (minErrLevel<errLevel)
	  	    	  errLevel = minErrLevel;
	  	      }

		    errNorm = 0 ;
		    for (k=0; k < estRhs.size() ; k++ )
		      errNorm +=  fabs(estRhs[k]*estSolVec[k]) ;

	        if (errNorm<requested)
	          accurate = true ;
	        else
	          {

	  // Refine mesh.

				int noToRefine = 0;
				double alphaSave = 1.0 ;
				std::vector<bool> toRefine( is.size(0), false ) ; //for adaptivity in compression
				std::vector< std::vector<bool> > refinements;
				for (CellIterator ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
				  if (fabs(errorDistribution[is.index(*ci)]) >= alphaSave*errLevel)
					{
					  noToRefine++;
					  toRefine[is.index(*ci)] = true ;
					  gridManager.mark(1,*ci);
					}

				refinements.push_back(toRefine);
				accurate = !gridManager.adaptAtOnce();
	          }
        }
        // apply the Newton correction here
       x += dx;

//           { // Compute spatial error estimate
//     		   std::vector<double> norm2, error2;
//     		   norm2.resize(1);
//     		   error2.resize(1);
//             tmp = x;
//             projectHierarchically(variableSet,tmp);
//             tmp -= x;
//
//           // perform mesh adaptation
//             accurate = embeddedErrorEstimator(variableSet, tmp, x, scal, tolX, gridManager, norm2, error2);
//             printf(" %e %e\n", norm2[0], error2[0]);
//             if (!accurate)
//               {
//                 nnz = gop.nnz(0,1,0,1,onlyLowerTriangle);
//                 size = variableSet.degreesOfFreedom(0,1);
// 				ridx.resize(nnz);
// 				cidx.resize(nnz);
// 				data.resize(nnz);
// 				rhs.resize(size);
// 				sol.resize(size);
//               }
//           }
        refSteps++;

        printf("%3d %10ld %5d   %e   %e\n", refSteps, size, res.iterations, errNorm,
               iteEps);

        size = variableSet.degreesOfFreedom(0,1);
	    qk = size/dNk;
		dNk = size;
	    yk += pow(dNk,alpha);
	    zk = pow(dNk,alpha)*(pow(errNorm/requested,d*alpha)-pow(qk,alpha))/(pow(qk,alpha)-1.0);
        iteEps = safety*beta*errNorm*yk/(yk+zk);

//        printf("  %10ld %e   %e   %e\n", size, yk, zk, yk+zk);

        if (refSteps>maxAdaptSteps) break;
//
// 		std::ostringstream fn;
// 		fn << "graph3d/cmg-grid";
// 		fn.width(3);
// 		fn.fill('0');
// 		fn.setf(std::ios_base::right,std::ios_base::adjustfield);
// 		fn << refSteps;
// 		fn.flush();
// 	    LeafView leafView = gridManager.grid().leafView();
// 	    writeVTKFile(leafView,variableSet,x,fn.str(),options,order);

      } while (!accurate);

    std::cout << "End cmgtest" << std::endl;
  }


/*
 * This file is a part of
 *
 * ============================================
 * ###   Pteros molecular modeling library  ###
 * ============================================
 *
 * (C) 2009-2018, Semen Yesylevskyy
 *
 * All works, which use Pteros, should cite the following papers:
 *
 *  1.  Semen O. Yesylevskyy, "Pteros 2.0: Evolution of the fast parallel
 *      molecular analysis library for C++ and python",
 *      Journal of Computational Chemistry, 2015, 36(19), 1480–1488.
 *      doi: 10.1002/jcc.23943.
 *
 *  2.  Semen O. Yesylevskyy, "Pteros: Fast and easy to use open-source C++
 *      library for molecular analysis",
 *      Journal of Computational Chemistry, 2012, 33(19), 1632–1636.
 *      doi: 10.1002/jcc.22989.
 *
 * This is free software distributed under Artistic License:
 * http://www.opensource.org/licenses/artistic-license-2.0.php
 *
*/

#include "pteros/core/system.h"
#include "pteros/core/pteros_error.h"
#include "pteros/core/distance_search.h"
#include "pteros/core/force_field.h"
#include <cmath>
#include <functional>
#include "pteros/core/logging.h"
#include <boost/algorithm/string.hpp>

using namespace std;
using namespace pteros;
using namespace Eigen;

Vector3f get_shift_coefs(int alpha, float r1, float rc){
    Vector3f res;
    res(0) = -(( (alpha+4)*rc - (alpha+1)*r1 )/( pow(rc,alpha+2)*pow(rc-r1,2) ));
    res(1) = ( (alpha+3)*rc - (alpha+1)*r1 )/( pow(rc,alpha+2)*pow(rc-r1,3) );
    res(2) = 1.0/pow(rc,alpha) - (res(0)/3.0)*pow(rc-r1,3) - (res(1)/4.0)*pow(rc-r1,4);
    return res;
}

// Plain LJ kernel
float LJ_en_kernel(float C6, float C12, float r, const Force_field& ff){
    float r_inv = 1.0/r;
    float tmp = r_inv*r_inv; // (1/r)^2
    tmp = tmp*tmp*tmp; // (1/r)^6
    return C12*tmp*tmp-C6*tmp;
}

// Cutoff LJ kernel
float LJ_en_kernel_cutoff(float C6, float C12, float r, const Force_field& ff){
    if(r>ff.rvdw) return 0.0;
    float r_inv = 1.0/r;
    float tmp = r_inv*r_inv; // (1/r)^2
    tmp = tmp*tmp*tmp; // (1/r)^6
    return C12*tmp*tmp-C6*tmp;
}

// Shifted LJ kernel
float LJ_en_kernel_shifted(float C6, float C12, float r, const Force_field& ff){
    if(r>ff.rvdw) return 0.0;

    float val12 = pow(r,-12)
            -(ff.shift_12(0)/3.0)*pow(r-ff.rvdw_switch,3)
            -(ff.shift_12(1)/4.0)*pow(r-ff.rvdw_switch,4)
            -ff.shift_12(2);
    float val6 = pow(r,-6)
            -(ff.shift_6(0)/3.0)*pow(r-ff.rvdw_switch,3)
            -(ff.shift_6(1)/4.0)*pow(r-ff.rvdw_switch,4)
            -ff.shift_6(2);

    return C12*val12 - C6*val6;
}

#define ONE_4PI_EPS0      138.935456

// Plane Coulomb kernel
float Coulomb_en_kernel(float q1, float q2, float r, const Force_field& ff){
    return ff.coulomb_prefactor*q1*q2/r;
}

// Cutoff Coulomb kernel
float Coulomb_en_kernel_cutoff(float q1, float q2, float r, const Force_field& ff){
    if(r>ff.rcoulomb) return 0.0;
    return ff.coulomb_prefactor*q1*q2/r;
}

// Reaction field Coulomb kernel
float Coulomb_en_kernel_rf(float q1, float q2, float r, const Force_field& ff){
    return ff.coulomb_prefactor*q1*q2*(1.0/r + ff.k_rf*r*r - ff.c_rf);
}

// Shifted Coulomb kernel
float Coulomb_en_kernel_shifted(float q1, float q2, float r, const Force_field& ff){
    return ff.coulomb_prefactor*q1*q2*( 1.0/r
                                     -(ff.shift_1(0)/3.0)*pow(r-ff.rcoulomb_switch,3)
                                     -(ff.shift_1(1)/4.0)*pow(r-ff.rcoulomb_switch,4)
                                     -ff.shift_1(2)
                                     );
}

#define LOWER(s) boost::algorithm::to_lower_copy(string(s))

void Force_field::setup_kernels(){
    using namespace placeholders;

    LOG()->debug("Coulomb type: {}",coulomb_type);
    LOG()->debug("Coulomb modifier: {}",coulomb_modifier);
    LOG()->debug("VdW type: {}",vdw_type);
    LOG()->debug("VdW modifier: {}",vdw_modifier);

    // Set Coulomb prefactor
    coulomb_prefactor = ONE_4PI_EPS0 / epsilon_r;

    // Set Coulomb kernel
    if(LOWER(coulomb_type)=="reaction-field"){
        // In case of reaction field precompute constanst
        if(epsilon_rf){
            k_rf = (1.0/(rcoulomb*rcoulomb*rcoulomb))
                    * (epsilon_rf-epsilon_r) / (2.0*epsilon_rf+epsilon_r);
        } else {
            // for epsilon_rf = 0 (which means inf)
            k_rf = 0.5/(rcoulomb*rcoulomb*rcoulomb);
        }
        c_rf = (1.0/rcoulomb) + k_rf*rcoulomb*rcoulomb;

        // Set coulomb kernel pointer
        coulomb_kernel_ptr = &Coulomb_en_kernel_rf;
        LOG()->debug("\tCoulomb kernel: reaction_field");

    } else if( ( LOWER(coulomb_type)=="cut-off"
                 && LOWER(coulomb_modifier)== "potential-shift"
               )
              || LOWER(coulomb_type)=="shift"
              || LOWER(coulomb_type)=="pme") {
        // Compute shift constants for power 1
        shift_1 = get_shift_coefs(1,rcoulomb_switch,rcoulomb);

        coulomb_kernel_ptr = &Coulomb_en_kernel_shifted;
        LOG()->debug("\tCoulomb kernel: shifted");

    } else if(LOWER(coulomb_type)==LOWER("cut-off")) {
        // In other cases set plain Coulomb interaction
        coulomb_kernel_ptr = &Coulomb_en_kernel_cutoff;
        LOG()->debug("\tCoulomb kernel: cutoff");
    } else {
        coulomb_kernel_ptr = &Coulomb_en_kernel;
        LOG()->debug("\tCoulomb kernel: plain");
    }

    // Set LJ kernel
    if(LOWER(vdw_type)== "shift"){
        // Compute shift constants for powers 6 and 12
        shift_6 = get_shift_coefs(6,rvdw_switch,rvdw);
        shift_12 = get_shift_coefs(12,rvdw_switch,rvdw);

        LJ_kernel_ptr = &LJ_en_kernel_shifted;
        LOG()->debug("\tLJ kernel: shifted");

    } else if(LOWER(vdw_type)== "cut-off") {
        LJ_kernel_ptr = &LJ_en_kernel_cutoff;
        LOG()->debug("\tLJ kernel: cutoff");

    } else {
        LJ_kernel_ptr = &LJ_en_kernel;
        LOG()->debug("\tLJ kernel: plain");
    }
}

Vector2f Force_field::pair_energy(int at1, int at2, float r, float q1, float q2, int type1, int type2)
{
    float c6,c12;
    // indexes have to be in increasing order
    if(at1>at2){
        std::swap(at1,at2);
        std::swap(q1,q2);
        std::swap(type1,type2);
    }
    // Check is the pair is excluded
    if(exclusions[at1].count(at2)) return {0,0};
    // Check if this pair is 1-4 pair
    auto it = LJ14_pairs.find(at1*natoms+at2);
    if(it==std::end(LJ14_pairs)){
        // normal pair
        c6 = LJ_C6(type1,type2);
        c12 = LJ_C12(type1,type2);
        return {coulomb_kernel_ptr(q1,q2,r,*this), LJ_kernel_ptr(c6,c12,r,*this)};
    } else {
        // 1-4 pair
        c6 = LJ14_interactions[it->second](0);
        c12 = LJ14_interactions[it->second](1);
        return {coulomb_kernel_ptr(q1,q2,r,*this)*fudgeQQ, LJ_kernel_ptr(c6,c12,r,*this)};
    }
}

float Force_field::get_cutoff(){
    return std::min(rcoulomb,rvdw);
}

Force_field::Force_field():  ready(false) {}

Force_field::Force_field(const Force_field &other){
    exclusions = other.exclusions;
    LJ_C6 = other.LJ_C6;
    LJ_C12 = other.LJ_C12;
    LJ14_interactions = other.LJ14_interactions;
    LJ14_pairs = other.LJ14_pairs;
    fudgeQQ = other.fudgeQQ;
    rcoulomb = other.rcoulomb;
    epsilon_r = other.epsilon_r;
    epsilon_rf = other.epsilon_rf;
    rcoulomb_switch = other.rcoulomb_switch;
    rvdw_switch = other.rvdw_switch;
    rvdw = other.rvdw;
    coulomb_type = other.coulomb_type;
    coulomb_modifier = other.coulomb_modifier;
    vdw_type = other.vdw_type;
    vdw_modifier = other.vdw_modifier;

    ready = other.ready;

    if(ready) setup_kernels();
}

Force_field &Force_field::operator=(Force_field other){
    exclusions = other.exclusions;
    LJ_C6 = other.LJ_C6;
    LJ_C12 = other.LJ_C12;
    LJ14_interactions = other.LJ14_interactions;
    LJ14_pairs = other.LJ14_pairs;
    fudgeQQ = other.fudgeQQ;
    rcoulomb = other.rcoulomb;
    epsilon_r = other.epsilon_r;
    epsilon_rf = other.epsilon_rf;
    rcoulomb_switch = other.rcoulomb_switch;
    rvdw_switch = other.rvdw_switch;
    rvdw = other.rvdw;
    coulomb_type = other.coulomb_type;
    coulomb_modifier = other.coulomb_modifier;
    vdw_type = other.vdw_type;
    vdw_modifier = other.vdw_modifier;

    ready = other.ready;

    if(ready) setup_kernels();

    return *this;
}

void Force_field::clear(){
    exclusions.clear();
    LJ_C6.fill(0.0);
    LJ_C12.fill(0.0);
    LJ14_interactions.clear();
    LJ14_pairs.clear();
    fudgeQQ = 0.0;

    ready = false;
}


/*! @file pitts_tensortrain_from_dense.hpp
* @brief conversion of a dense tensor to the tensor-train format (based on a hopefully faster TSQR algorithm)
* @author Melven Roehrig-Zoellner <Melven.Roehrig-Zoellner@DLR.de>
* @date 2020-06-19
* @copyright Deutsches Zentrum fuer Luft- und Raumfahrt e. V. (DLR), German Aerospace Center
*
**/

// include guard
#ifndef PITTS_TENSORTRAIN_FROM_DENSE_HPP
#define PITTS_TENSORTRAIN_FROM_DENSE_HPP

// includes
#include "pitts_parallel.hpp"
#include "pitts_tensortrain.hpp"
#include "pitts_multivector.hpp"
#include "pitts_multivector_tsqr.hpp"
#include "pitts_multivector_transform.hpp"
#include "pitts_tensor2.hpp"
#include "pitts_tensor2_eigen_adaptor.hpp"
#include "pitts_timer.hpp"
#include <limits>
#include <numeric>
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options

//! namespace for the library PITTS (parallel iterative tensor train solvers)
namespace PITTS
{
  //! calculate tensor-train decomposition of a tensor stored in fully dense format
  //!
  //! Passing a large enough buffer in work helps to avoid costly reallocations + later page-faults for large data.
  //!
  //! @warning To reduce memory overhead, this function will overwrite the input arguments with temporary data.
  //!          Please pass a copy of the data if you still need it!
  //!
  //! @tparam T         underlying data type (double, complex, ...)
  //!
  //! @param X              input tensor, overwritten and modified output, dimension must be (size/lastDim, lastDim) where lastDim = dimensions.back()
  //! @param dimensions     tensor dimensions, input is interpreted in Fortran storage order (first index changes the fastest)
  //! @param work           buffer for temporary data, will be resized and modified
  //! @param rankTolerance  approximation accuracy, used to reduce the TTranks of the resulting tensor train
  //! @param maxRank        maximal TTrank (bond dimension), unbounded by default
  //! @param mpiGlobal      (experimental) perform a MPI parallel decomposition, this assumes that the data is distributed on the MPI processes and dimensions specify the local dimensions
  //! @return               resulting tensor train
  //!
  template<typename T>
  TensorTrain<T> fromDense(MultiVector<T>& X, MultiVector<T>& work, const std::vector<int>& dimensions, T rankTolerance = std::sqrt(std::numeric_limits<T>::epsilon()), int maxRank = -1, bool mpiGlobal = false)
  {
    // timer
    const auto timer = PITTS::timing::createScopedTimer<TensorTrain<T>>();

    // abort early for zero dimensions
    if( dimensions.size() == 0 )
    {
      if( X.rows()*X.cols() !=  0 )
        throw std::out_of_range("Mismatching dimensions in TensorTrain<T>::fromDense");
      return TensorTrain<T>{dimensions};
    }

    const auto totalSize = std::accumulate(begin(dimensions), end(dimensions), (std::ptrdiff_t)1, std::multiplies<std::ptrdiff_t>());
    const auto nDims = dimensions.size();
    if( X.rows() != totalSize/dimensions[nDims-1] || X.cols() != dimensions[nDims-1] )
      throw std::out_of_range("Mismatching dimensions in TensorTrain<T>::fromDense");

    bool root = true;
    if( mpiGlobal )
    {
      const auto& [iProc,nProcs] = internal::parallel::mpiProcInfo();
      root = iProc == 0;
    }

    TensorTrain<T> result(dimensions);

    // actually convert to tensor train format
    Tensor2<T> tmpR;
    using EigenMatrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
    Eigen::BDCSVD<EigenMatrix> svd;
    for(int iDim = nDims-1; iDim > 0; iDim--)
    {
      if( root )
        std::cout << "iDim: " << iDim << ", matrix dimensions: " << X.rows() << " x " << X.cols() << "\n";
      // calculate QR decomposition
      block_TSQR(X, tmpR, 0, mpiGlobal);
//std::cout << "tmpR:\n" << ConstEigenMap(tmpR) << "\n";

      // calculate SVD of R
      svd.compute(ConstEigenMap(tmpR), Eigen::ComputeThinU | Eigen::ComputeThinV);
      //Eigen::JacobiSVD<EigenMatrix> svd(ConstEigenMap(tmpR), Eigen::ComputeThinU | Eigen::ComputeThinV);
      svd.setThreshold(rankTolerance);
      if( root )
        std::cout << "singular values: " << svd.singularValues().transpose() << "\n";

      // copy V to the TT sub-tensor
      svd.setThreshold(rankTolerance);
      int rank = svd.rank();
      if( maxRank > 0 )
        rank = std::min(maxRank, rank);
      auto& subT = result.editableSubTensors()[iDim];
      subT.resize(rank, dimensions[iDim], X.cols()/dimensions[iDim]);
      for(int i = 0; i < subT.r1(); i++)
        for(int j = 0; j < subT.n(); j++)
          for(int k = 0; k < subT.r2(); k++)
            subT(i,j,k) = svd.matrixV()(j+subT.n()*k, i);

      tmpR.resize(X.cols(), rank);
      EigenMap(tmpR) = svd.matrixV().leftCols(rank);

      const auto nextDim = dimensions[iDim-1];
      transform(X, tmpR, work, {X.rows()/nextDim, rank*nextDim});
      std::swap(X, work);
    }
    // last sub-tensor is now in X
    auto& lastSubT = result.editableSubTensors()[0];
    lastSubT.resize(1, dimensions[0], X.cols()/dimensions[0]);
    for(int i = 0; i < lastSubT.n(); i++)
      for(int j = 0; j < lastSubT.r2(); j++)
        lastSubT(0, i, j) = X(0, i+j*dimensions[0]);

    // make sure we swap X and work back: prevents problems where the reserved space in X is used again later AND the data does only fit into memory once ;)
    if( nDims % 2 == 0 )
      std::swap(X, work);

    return result;
  }

}

#endif // PITTS_TENSORTRAIN_FROM_DENSE_HPP


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE PipelineTrainingTest0

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>

#include <algorithm>
#include <map>
#include <random>
#include <tuple>
#include <vector>

// We set weights as anchors, and verify that the returned values are sensible,
// agree with calls to readWeights()
//
BOOST_AUTO_TEST_CASE(WeightAnchorTest0) {

  using namespace popart;

  auto builder     = Builder::create();
  auto aiOnnx      = builder->aiOnnxOpset9();
  auto aiGraphcore = builder->aiGraphcoreOpset1();

  // accumulationFactor = replicationFactor = 1 in this example, so
  // batchSize = microBatchSize

  // samples in batch  (samples per weight update)
  int64_t batchSize = 4;

  // batches in a step (processed with each call to run(...))
  int64_t batchesPerStep = 100;

  // the number of weight update steps is batchesPerStep = 100
  // the number of samples in a step is batchesPerStep * batchSize = 400

  std::vector<int64_t> sampleShape{1, 1};

  // in this test, weights are of the same shape as data samples
  std::vector<int64_t> weightShape = sampleShape;

  // shape of a batch
  std::vector<int64_t> batchShape{batchSize, 1, 1};
  std::vector<int64_t> stepDataShape{batchesPerStep, batchSize, 1, 1};

  // When  weights are anchors (of ReturnType ALL), the the number of weights
  // that are returned will be batchesPerStep * accumulationFactor (probably,
  // see T10052)
  std::vector<int64_t> stepWeightShape{batchesPerStep, 1, 1};

  TensorInfo sampleInfo{"FLOAT", sampleShape};
  TensorInfo weightInfo = sampleInfo;
  TensorInfo batchInfo{"FLOAT", batchShape};
  TensorInfo stepDataInfo{"FLOAT", stepDataShape};
  TensorInfo stepWeightInfo{"FLOAT", stepWeightShape};

  int64_t sampleElms{1};
  int64_t batchElms      = sampleElms * batchSize;
  int64_t stepWeightElms = sampleElms * batchesPerStep;
  int64_t stepDataElms   = batchElms * batchesPerStep;

  auto input1 = builder->addInputTensor(batchInfo, "tupni");

  //
  // The model : A series of additions of a Weight, and scaling by 0.5
  //
  std::vector<float> w0Vals(sampleElms, 1.0f);
  ConstVoidData w0Data = {w0Vals.data(), sampleInfo};
  auto w0              = builder->addInitializedInputTensor(w0Data);
  auto act0            = aiOnnx.add({w0, input1}, "act0");
  act0                 = aiGraphcore.scale({act0}, 0.5);

  std::vector<float> w1Vals(sampleElms, 1.0f);
  ConstVoidData w1Data = {w1Vals.data(), sampleInfo};
  auto w1              = builder->addInitializedInputTensor(w1Data);
  auto act1            = aiOnnx.add({w1, act0}, "act1");
  act1                 = aiGraphcore.scale({act1}, 0.5);

  std::vector<float> w2Vals(sampleElms, 1.0f);
  ConstVoidData w2Data = {w2Vals.data(), sampleInfo};
  auto w2              = builder->addInitializedInputTensor(w2Data);
  auto act2            = aiOnnx.add({w2, act1}, "act2");
  act2                 = aiGraphcore.scale({act2}, 0.5);

  std::vector<float> w3Vals(sampleElms, 1.0f);
  ConstVoidData w3Data = {w3Vals.data(), sampleInfo};
  auto w3              = builder->addInitializedInputTensor(w3Data);
  auto act3            = aiOnnx.add({w3, act2}, "act3");
  act3                 = aiGraphcore.scale({act3}, 0.5);

  std::vector<float> w4Vals(sampleElms, 1.0f);
  ConstVoidData w4Data = {w4Vals.data(), sampleInfo};
  auto w4              = builder->addInitializedInputTensor(w4Data);
  auto act4            = aiOnnx.add({w4, act3}, "act4");
  act4                 = aiGraphcore.scale({act4}, 0.5);

  std::vector<float> w5Vals(sampleElms, 1.0f);
  ConstVoidData w5Data = {w5Vals.data(), sampleInfo};
  auto w5              = builder->addInitializedInputTensor(w5Data);
  auto act5            = aiOnnx.add({w5, act4}, "act5");
  act5                 = aiGraphcore.scale({act5}, 0.5);

  auto l1 = builder->aiGraphcoreOpset1().l1loss({act5}, 0.1);

  auto proto = builder->getModelProto();

  // Setting anchors as act5 and a bunch of weights
  //
  auto dataFlow = DataFlow(batchesPerStep,
                           {{act5, AnchorReturnType("All")},
                            {w1, AnchorReturnType("All")},
                            {w2, AnchorReturnType("All")},
                            {w3, AnchorReturnType("All")},
                            {w4, AnchorReturnType("All")},
                            {w5, AnchorReturnType("All")}});

  // shard over 3 IPUs
  //
  SessionOptions userOptions;
  userOptions.virtualGraphMode = VirtualGraphMode::Auto;

  auto optimizer = ConstSGD(0.01);

  auto device = createTestDevice(TEST_TARGET, 3);

  auto session = popart::TrainingSession::createFromOnnxModel(
      proto,
      dataFlow,
      l1,
      optimizer,
      device,
      InputShapeInfo(),
      userOptions,
      popart::Patterns(PatternsLevel::Default));

  session->prepareDevice();

  // All 400 samples are [[1.0f]]
  std::vector<float> v_input_0(stepDataElms, 1.0f);
  popart::NDArrayWrapper<float> input1_wrapper(v_input_0.data(), stepDataInfo);
  std::map<popart::TensorId, popart::IArray &> inputs = {
      {input1, input1_wrapper}};

  // Preparing the anchors.
  std::vector<float> v_act5_out(stepDataElms);
  popart::NDArrayWrapper<float> act5_wrapper(v_act5_out.data(), stepDataShape);

  // Note the size of the buffer we'll write to for weight anchors is
  // independent of batch-size. It is number of micro-batches (= number of
  // batches) * weight size (see T10052)
  std::vector<float> v_w1_out(stepWeightElms, -77.0f);
  popart::NDArrayWrapper<float> w1_wrapper(v_w1_out.data(), stepWeightShape);

  std::vector<float> v_w2_out(stepWeightElms, -77.0f);
  popart::NDArrayWrapper<float> w2_wrapper(v_w2_out.data(), stepWeightShape);

  std::vector<float> v_w3_out(stepWeightElms, -77.0f);
  popart::NDArrayWrapper<float> w3_wrapper(v_w3_out.data(), stepWeightShape);

  std::vector<float> v_w4_out(stepWeightElms, -77.0f);
  popart::NDArrayWrapper<float> w4_wrapper(v_w4_out.data(), stepWeightShape);

  std::vector<float> v_w5_out(stepWeightElms, -77.0f);
  popart::NDArrayWrapper<float> w5_wrapper(v_w5_out.data(), stepWeightShape);

  std::map<popart::TensorId, popart::IArray &> anchors = {{act5, act5_wrapper},
                                                          {w1, w1_wrapper},
                                                          {w2, w2_wrapper},
                                                          {w3, w3_wrapper},
                                                          {w4, w4_wrapper},
                                                          {w5, w5_wrapper}};

  WeightsIO weightsRead;
  std::vector<float> w0_readback(weightInfo.nelms(), -99.0f);
  std::vector<float> w1_readback(weightInfo.nelms(), -99.0f);
  std::vector<float> w2_readback(weightInfo.nelms(), -99.0f);
  std::vector<float> w3_readback(weightInfo.nelms(), -99.0f);
  std::vector<float> w4_readback(weightInfo.nelms(), -99.0f);
  std::vector<float> w5_readback(weightInfo.nelms(), -99.0f);
  weightsRead.insert(w0, {w0_readback.data(), weightInfo});
  weightsRead.insert(w1, {w1_readback.data(), weightInfo});
  weightsRead.insert(w2, {w2_readback.data(), weightInfo});
  weightsRead.insert(w3, {w3_readback.data(), weightInfo});
  weightsRead.insert(w4, {w4_readback.data(), weightInfo});
  weightsRead.insert(w5, {w5_readback.data(), weightInfo});

  // write initial weights to host
  session->weightsFromHost();

  // read final weights back
  session->weightsToHost();
  session->readWeights(weightsRead);

  // write followed by immediate read : read back initializer values
  std::cout << "Testing that immediately read back initializer vals are correct"
            << std::endl;
  BOOST_CHECK(w0_readback.back() == w0Vals.back());
  BOOST_CHECK(w1_readback.back() == w1Vals.back());
  BOOST_CHECK(w2_readback.back() == w2Vals.back());
  BOOST_CHECK(w3_readback.back() == w3Vals.back());
  BOOST_CHECK(w4_readback.back() == w4Vals.back());
  BOOST_CHECK(w5_readback.back() == w5Vals.back());

  for (int i = 0; i < 3; ++i) {
    std::cout << "Iteration (call to run(...)) # " << i << std::endl;

    // we will use the same data (inputs) for every step
    popart::StepIO stepio(inputs, anchors);

    // process the 400 samples (100 batches), streaming back anchors each batch
    session->run(stepio);

    // read final weights back
    session->weightsToHost();
    session->readWeights(weightsRead);

    std::cout << "Testing that final (weight) anchors are the same as values "
                 "from readWeights()"
              << std::endl;

    BOOST_CHECK(w1_readback.back() == v_w1_out.back());
    BOOST_CHECK(w2_readback.back() == v_w2_out.back());
    BOOST_CHECK(w3_readback.back() == v_w3_out.back());
    BOOST_CHECK(w4_readback.back() == v_w4_out.back());
    BOOST_CHECK(w5_readback.back() == v_w5_out.back());

    std::cout << "all the w1 anchor returns" << std::endl;
    for (auto x : v_w1_out) {
      std::cout << x << "  ";
    }
    std::cout << std::endl;
  }
}


#include <ros/ros.h>
#include <Eigen/Core>
#include <amcl3d/KdTreeNanoPclWrapper.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/plane_clipper3D.h>

#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/angles.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/gicp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/surface/convex_hull.h>

#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/sample_consensus/sac_model_parallel_plane.h>
#include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

#include <vector>
#include <algorithm>
#include <queue>
#include <iostream>
#include <memory>
#include <fstream>
#include <string>
#include <limits>

#define PointType pcl::PointXYZI

int main(int argc, char** argv){
    ros::init(argc, argv, "point_cloud_registration_node");
    ros::NodeHandle n;

    ros::Rate t(100);

    Eigen::Matrix4f m;
    m << 0,  0,  1, 0,
         1,  0,  0, 0,
         0,  1,  0, 0,
         0,  0,  0, 1;

    pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>());

    pcl::io::loadPCDFile("/home/ayush/Downloads/finalCloudCorrected.pcd", *cloud);
    pcl::PassThrough<PointType> m_passThroughZ;
    m_passThroughZ.setFilterFieldName("z");
    m_passThroughZ.setFilterLimits(-1.0, 10.0);
    m_passThroughZ.setInputCloud(cloud);
    m_passThroughZ.filter(*cloud);
    // pcl::transformPointCloud(*cloud, *cloud, m);

    pcl::visualization::CloudViewer viewer ("Cluster viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped ())
    {
    }
    pcl::io::savePCDFileASCII("/home/ayush/Downloads/finalCloud.pcd", *cloud);

}


#ifndef DEFINING_ELEMENT_X8346D3
#define DEFINING_ELEMENT_X8346D3
// #define e 2.71828
// #define pi 3.14159
#include <bits/stdc++.h>
// #include <Eigen/Eigen>

// using namespace Eigen;
using namespace std;

class matrix
{
public:
    float **data;
    int shape[2];

    matrix()
    {
        this->shape[0] = 0;
        this->shape[1] = 0;
    }

    matrix(int a, int b)
    {
        this->shape[0] = a;
        this->shape[1] = b;
        this->data = (float **)malloc(sizeof(float *) * a);
        for (int i = 0; i < a; i++)
        {
            this->data[i] = (float *)malloc(sizeof(float) * b);
        }
        for (int i = 0; i < a; i++)
        {
            for (int j = 0; j < b; j++)
            {
                this->data[i][j] = 0;
            }
        }
    }

    void print()
    {
        printf("\n{ ");
        printf("Class : matrix , shape = [ %d , %d ] \n", this->shape[0], this->shape[1]);
        printf("  [ \n");
        for (int i = 0; i < this->shape[0]; i++)
        {
            printf("   [ ");
            for (int j = 0; j < this->shape[1]; j++)
            {
                if (j == this->shape[1] - 1)
                {
                    printf("%f ", data[i][j]);
                }
                else
                {
                    printf("%f , ", data[i][j]);
                }
            }
            if (i == this->shape[0] - 1)
                printf(" ]\n");
            else
            {
                printf(" ] ,\n");
            }
        }
        printf("  ] \n}\n", this->shape[0], this->shape[1]);
    }

    friend matrix operator>=(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] >= n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend matrix operator<=(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] <= n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend matrix operator>(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] > n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend matrix operator<(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] > n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend int operator==(matrix const m, matrix const &n)
    {
        if (m.shape[0] != n.shape[0] || m.shape[1] != n.shape[1])
        {
            return 0;
        }
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] != n.data[i][j])
                {
                    return 0;
                }
            }
        }
        return 1;
    }

    friend int operator!=(matrix const m, matrix const &n)
    {
        if (m.shape[0] != n.shape[0] || m.shape[1] != n.shape[1])
        {
            return 1;
        }

        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] != n.data[i][j])
                {
                    return 1;
                }
            }
        }
        return 0;
    }

    matrix operator()(int index)
    {
        matrix res(shape[0], 1);
        for (int i = 0; i < shape[0]; i++)
        {
            res.data[i][0] = this->data[i][index];
        }
        return res;
    }

    matrix operator()(int index1, int index2, int axis = 0)
    {
        if (axis == 0)
        {
            if (!((index1 >= 0 && index1 < shape[0]) && (index2 >= 0 && index2 < shape[0]) && index1 < index2))
            {
                fprintf(stderr, "enter correct indexing and axis , m.shape = [%d  %d ]", shape[0], shape[1]);
                exit(1);
            }
            matrix res(index2 - index1, shape[1]);
            for (int i = index1; i < index2; i++)
            {
                for (int j = 0; j < shape[1]; j++)
                {
                    res.data[i - index1][j] = data[i][j];
                }
            }

            return res;
        }
        else if (axis == 1)
        {
            if (!((index1 >= 0 && index1 < shape[1]) && (index2 >= 0 && index2 < shape[1]) && index1 < index2))
            {
                fprintf(stderr, "enter correct indexing and axis , m.shape = [%d  %d ]", shape[0], shape[1]);
                exit(1);
            }
            matrix res(shape[0], index2 - index1);

            for (int i = 0; i < shape[0]; i++)
            {
                for (int j = index1; j < index2; j++)
                {
                    res.data[i][j - index1] = data[i][j];
                }
            }

            return res;
        }
    }

    friend matrix operator/(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] / m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] / m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] / m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] / m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] / m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] / m2.data[i][j];
                }
            }
            return res;
        }
        else
        {
            fprintf(stderr, "cannot divide element wise ,dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator/(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = n / (m.data[i][j]);
            }
        }
        return res;
    }

    friend matrix operator/(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = (m.data[i][j]) / n;
            }
        }
        return res;
    }

    // friend matrix operator+(matrix const m1, matrix const m2)
    // {
    //     if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
    //     {
    //         matrix res(m1.shape[0], m2.shape[1]);
    //         for (int i = 0; i < m1.shape[0]; i++)
    //         {
    //             for (int j = 0; j < m1.shape[1]; j++)
    //             {
    //                 res.data[i][j] = m1.data[i][j] + m2.data[i][j];
    //             }
    //         }
    //         return res;
    //     }
    // }

    friend matrix operator+(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] + m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][0];
                }
            }
            return res;
        }
        // else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        // {
        //     matrix res(m2.shape[0], m2.shape[1]);
        //     for (int i = 0; i < m2.shape[0]; i++)
        //     {
        //         for (int j = 0; j < m2.shape[1]; j++)
        //         {
        //             res.data[i][j] = m1.data[0][0] + m2.data[i][j];
        //         }
        //     }
        //     return res;
        // }

        else
        {
            fprintf(stderr, "cannot add dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator+(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] + n;
            }
        }
        return res;
    }

    friend matrix operator+(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] + n;
            }
        }
        return res;
    }

    friend matrix operator-(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] - m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] - m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] - m2.data[i][j];
                }
            }
            return res;
        }
        else
        {
            fprintf(stderr, "cannot subtract ,dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator-(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] - n;
            }
        }
        return res;
    }

    friend matrix operator-(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = n - m.data[i][j];
            }
        }
        return res;
    }

    friend matrix operator*(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] * m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] * m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] * m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] * m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] * m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] * m2.data[i][j];
                }
            }
            return res;
        }
        else
        {
            fprintf(stderr, "cannot multiple element wise, dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator*(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] * n;
            }
        }
        return res;
    }

    friend matrix operator*(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] * n;
            }
        }
        return res;
    }

    friend matrix operator^(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = pow(m.data[i][j], n);
            }
        }
        return res;
    }
    matrix T()
    {
        matrix m1(this->shape[1], this->shape[0]);

        for (int i = 0; i < shape[0]; i++)
        {
            for (int j = 0; j < shape[1]; j++)
            {
                m1.data[j][i] = this->data[i][j];
            }
        }
        return m1;
    }
};

matrix randn(int a, int b)
{
    matrix m(a, b);
    srand(time(0));
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            m.data[i][j] = 2 * (rand() / (float)RAND_MAX) - 1;
        }
    }
    return m;
}

void print(matrix m)
{
    printf("\n{ ");
    printf("Class : matrix , shape = [ %d , %d ] \n", m.shape[0], m.shape[1]);
    printf("  [ \n");
    for (int i = 0; i < m.shape[0]; i++)
    {
        printf("   [ ");
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (j == m.shape[1] - 1)
            {
                printf("%f ", m.data[i][j]);
            }
            else
            {
                printf("%f , ", m.data[i][j]);
            }
        }
        if (i == m.shape[0] - 1)
            printf(" ]\n");
        else
        {
            printf(" ] ,\n");
        }
    }
    printf("  ] \n}", m.shape[0], m.shape[1]);
}

matrix take_matrix(char *path)
{
    FILE *fp;
    fp = fopen(path, "r");
    int dim1, dim2;
    fscanf(fp, "%d %d", &dim1, &dim2);
    matrix X(dim1, dim2);
    for (int i = 0; i < dim1; i++)
    {
        for (int j = 0; j < dim2; j++)
        {
            fscanf(fp, "%f", &X.data[i][j]);
        }
    }

    return X;
}

matrix zeros(int a, int b)
{
    matrix m(a, b);
    for (int i = 0; i < a; i++)
    {
        for (int j = 0; j < b; j++)
        {
            m.data[i][j] = 0;
        }
    }
    return m;
}

matrix dot(matrix m1, matrix m2)
{
    if (m1.shape[1] != m2.shape[0])
    {
        fprintf(stderr, "ERROR : Dimensions Mismatch in performing dot product m1.shape = [%d ,%d]  and  m2.shape = [%d ,%d]\n", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
        exit(1);
    }
    matrix multi(m1.shape[0], m2.shape[1]);

    for (int i = 0; i < m1.shape[0]; ++i)
    {
        for (int j = 0; j < m2.shape[1]; ++j)
        {
            for (int k = 0; k < m1.shape[1]; ++k)
            {
                multi.data[i][j] += m1.data[i][k] * m2.data[k][j];
            }
        }
    }
    return multi;
}

matrix abs(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (m.data[i][j] > 0)
                res.data[i][j] = m.data[i][j];
            else
                res.data[i][j] = -1 * m.data[i][j];
        }
    }
    return res;
}

matrix exp(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = pow(2.71828, m.data[i][j]);
        }
    }
    return res;
}

matrix log(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (m.data[i][j] < 0)
            {
                fprintf(stderr, "Domain error in log");
                exit(1);
            }
            else if (m.data[i][j] == 0)
                res.data[i][j] = 0;
            else
                res.data[i][j] = log(m.data[i][j]);
        }
    }
    return res;
}

float sum(matrix m)
{

    float sum = 0;
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            sum += m.data[i][j];
        }
    }
    return sum;
}

matrix m_sum(matrix m, int axis = -1)
{
    if (axis == 0)
    {
        matrix res(1, m.shape[1]);
        for (int i = 0; i < m.shape[1]; i++)
        {
            float sum = 0;
            for (int j = 0; j < m.shape[0]; j++)
            {
                sum = sum + m.data[j][i];
            }
            res.data[0][i] = sum;
        }
        return res;
    }
    else if (axis == 1)
    {
        matrix res(m.shape[0], 1);
        for (int i = 0; i < m.shape[0]; i++)
        {
            float sum = 0;
            for (int j = 0; j < m.shape[1]; j++)
            {
                sum = sum + m.data[i][j];
            }
            res.data[i][0] = sum;
        }
        return res;
    }
    else
    {
        matrix res = zeros(1, 1);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[0][0] += m.data[i][j];
            }
        }
        return res;
    }
}

matrix sigmoid(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = 1.0 / (1.0 + pow(2.71828, -1 * m.data[i][j]));
        }
    }
    return res;
}

matrix tanh(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = tanh(m.data[i][j]);
        }
    }
    return res;
}
matrix relu(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (m.data[i][j] >= 0)
            {
                res.data[i][j] = m.data[i][j];
            }
            else
            {
                res.data[i][j] = 0;
            }
        }
    }
    return res;
}

matrix normalize(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        float max = m.data[i][0];
        float min = m.data[i][0];
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (max < m.data[i][j])
            {
                max = m.data[i][j];
            }
            if (min > m.data[i][j])
            {
                min = m.data[i][j];
            }
        }

        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = (m.data[i][j] - min) / (max - min);
        }
    }
    return res;
}
#endif

// matrix cost(matrix Y, matrix A_L)
// {
//     float m = Y.shape[1];
//     matrix cost = -1 * m_sum(Y * log(A_L) + (1 - Y) * log(1 - A_L), 1) / m;
//     return cost;
// }

// int main()
// {
//     matrix m1 = randn(3, 5);
//     print(m1);
//     matrix m2 = sigmoid(m1);
//     print(m2);
//     print(1 - m2);
//     print(log(m2));
// }

// int main()
// {
//     clock_t start, end;
//     float time;
//     printf("Matrix : Computing....\n");
//     for (int i = 10; i <= 1000; i += 20)
//     {
//         start = clock();
//         matrix m1 = randn(i, i), m2 = randn(i, i);
//         matrix m3 = dot(m1, m2.T());
//         end = clock();
//         float time = (float)(end - start) / CLOCKS_PER_SEC;
//         printf("%f\n", time);

//         //     start = clock();
//         //     MatrixXf m4 = MatrixXf::Random(i, i), m5 = MatrixXf::Random(i, i);
//         //     MatrixXf temp = m4 * m5;
//         //     end = clock();
//         //     time = (float)(end - start) / CLOCKS_PER_SEC;
//         //     printf("%f", time);
//         // }
//     }
// }

// class test
// {
// private:
//     matrix A = zeros(0, 0), B = zeros(0, 0);

// public:
//     void get_data(matrix X, matrix Y)
//     {
//         this->A = X;
//         this->B = Y;
//     }

//     void print_a_b()
//     {
//         this->A.print();
//         this->B.print();
//     }
// };

// int main()
// {
//     test t;
//     matrix m1=zeros(10 ,4),m2=randn(10,3);
//     t.get_data(m1,m2);
//     t.print_a_b();
//}

// int main()
// {
// matrix m = randn(100, 100);
// matrix m1 = sigmoid(m);
// matrix m2 = log(m1);
// for (int i = 0; i < 100; i++)
// {
//     for (int j = 0; j < 100; j++)
//     {
//         if (!(m1.data[i][j] < 1 && m1.data[i][j] > 0))
//         {
//             printf("Error");
//         }
//     }
// }

// float ans;
// ans = 1 / (1 + pow(2.71828, 14));
// printf("%f",ans );

// printf("%f",log(0.001));
// }

// int main()
// {
//     matrix m = randn(10, 6);
//     matrix m2 = m(2, 4, 1);
//     m.print();
//     m2.print();
// }