// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm core result_value
#include <boost/test/unit_test.hpp>
#include "fern/algorithm/core/result_value.h"

namespace fa = fern::algorithm;

#define verify_result_value_type(                                 \
    A1, A2, TypeWeWant)                                           \
{                                                                 \
    using TypeWeGet = fa::ResultValue<A1, A2>::type;              \
                                                                  \
    BOOST_CHECK_MESSAGE((std::is_same<TypeWeGet, TypeWeWant>()),  \
        fern::TypeTraits<TypeWeGet>::name + " != " +              \
        fern::TypeTraits<TypeWeWant>::name);                      \
}

BOOST_AUTO_TEST_CASE(ResultValue)
{
    // uint + uint
    // Pick largest uint type.
    verify_result_value_type(uint8_t, uint8_t, uint8_t);
    verify_result_value_type(uint8_t, uint16_t, uint16_t);
    verify_result_value_type(uint8_t, uint32_t, uint32_t);
    verify_result_value_type(uint8_t, uint64_t, uint64_t);

    verify_result_value_type(uint16_t, uint16_t, uint16_t);
    verify_result_value_type(uint16_t, uint32_t, uint32_t);
    verify_result_value_type(uint16_t, uint64_t, uint64_t);

    verify_result_value_type(uint32_t, uint32_t, uint32_t);
    verify_result_value_type(uint32_t, uint64_t, uint64_t);

    verify_result_value_type(uint64_t, uint64_t, uint64_t);

    // int + int
    // Pick largest int type.
    verify_result_value_type(int8_t, int8_t, int8_t);
    verify_result_value_type(int8_t, int16_t, int16_t);
    verify_result_value_type(int8_t, int32_t, int32_t);
    verify_result_value_type(int8_t, int64_t, int64_t);

    verify_result_value_type(int16_t, int16_t, int16_t);
    verify_result_value_type(int16_t, int32_t, int32_t);
    verify_result_value_type(int16_t, int64_t, int64_t);

    verify_result_value_type(int32_t, int32_t, int32_t);
    verify_result_value_type(int32_t, int64_t, int64_t);

    verify_result_value_type(int64_t, int64_t, int64_t);

    // uint + int
    // Pick a signed int type that can store values from both types. If there
    // is no such type, pick int64_t.
    verify_result_value_type(uint8_t, int8_t, int16_t);
    verify_result_value_type(uint8_t, int16_t, int16_t);
    verify_result_value_type(uint8_t, int32_t, int32_t);
    verify_result_value_type(uint8_t, int64_t, int64_t);

    verify_result_value_type(uint16_t, int8_t, int32_t);
    verify_result_value_type(uint16_t, int16_t, int32_t);
    verify_result_value_type(uint16_t, int32_t, int32_t);
    verify_result_value_type(uint16_t, int64_t, int64_t);

    verify_result_value_type(uint32_t, int8_t, int64_t);
    verify_result_value_type(uint32_t, int16_t, int64_t);
    verify_result_value_type(uint32_t, int32_t, int64_t);
    verify_result_value_type(uint32_t, int64_t, int64_t);

    verify_result_value_type(uint64_t, int8_t, int64_t);
    verify_result_value_type(uint64_t, int16_t, int64_t);
    verify_result_value_type(uint64_t, int32_t, int64_t);
    verify_result_value_type(uint64_t, int64_t, int64_t);

    // float + float
    // Pick the largest float type.
    verify_result_value_type(float, float, float);
    verify_result_value_type(double, double, double);
    verify_result_value_type(float, double, double);

    // uint + float
    verify_result_value_type(uint8_t, float, float);
    verify_result_value_type(uint8_t, double, double);
    verify_result_value_type(uint16_t, float, float);
    verify_result_value_type(uint16_t, double, double);
    verify_result_value_type(uint32_t, float, float);
    verify_result_value_type(uint32_t, double, double);
    verify_result_value_type(uint64_t, float, float);
    verify_result_value_type(uint64_t, double, double);

    // int + float
    verify_result_value_type(int8_t, float, float);
    verify_result_value_type(int8_t, double, double);
    verify_result_value_type(int16_t, float, float);
    verify_result_value_type(int16_t, double, double);
    verify_result_value_type(int32_t, float, float);
    verify_result_value_type(int32_t, double, double);
    verify_result_value_type(int64_t, float, float);
    verify_result_value_type(int64_t, double, double);
}


#pragma once
#ifndef OPENGM_SUBGRADIENT_SSVM_LEARNER_HXX
#define OPENGM_SUBGRADIENT_SSVM_LEARNER_HXX

#include <iomanip>
#include <vector>
#include <opengm/inference/inference.hxx>
#include <opengm/graphicalmodel/weights.hxx>
#include <opengm/utilities/random.hxx>
#include <opengm/learning/gradient-accumulator.hxx>
#include <opengm/learning/weight_averaging.hxx>

#ifdef WITH_OPENMP
#include <omp.h>
#endif

#include <boost/circular_buffer.hpp>

namespace opengm {
    namespace learning {

    template<class DATASET>
    class SubgradientSSVM
    {
    public:
        typedef DATASET DatasetType;
        typedef typename DATASET::GMType   GMType;
        typedef typename DATASET::GMWITHLOSS GMWITHLOSS;
        typedef typename DATASET::LossType LossType;
        typedef typename GMType::ValueType ValueType;
        typedef typename GMType::IndexType IndexType;
        typedef typename GMType::LabelType LabelType;
        typedef opengm::learning::Weights<double> WeightsType;
        typedef typename std::vector<LabelType>::const_iterator LabelIterator;
        typedef FeatureAccumulator<GMType, LabelIterator> FeatureAcc;

        typedef std::vector<LabelType> ConfType;
        typedef boost::circular_buffer<ConfType> ConfBuffer;
        typedef std::vector<ConfBuffer> ConfBufferVec;

        class Parameter{
        public:

            enum LearningMode{
                Online = 0,
                Batch = 1
            };

            Parameter(){
                eps_ = 0.00001;
                maxIterations_ = 10000;
                stopLoss_ = 0.0;
                learningRate_ = 1.0;
                C_ = 1.0;
                learningMode_ = Batch;
                averaging_ = -1;
                nConf_ = 0;
            }

            double eps_;
            size_t maxIterations_;
            double stopLoss_;
            double learningRate_;
            double C_;
            LearningMode learningMode_;
            int averaging_;
            int nConf_;
        };

        SubgradientSSVM(DATASET&, const Parameter& );

        template<class INF>
        void learn(const typename INF::Parameter& para);
        //template<class INF, class VISITOR>
        //void learn(typename INF::Parameter para, VITITOR vis);

        const opengm::learning::Weights<double>& getWeights(){return weights_;}
        Parameter& getLerningParameters(){return para_;}

        double getLearningRate( )const{
            if(para_.decayExponent_<=0.000000001 && para_.decayExponent_>=-0.000000001 ){
                return 1.0;
            }
            else{
                return std::pow(para_.decayT0_ + static_cast<double>(iteration_),para_.decayExponent_);
            }
        }

        double getLoss(const GMType & gm ,const GMWITHLOSS  & gmWithLoss, std::vector<LabelType> & labels){

            double loss = 0 ;
            std::vector<LabelType> subConf(20,0);

            for(size_t fi=gm.numberOfFactors(); fi<gmWithLoss.numberOfFactors(); ++fi){
                for(size_t v=0; v<gmWithLoss[fi].numberOfVariables(); ++v){
                    subConf[v] = labels[ gmWithLoss[fi].variableIndex(v)];
                }
                loss +=  gmWithLoss[fi](subConf.begin());
            }
            return loss;
        }

    private:

        double updateWeights();

        DATASET& dataset_;
        WeightsType  weights_;
        Parameter para_;
        size_t iteration_;
        FeatureAcc featureAcc_;
        WeightRegularizer<ValueType> wReg_;
        WeightAveraging<double> weightAveraging_;
    };

    template<class DATASET>
    SubgradientSSVM<DATASET>::SubgradientSSVM(DATASET& ds, const Parameter& p )
    :   dataset_(ds),
        para_(p),
        iteration_(0),
        featureAcc_(ds.getNumberOfWeights()),
        wReg_(2, 1.0/p.C_),
        weightAveraging_(ds.getWeights(),p.averaging_)
    {
        featureAcc_.resetWeights();
        weights_ = opengm::learning::Weights<double>(ds.getNumberOfWeights());
    }

    template<class DATASET>
    template<class INF>
    void SubgradientSSVM<DATASET>::learn(const typename INF::Parameter& para){

        typedef typename INF:: template RebindGm<GMWITHLOSS>::type InfLossGm;
        typedef typename InfLossGm::Parameter InfLossGmParam;
        InfLossGmParam infLossGmParam(para);

        const size_t nModels = dataset_.getNumberOfModels();
        const size_t nWegihts = dataset_.getNumberOfWeights();

        for(size_t wi=0; wi<nWegihts; ++wi){
            dataset_.getWeights().setWeight(wi, 0.0);
        }
        std::cout<<"PARAM nConf_"<<para_.nConf_<<"\n";
        const bool useWorkingSets = para_.nConf_>0;

        ConfBufferVec buffer(useWorkingSets? nModels : 0, ConfBuffer(para_.nConf_));

        std::vector<bool> isViolated(para_.nConf_);

        if(para_.learningMode_ == Parameter::Online){
            RandomUniform<size_t> randModel(0, nModels);
            //std::cout<<"online mode\n";
            for(iteration_=0 ; iteration_<para_.maxIterations_; ++iteration_){

                // get random model
                const size_t gmi = randModel();
                // lock the model
                dataset_.lockModel(gmi);
                const GMWITHLOSS & gmWithLoss = dataset_.getModelWithLoss(gmi);

                // do inference
                std::vector<LabelType> arg;
                opengm::infer<InfLossGm>(gmWithLoss, infLossGmParam, arg);
                featureAcc_.resetWeights();
                featureAcc_.accumulateModelFeatures(dataset_.getModel(gmi), dataset_.getGT(gmi).begin(), arg.begin());
                dataset_.unlockModel(gmi);

                // update weights
                const double wChange =updateWeights();

                if(iteration_%nModels*2 == 0 ){
                    std::cout << '\r'
                              << std::setw(6) << std::setfill(' ') << iteration_ << ':'
                              << std::setw(8) << dataset_. template getTotalLossParallel<INF>(para) <<"  "<< std::flush;

                }

            }
        }
        else if(para_.learningMode_ == Parameter::Batch){
            //std::cout<<"batch mode\n";
            for(iteration_=0 ; iteration_<para_.maxIterations_; ++iteration_){
                // this

                // reset the weights
                featureAcc_.resetWeights();
                double totalLoss = 0;

                #ifdef WITH_OPENMP
                omp_lock_t modelLockUnlock;
                omp_init_lock(&modelLockUnlock);
                omp_lock_t featureAccLock;
                omp_init_lock(&featureAccLock);
                #pragma omp parallel for reduction(+:totalLoss)
                #endif
                for(size_t gmi=0; gmi<nModels; ++gmi){

                    // lock the model
                    #ifdef WITH_OPENMP
                    omp_set_lock(&modelLockUnlock);
                    dataset_.lockModel(gmi);
                    omp_unset_lock(&modelLockUnlock);
                    #else
                    dataset_.lockModel(gmi);
                    #endif

                    const GMWITHLOSS & gmWithLoss = dataset_.getModelWithLoss(gmi);
                    const GMType     & gm = dataset_.getModel(gmi);
                    //run inference
                    std::vector<LabelType> arg;
                    opengm::infer<InfLossGm>(gmWithLoss, infLossGmParam, arg);

                    totalLoss = totalLoss + getLoss(gm, gmWithLoss, arg);

                    if(useWorkingSets){
                        // append current solution
                        buffer[gmi].push_back(arg);

                        size_t vCount=0;
                        // check which violates
                        for(size_t cc=0; cc<buffer[gmi].size(); ++cc){
                            const double mLoss = dataset_.getLoss(buffer[gmi][cc], gmi);
                            const double argVal = gm.evaluate(buffer[gmi][cc]);
                            const double gtVal =  gm.evaluate(dataset_.getGT(gmi));
                            const double ll = (argVal - mLoss) - gtVal;
                            //std::cout<<" argVal "<<argVal<<" gtVal "<<gtVal<<" mLoss "<<mLoss<<"   VV "<<ll<<"\n";
                            if(ll<0){
                                isViolated[cc] = true;
                                ++vCount;
                            }
                        }
                        FeatureAcc featureAcc(nWegihts);
                        for(size_t cc=0; cc<buffer[gmi].size(); ++cc){
                            if(isViolated[cc]){

                                featureAcc.accumulateModelFeatures(gm, dataset_.getGT(gmi).begin(), buffer[gmi][cc].begin(),1.0/double(vCount));

                            }
                        }
                        #ifdef WITH_OPENMP
                        omp_set_lock(&featureAccLock);
                        featureAcc_.accumulateFromOther(featureAcc);
                        omp_unset_lock(&featureAccLock);
                        #else
                        featureAcc_.accumulateFromOther(featureAcc);
                        #endif
                    }
                    else{
                        FeatureAcc featureAcc(nWegihts);
                        featureAcc.accumulateModelFeatures(gm, dataset_.getGT(gmi).begin(), arg.begin());
                        #ifdef WITH_OPENMP
                        omp_set_lock(&featureAccLock);
                        featureAcc_.accumulateFromOther(featureAcc);
                        omp_unset_lock(&featureAccLock);
                        #else
                        featureAcc_.accumulateFromOther(featureAcc);
                        #endif
                    }

                    // acc features
                    //omp_set_lock(&featureAccLock);
                    //featureAcc_.accumulateFromOther(featureAcc);
                    //omp_unset_lock(&featureAccLock);

                    // unlock the model
                    #ifdef WITH_OPENMP
                    omp_set_lock(&modelLockUnlock);
                    dataset_.unlockModel(gmi);
                    omp_unset_lock(&modelLockUnlock);
                    #else
                    dataset_.unlockModel(gmi);
                    #endif

                }

                //const double wRegVal = wReg_(dataset_.getWeights());
                //const double tObj = std::abs(totalLoss) + wRegVal;
                if(iteration_%1==0){
                    std::cout << '\r'
                              << std::setw(6) << std::setfill(' ') << iteration_ << ':'
                              << std::setw(8) << -1.0*totalLoss <<"  "<< std::flush;
                }
                // update the weights
                const double wChange =updateWeights();

            }
        }
        weights_ = dataset_.getWeights();
    }

    template<class DATASET>
    double SubgradientSSVM<DATASET>::updateWeights(){

        const size_t nWegihts = dataset_.getNumberOfWeights();

        WeightsType p(nWegihts);
        WeightsType newWeights(nWegihts);

        if(para_.learningMode_ == Parameter::Batch){
            for(size_t wi=0; wi<nWegihts; ++wi){
                p[wi] =  dataset_.getWeights().getWeight(wi);
                p[wi] += para_.C_ * featureAcc_.getWeight(wi)/double(dataset_.getNumberOfModels());
            }
        }
        else{
            for(size_t wi=0; wi<nWegihts; ++wi){
                p[wi] =  dataset_.getWeights().getWeight(wi);
                p[wi] += para_.C_ * featureAcc_.getWeight(wi);
            }
        }

        double wChange = 0.0;

        for(size_t wi=0; wi<nWegihts; ++wi){
            const double wOld = dataset_.getWeights().getWeight(wi);
            const double wNew = wOld - (para_.learningRate_/double(iteration_+1))*p[wi];
            newWeights[wi] = wNew;
        }

        weightAveraging_(newWeights);

        weights_ = dataset_.getWeights();
        return wChange;
    }
}
}
#endif


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_DETAIL_IMPLODER_HPP
#define BOOST_HASH_DETAIL_IMPLODER_HPP

#include <boost/hash/stream_endian.hpp>
#include <boost/hash/detail/unbounded_shift.hpp>
#include <boost/static_assert.hpp>

#include <climits>
#include <cstring>

namespace boost {
namespace hashes {
namespace detail {

// By definition, for all imploders, InputBits < OutputBits,
// so we're taking many smaller values and combining them into one value

template <typename Endianness,
          int InputBits, int OutputBits,
          int k>
struct imploder_step;

template <int UnitBits, int InputBits, int OutputBits, int k>
struct imploder_step<stream_endian::big_unit_big_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutputValue>
    static void step(InputValue z, OutputValue &x) {
        int const shift = OutputBits - (InputBits+k);
        OutputValue y = low_bits<InputBits>(OutputValue(z));
        x |= unbounded_shl<shift>(y);
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct imploder_step<stream_endian::little_unit_big_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutputValue>
    static void step(InputValue z, OutputValue &x) {
        int const kb = (k % UnitBits);
        int const ku = k - kb;
        int const shift =
            InputBits  >= UnitBits ? k :
            OutputBits >= UnitBits ? ku + (UnitBits-(InputBits+kb)) :
                                     OutputBits - (InputBits+kb);
        OutputValue y = low_bits<InputBits>(OutputValue(z));
        x |= unbounded_shl<shift>(y);
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct imploder_step<stream_endian::big_unit_little_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutputValue>
    static void step(InputValue z, OutputValue &x) {
        int const kb = (k % UnitBits);
        int const ku = k - kb;
        int const shift =
            InputBits  >= UnitBits ? OutputBits - (InputBits+k) :
            OutputBits >= UnitBits ? OutputBits - (UnitBits+ku) + kb :
                                     kb;
        OutputValue y = low_bits<InputBits>(OutputValue(z));
        x |= unbounded_shl<shift>(y);
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct imploder_step<stream_endian::little_unit_little_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutputValue>
    static void step(InputValue z, OutputValue &x) {
        int const shift = k;
        OutputValue y = low_bits<InputBits>(OutputValue(z));
        x |= unbounded_shl<shift>(y);
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct imploder_step<stream_endian::host_unit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutputValue>
    static void step(InputValue z, OutputValue &x) {
        BOOST_STATIC_ASSERT(sizeof(InputValue)*CHAR_BIT == InputBits);
        BOOST_STATIC_ASSERT(sizeof(OutputValue)*CHAR_BIT == OutputBits);
        std::memcpy((char*)&x + k/CHAR_BIT, &z, InputBits/CHAR_BIT);
    }
};

template <typename Endianness,
          int InputBits, int OutputBits,
          int k = 0>
struct imploder;

template <template <int> class Endian, int UnitBits,
          int InputBits, int OutputBits,
          int k>
struct imploder<Endian<UnitBits>, InputBits, OutputBits, k> {

    // To keep the implementation managable, input and output sizes must
    // be multiples or factors of the unit size.
    // If one of these is firing, you may want a bit-only stream_endian
    // rather than one that mentions bytes or octets.
    BOOST_STATIC_ASSERT(!(InputBits  % UnitBits && UnitBits % InputBits ));
    BOOST_STATIC_ASSERT(!(OutputBits % UnitBits && UnitBits % OutputBits));

    typedef Endian<UnitBits> Endianness;
    typedef imploder_step<Endianness, InputBits, OutputBits, k> step_type;
    typedef imploder<Endianness, InputBits, OutputBits, k+InputBits> next_type;

    template <typename InIter, typename OutputValue>
    static void implode(InIter &in, OutputValue &x) {
        step_type::step(*in++, x);
        next_type::implode(in, x);
    }

};
template <template <int> class Endian, int UnitBits,
          int InputBits, int OutputBits>
struct imploder<Endian<UnitBits>, InputBits, OutputBits, OutputBits> {
    template <typename InIter, typename OutputValue>
    static void implode(InIter &, OutputValue &) {}
};

} // namespace detail
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_DETAIL_IMPLODER_HPP


/*
 * Copyright 2018 James Dyer
 *
 * 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.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <NTL/RR.h>
#include <json/json.h>
#include "HE2Encrypter.h"
#include "Random.h"

HE2Encrypter::HE2Encrypter(int lambda, int eta){
	generateParameters(lambda,eta);
}

HE2Encrypter::HE2Encrypter(int n, int d, int rho)
{
    /* Compute lower bound for p */
	NTL::RR rtwo(2);
	NTL::RR exp1(rho*d);
	NTL::RR nplusone(n+1);
	NTL::RR exp2(d);
	NTL::RR ploBound = pow(rtwo,exp1)*pow(nplusone,exp2);
	NTL::ZZ pLowerBound = to_ZZ(ploBound);

	/* Derive lambda and eta */
	long lambda = NumBits(pLowerBound) + 1;
	long eta = ((lambda * lambda / rho) - lambda);
	generateParameters(lambda,eta);
}

NTL::vec_ZZ_p HE2Encrypter::encrypt(NTL::ZZ& plaintext)
{
    NTL::ZZ_p r = to_ZZ_p(rng->nextBigInteger(q));
    NTL::ZZ_p s = to_ZZ_p(rng->nextBigInteger(modulus));
    NTL::ZZ_p ptext = to_ZZ_p(plaintext);
    NTL::ZZ_p c = ptext + r*(*pmod);
    return ONE_VECTOR*c + (*a)*s;
}

std::string HE2Encrypter::writeSecretsToJSON()
{
	Json::Value root;
	std::ostringstream pStr,modStr,gammaStr;
	pStr << p;
	root["p"] = pStr.str();
	modStr << modulus;
	root["modulus"] = modStr.str();
	gammaStr << (*gamma);
    root["gamma"]=gammaStr.str();
	Json::FastWriter writer;
	return writer.write(root);
}


/* vim: set tabstop=4 : */
#ifndef __terark_io_custom_int_h__
#define __terark_io_custom_int_h__

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

#include <boost/config.hpp>
#include <boost/static_assert.hpp>
#include <terark/stdtypes.hpp>

// should be the last #include
#include <boost/type_traits/detail/bool_trait_def.hpp>

namespace terark {

template<class BaseInt, int Bits>
class custom_int
{
	BaseInt m_val;
	BOOST_STATIC_ASSERT(Bits % 8 == 0);
public:
	typedef BaseInt presentation_type;
	BOOST_STATIC_CONSTANT(int, bits  = Bits);
	BOOST_STATIC_CONSTANT(int, bytes = Bits / 8);

	custom_int(BaseInt val = BaseInt()) : m_val(val % (BaseInt(1) << Bits)) {}

	operator BaseInt() const { return m_val; }

	BaseInt& value() { return m_val; }
	BaseInt  value() const { return m_val; }

#define TERARK_custom_int_gen_operator(op) \
	BaseInt operator op(custom_int y) const { return m_val op y.m_val; }

	TERARK_custom_int_gen_operator(+)
	TERARK_custom_int_gen_operator(-)
	TERARK_custom_int_gen_operator(*)
	TERARK_custom_int_gen_operator(/)
	TERARK_custom_int_gen_operator(%)
	TERARK_custom_int_gen_operator(^)
	TERARK_custom_int_gen_operator(&)
	TERARK_custom_int_gen_operator(|)
	TERARK_custom_int_gen_operator(<<)
	TERARK_custom_int_gen_operator(>>)

#undef TERARK_custom_int_gen_operator

	bool operator&&(custom_int y) const { return m_val && y.m_val; }
	bool operator||(custom_int y) const { return m_val || y.m_val; }
	bool operator!() const { return !m_val; }

	BaseInt operator~() const { return ~m_val; }
	BaseInt operator-() const { return -m_val; }

	custom_int operator+() const { return +m_val; }

	custom_int operator++() { return m_val = ++m_val % (BaseInt(1) << Bits); }
	custom_int operator--() { return m_val = --m_val % (BaseInt(1) << Bits); }

	custom_int operator++(int)
	{
		BaseInt t = m_val;
		m_val = ++m_val % (BaseInt(1) << Bits);
		return t;
	}
	custom_int operator--(int)
	{
		BaseInt t = m_val;
		m_val = --m_val % (BaseInt(1) << Bits);
		return t;
	}

#define TERARK_custom_int_gen_assign_op(op)\
	custom_int& operator op(custom_int y)	\
	{	m_val op y;						\
		m_val %= (BaseInt(1) << Bits);	\
		return *this;					\
	}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	TERARK_custom_int_gen_assign_op(+=)
	TERARK_custom_int_gen_assign_op(-=)
	TERARK_custom_int_gen_assign_op(*=)
#undef TERARK_custom_int_gen_assign_op

// not need: "m_val %= (BaseInt(1) << Bits);"
#define TERARK_custom_int_gen_assign_op(op)\
	custom_int& operator op(custom_int y)	\
	{	m_val op y;						\
		return *this;					\
	}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	TERARK_custom_int_gen_assign_op(/=)
	TERARK_custom_int_gen_assign_op(%=)
	TERARK_custom_int_gen_assign_op(&=)
	TERARK_custom_int_gen_assign_op(|=)
	TERARK_custom_int_gen_assign_op(^=)
	TERARK_custom_int_gen_assign_op(<<=)
	TERARK_custom_int_gen_assign_op(>>=)
#undef TERARK_custom_int_gen_assign_op

	template<class Input> friend void DataIO_loadObject(Input& input, custom_int& x)
	{
		unsigned char b;
		x.m_val = 0;
		for (int bytes = 0; bytes != Bits/8; ++bytes)
		{
			input >> b;
			x.m_val = x.m_val << 8 | b;
		}
	}
	template<class Output> friend void DataIO_saveObject(Output& output, custom_int x)
	{
		BaseInt y = x.m_val;
		for (int bytes = 0; bytes != Bits/8; ++bytes)
		{
			output << b;
			y >>= 8;
		}
	}
};

typedef custom_int<int32_t, 24> int24_t;
typedef custom_int<int64_t, 40> int40_t;
typedef custom_int<int64_t, 48> int48_t;
typedef custom_int<int64_t, 56> int56_t;

typedef custom_int<uint32_t, 24> uint24_t;
typedef custom_int<uint64_t, 40> uint40_t;
typedef custom_int<uint64_t, 48> uint48_t;
typedef custom_int<uint64_t, 56> uint56_t;

BOOST_TT_AUX_BOOL_TRAIT_DEF1(is_custom_int, T, false)

BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, int24_t, true)
BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, int40_t, true)
BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, int48_t, true)
BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, int56_t, true)

BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, uint24_t, true)
BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, uint40_t, true)
BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, uint48_t, true)
BOOST_TT_AUX_BOOL_TRAIT_CV_SPEC1(is_custom_int, uint56_t, true)

//! declaration
template<class IntType> struct presentation_type;

template<> struct presentation_type<int8_t>  { typedef int8_t type; };
template<> struct presentation_type<int16_t> { typedef int16_t type; };
template<> struct presentation_type<int32_t> { typedef int32_t type; };
template<> struct presentation_type<int64_t> { typedef int64_t type; };

template<> struct presentation_type<uint8_t>  { typedef uint8_t type; };
template<> struct presentation_type<uint16_t> { typedef uint16_t type; };
template<> struct presentation_type<uint32_t> { typedef uint32_t type; };
template<> struct presentation_type<uint64_t> { typedef uint64_t type; };

template<> struct presentation_type<int24_t> { typedef int32_t type; };
template<> struct presentation_type<int40_t> { typedef int64_t type; };
template<> struct presentation_type<int48_t> { typedef int64_t type; };
template<> struct presentation_type<int56_t> { typedef int64_t type; };

template<> struct presentation_type<uint24_t> { typedef uint32_t type; };
template<> struct presentation_type<uint40_t> { typedef uint64_t type; };
template<> struct presentation_type<uint48_t> { typedef uint64_t type; };
template<> struct presentation_type<uint56_t> { typedef uint64_t type; };

} // namespace terark

#include "boost/type_traits/detail/bool_trait_undef.hpp"

#endif // __terark_io_custom_int_h__


#ifndef MEMBRANECELLPROPERTY_HPP
#define MEMBRANECELLPROPERTY_HPP

//general includes
#include <vector>
#include <string>
#include <boost/shared_ptr.hpp>

// chaste includes
#include "Cell.hpp"
#include "AbstractCellProperty.hpp"
#include "StateVariableRegister.hpp"
#include "AbstractMembraneReactionSystem.hpp"
#include "AbstractMembraneOdeSystem.hpp"
#include "AbstractIvpOdeSolver.hpp"
#include "EulerIvpOdeSolver.hpp"

class MembraneCellProperty : public AbstractCellProperty
{
protected:

    // CellPtr to a given cell for access to cell data and for properties that relate to the membrane ode
    CellPtr m_membrane_cellPtr;

    // for the calculation of gradients
    bool mIsExtent =false;
    double mMembraneThickness = 0.0;

    bool mIsDoubleMembrane =false;

    // membrane constituent ; lipids, proteins, channels etc
    // register for the state variables present in the membrane region
    StateVariableRegister* mpMembraneStateVariableRegister;
    // concentration vector of the membrane, if double memebrane
    std::vector<double> mMembraneConcentrationVector;

    // register for variables that cross the membrane; either side
    StateVariableRegister* mpBulkStateVariableRegister;
    StateVariableRegister* mpCellStateVariableRegister;

    AbstractMembraneReactionSystem* mpMembraneReactionSystem;

    // membrane reaction system Ode corresponding to the mpMembraneReactionSystem
    AbstractMembraneOdeSystem* mpMembraneOdeSystem;

    boost::shared_ptr<AbstractIvpOdeSolver> mpMembraneOdeSolver;

    // concentration of state variables at the internal boundary of the cell if this is to remain constant
    bool mIsConstantCellConcentration = false; // for use in averaged source for example?

    std::vector<double> mBulkBoundaryConcentrationVector;

    std::vector<double> mChangeBulkBoundaryConcentrationVector;

    std::vector<double> mCellBoundaryConcentrationVector;

    std::vector<double> mChangeCellBoundaryConcentrationVector;

    bool mIncludeMembraneOdeInterpolationOnBoundary=false;

    //15/10/2020
    std::vector<double> mInitBulkBoundaryConcentrationVector;

    unsigned mNumberOCalls_this_reaction_step =0;

public:

    MembraneCellProperty();

    virtual ~MembraneCellProperty();

    MembraneCellProperty(const MembraneCellProperty&);

    // virtual methods

    virtual void SetUp(AbstractMembraneReactionSystem*, CellPtr);

    virtual void UpdateMembraneConcentrationVector(std::vector<double>);

    virtual void UpdateCellConcentrationVector(std::vector<double>);

    virtual void UpdateBulkConcentrationVector(std::vector<double>);

    virtual void PerformMembraneSystem(const std::vector<double>&, const std::vector<double>&, std::vector<double>&, std::vector<double>&);

    virtual void UpdateMembraneReactionSystem(AbstractMembraneReactionSystem*);

    virtual void UpdateMembraneOdeSystem(AbstractMembraneOdeSystem*);

    virtual void PreparePostDivisionParent(double);

    virtual void PreparePostDivisionDaughter(const MembraneCellProperty& ,double);

    virtual void InitialiseMembrane(std::vector<std::string>, std::vector<double>);

    virtual void InitialiseMembrane(StateVariableRegister*, std::vector<double>);

    // concrete methods

    void SetUpCellConcentrationVector(unsigned);

    void SetUpBulkConcentrationVector(unsigned);

    void SetUpChangeCellConcentrationVector(unsigned);

    void SetUpChangeBulkConcentrationVector(unsigned);

    double RetrieveBoundarySourceByStateName(std::string);

    double RetrieveChangeBoundarySourceByStateName(std::string);

    void AppendInternalCellBoundaryConcentrations(std::vector<double>&);

    void ReplaceBoundaryStateVariables(std::vector<double>&);

    void ReplaceChangeBoundaryStateVariables(std::vector<double>&);

    void ResetReactionCalls();

    unsigned GetReactionCalls();

    // set methods

    void SetMembraneExtentBool(bool);

    void SetMembraneThickness(double);

    void SetDoubleMembraneBool(bool);

    void SetBulkStateVariableRegister(StateVariableRegister*);

    void SetCellStateVariableRegister(StateVariableRegister*);

    void SetConstantCellConcentrationBool(bool);

    void SetCellBoundaryConcentrationVector(std::vector<double>);

    void SetBulkBoundaryConcentrationVector(std::vector<double>);

    void SetInitBulkBoundaryConcentrationVector(std::vector<double>);

    void SetMembraneOdeSolver(boost::shared_ptr<AbstractIvpOdeSolver>);

    void SetIncludeMembraneOdeInterpolationOnBoundary(bool);

    void SetCellPtr(CellPtr);

    // get methods

    bool GetMembraneExtentBool();

    double GetMembraneThickness();

    bool GetDoubleMembraneBool();

    StateVariableRegister* GetBulkStateVariableRegister();

    StateVariableRegister* GetCellStateVariableRegister();

    bool GetConstantCellConcentrationBool();

    std::vector<double> GetInternalCellBoundaryConcentrationVector();

    double GetInternalCellBoundaryConcentrationByIndex(unsigned);

    double GetInternalCellBoundaryConcentrationByName(std::string);

    double GetChangeInternalCellBoundaryConcentrationByName(std::string);

    std::vector<double> GetExternalCellBoundaryConcentrationVector();

    double GetExternalCellBoundaryConcentrationByIndex(unsigned);

    double GetChangeExternalCellBoundaryConcentrationByIndex(unsigned);

    double GetExternalCellBoundaryConcentrationByName(std::string);

    AbstractMembraneReactionSystem* GetMembraneReactionSystem();

    AbstractMembraneOdeSystem* GetMembraneOdeSystem();

    boost::shared_ptr<AbstractIvpOdeSolver> GetMembraneOdeSolver();

    bool GetIncludeMembraneOdeInterpolationOnBoundary();

    CellPtr GetCellPtr();

    // membrane parameters

    void SetMembraneStateVariableRegister(StateVariableRegister*);

    void SetMembraneConcentrationVector(std::vector<double>);

    StateVariableRegister* GetMembraneStateVariableRegister();

    std::vector<double> GetMembraneConcentrationVector();

    double GetMembraneConcentrationByIndex(unsigned);

    double GetMembraneConcentrationByName(std::string);

};

#endif


#include "myFunctions.h"
#include <iostream>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <pcl/visualization/cloud_viewer.h>
#include <android/log.h>
#define LOGI(...) ((void)__android_log_print(ANDROID_LOG_INFO, "PointCloud", __VA_ARGS__))

using namespace Eigen;

MyReg::MyReg()
{
    width_orig=320;
    height_orig=240;
    width_ds=64;
    height_ds=48;

}

void  MyReg::DownSampling(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr src, PointCloud::Ptr tgt)
{
    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);

        for(int  i=0;i<height_ds;i++)
        {
            for(int j=0;j<width_ds;j++)
            {

                int iold=i*height_orig/height_ds;
                int jold=j*width_orig/width_ds;

                (*src).points[i*width_ds+j]=(*cloud_src).points[iold*width_orig+jold];
                (*tgt).points[i*width_ds+j]=(*cloud_tgt).points[iold*width_orig+jold];

            /*    if(isnan((*cloud_src).points[iold*width_orig+jold].x) || isnan((*cloud_src).points[iold*width_orig+jold].y) || isnan((*cloud_src).points[iold*width_orig+jold].z) )
                {
                    (*src).points[i*width_ds+j].x=0;
                    (*src).points[i*width_ds+j].y=0;
                    (*src).points[i*width_ds+j].z=0;
                }

                if(isnan((*cloud_tgt).points[iold*width_orig+jold].x) || isnan((*cloud_tgt).points[iold*width_orig+jold].y) || isnan((*cloud_tgt).points[iold*width_orig+jold].z) )
                {
                    (*tgt).points[i*width_ds+j].x=0;
                    (*tgt).points[i*width_ds+j].y=0;
                    (*tgt).points[i*width_ds+j].z=0;
                }
            */
            }
        }

    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
    std::cout<<"Time of downsampling: "<<diff(time1,time2).tv_sec<<":"<<diff(time1,time2).tv_nsec<<std::endl;
LOGI("Time  of downsampling: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
}
void  MyReg::Filter(PointCloud::Ptr src, PointCloud::Ptr tgt)
{
    Eigen::Matrix4f myTi = Eigen::Matrix4f::Identity ();
    PointCloud::Ptr tmp (new PointCloud(width_ds,height_ds));
    pcl::transformPointCloud (*src, *tmp, myTi);

    for(int  i=0;i<height_ds;i++)
    {
        for(int j=0;j<width_ds;j++)
        {
           float sq = ( (*tgt).points[i*width_ds+j].x-(*tmp)[i*width_ds+j].x) * ((*tgt).points[i*width_ds+j].x-(*tmp)[i*width_ds+j].x)+
                   ( (*tgt).points[i*width_ds+j].y-(*tmp)[i*width_ds+j].y) * ((*tgt).points[i*width_ds+j].y-(*tmp)[i*width_ds+j].y)+
                   ( (*tgt).points[i*width_ds+j].z-(*tmp)[i*width_ds+j].z) * ((*tgt).points[i*width_ds+j].z-(*tmp)[i*width_ds+j].z);
           std::cout << sq << "\t";

      /*     if(sq<0.01) {
               (*src).points[i*width_ds+j].x=0;
               (*src).points[i*width_ds+j].y=0;
               (*src).points[i*width_ds+j].z=0;
                       }
       */
        }
        std::cout << std::endl;
    }

}
void  MyReg::OldDownSampling (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr src, PointCloud::Ptr tgt )
    {
  /*  pcl::VoxelGrid<PointT> grid;
               clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);

    grid.setLeafSize (0.05, 0.05, 0.05);
    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
               cout<<"Time of setLeafSize: "<<diff(time1,time2).tv_sec<<":"<<diff(time1,time2).tv_nsec<<endl;

                  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);

    grid.setInputCloud (cloud_src);
    grid.filter (*src);

    grid.setInputCloud (cloud_tgt);
    grid.filter (*tgt);

              clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
              cout<<"Time of downsampling: "<<diff(time1,time2).tv_sec<<":"<<diff(time1,time2).tv_nsec<<endl;
*/
    }
void  MyReg::ZNormalization(pcl::PointCloud<PointNormalT>::Ptr  src, pcl::PointCloud<PointNormalT>::Ptr tgt)
{

    for(int  i=0;i<height_ds;i++)
    {
        for(int j=0;j<width_ds;j++)
        {

            std::cout << (*src).points[i*width_ds+j];

            ////PlaneProjective

            double fx=52.5;
            double fy=52.5;
            double cx=64/2;
            double cy=48/2;

            int jj=fx*(*src).points[i*width_ds+j].x/(*src).points[i*width_ds+j].z+cx;
            int ii=fy*(*src).points[i*width_ds+j].y/(*src).points[i*width_ds+j].z+cy;

            std::cout << i <<"-"<<ii<<"\t";
            std::cout << j <<"-"<<jj<<endl;

            (*src).points[i*width_ds+j].x=(*src).points[i*width_ds+j].x/(*src).points[i*width_ds+j].z;
            (*src).points[i*width_ds+j].y=(*src).points[i*width_ds+j].y/(*src).points[i*width_ds+j].z;
            (*src).points[i*width_ds+j].z=1;

            (*tgt).points[i*width_ds+j].x=(*tgt).points[i*width_ds+j].x/(*tgt).points[i*width_ds+j].z;
            (*tgt).points[i*width_ds+j].y=(*tgt).points[i*width_ds+j].y/(*tgt).points[i*width_ds+j].z;
            (*tgt).points[i*width_ds+j].z=2;

            std::cout<<"--" << (*src).points[i*width_ds+j]<<std::endl;

/*
            if(isnan((*cloud_src).points[iold*width_orig+jold].x) || isnan((*cloud_src).points[iold*width_orig+jold].y) || isnan((*cloud_src).points[iold*width_orig+jold].z) ) {
                (*src).points[i*width_ds+j].x=0;
                (*src).points[i*width_ds+j].y=0;
                (*src).points[i*width_ds+j].z=0;
        }

            if(isnan((*cloud_tgt).points[iold*width_orig+jold].x) || isnan((*cloud_tgt).points[iold*width_orig+jold].y) || isnan((*cloud_tgt).points[iold*width_orig+jold].z) ) {
                (*tgt).points[i*width_ds+j].x=0;
                (*tgt).points[i*width_ds+j].y=0;
                (*tgt).points[i*width_ds+j].z=0;
        }
*/

        }

    }
}

void MyReg:: Normals(pcl::PointCloud<PointNormalT>::Ptr points_with_normals_src, pcl::PointCloud<PointNormalT>::Ptr points_with_normals_tgt,PointCloud::Ptr src, PointCloud::Ptr tgt)
{

      pcl::IntegralImageNormalEstimation<pcl::PointXYZ, PointNormalT> ne;
    //  pcl::NormalEstimation<PointT, PointNormalT> ne;
    //  ne.setKSearch(30);

      //pcl::search::OrganizedNeighbor<PointT>::Ptr organized (new pcl::search::OrganizedNeighbor<PointT> ());
      //ne.setSearchMethod (organized);

      ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);

      // ne.setMaxDepthChangeFactor(0.02f);
      // ne.setNormalSmoothingSize(100.0f);

      // ne.setRadiusSearch(0.002);
      // ne.setNormalSmoothingSize(0.001f);
      // ne.setKSearch(0);
      // ne.setMaxDepthChangeFactor(0.02f);
      // ne.setNormalSmoothingSize(0.01f);

                     clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);

      ne.setInputCloud(src);
      ne.compute(*points_with_normals_src);
      pcl::copyPointCloud (*src, *points_with_normals_src);

      ne.setInputCloud(tgt);
      ne.compute(*points_with_normals_tgt);
      pcl::copyPointCloud (*tgt, *points_with_normals_tgt);

                  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
                  cout<<"Time of normal_estimation: "<<diff(time1,time2).tv_sec<<":"<<diff(time1,time2).tv_nsec<<endl;
LOGI("Time of normal_estimation :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);

  /*
            for(int  i=0;i<height_ds;i++)
            {
                for(int j=0;j<width_ds;j++)
                {
             //   (*points_with_normals_src)[i*width_ds+j].curvature=0.01f*i;
             //   (*points_with_normals_tgt)[i*width_ds+j].curvature=0.01f*j;
                    std::cout << (*src)[i*width_ds+j];
                    std::cout << (*points_with_normals_src)[i*width_ds+j]<<"\t";
                }
                std::cout << endl;

              }

  */

  /*  pcl::visualization::PCLVisualizer viewer("PCL Viewer1");
    viewer.setBackgroundColor (0.0, 0.0, 0.5);

    viewer.addPointCloudNormals<pcl::PointXYZ,PointNormalT>(src, points_with_normals_src,1);

    while (!viewer.wasStopped ())
    {
      viewer.spinOnce ();
    }

    showCloudsRight(points_with_normals_tgt, points_with_normals_src);
  p->spin();

    //////////////////////////////
  */
}
void MyReg::OldNormals(pcl::PointCloud<PointNormalT>::Ptr points_with_normals_src, pcl::PointCloud<PointNormalT>::Ptr points_with_normals_tgt,PointCloud::Ptr src, PointCloud::Ptr tgt)
{

      clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);

      pcl::NormalEstimation<PointT, PointNormalT> norm_est;

     // pcl::search::OrganizedNeighbor<PointT>::Ptr organized (new pcl::search::OrganizedNeighbor<PointT> ());
    //  norm_est.setSearchMethod (organized);
    //  norm_est.setRadiusSearch(0.3);

      pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
      norm_est.setSearchMethod (tree);
      norm_est.setKSearch (30);

      norm_est.setInputCloud (src);
      norm_est.compute (*points_with_normals_src);
      pcl::copyPointCloud (*src, *points_with_normals_src);

      norm_est.setInputCloud (tgt);
      norm_est.compute (*points_with_normals_tgt);
      pcl::copyPointCloud (*tgt, *points_with_normals_tgt);

                          clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
                          cout<<"Time of compute surface normals and curvature: "<<diff(time1,time2).tv_sec<<":"<<diff(time1,time2).tv_nsec<<endl;
LOGI("Time of compute surface normals and curvature: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);

                          for(int  i=0;i<height_ds;i++)
                          {
                              for(int j=0;j<width_ds;j++)
                              {
                                  std::cout << (*src)[i*width_ds+j];
                                  std::cout << (*points_with_normals_src)[i*width_ds+j]<<"\t";
                              }
                              std::cout << endl;

                            }

}

void MyReg::MatrixEstimation(pcl::PointCloud<PointNormalT>::Ptr src, pcl::PointCloud<PointNormalT>::Ptr tgt, Eigen::Matrix4f & GlobalTransf)
{
    std::cout <<"MatrixEstimation start" << std::endl;

 //pcl::visualization::PCLVisualizer viewer("PCL Viewer1");

    Eigen::Matrix<double, 64*48, 6, Eigen::RowMajor> A;
    Eigen::Matrix<double, 64*48, 1, Eigen::ColMajor> b;

    GlobalTransf << 1,0,0,0,
                    0,1,0,0,
                    0,0,1,0,
                    0,0,0,1;

    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);

    for(int s=0;s<10;s++)
    {
        int q=0;
        for(int  i=0;i<height_ds;i++)
        {
            for(int j=0;j<width_ds;j++)
            {
                if(     isnan((*src).points[i*width_ds+j].x) ||
                        isnan((*src).points[i*width_ds+j].y) ||
                        isnan((*src).points[i*width_ds+j].z)
                   )
                {
                continue;
                }

                double six=(*src).points[i*width_ds+j].x;
                double siy=(*src).points[i*width_ds+j].y;
                double siz=(*src).points[i*width_ds+j].z;

                double fx=52.5;
                double fy=52.5;
                double cx=64/2;
                double cy=48/2;

                if((*src).points[i*width_ds+j].z==0)
                {continue;
                }

                int jj=fx*(*src).points[i*width_ds+j].x/(*src).points[i*width_ds+j].z+cx;
                int ii=fy*(*src).points[i*width_ds+j].y/(*src).points[i*width_ds+j].z+cy;

                 if(       isnan((*tgt).points[ii*width_ds+jj].x) ||
                           isnan((*tgt).points[ii*width_ds+jj].y) ||
                           isnan((*tgt).points[ii*width_ds+jj].z) ||

                           isnan((*tgt).points[ii*width_ds+jj].normal_x) ||
                           isnan((*tgt).points[ii*width_ds+jj].normal_y) ||
                           isnan((*tgt).points[ii*width_ds+jj].normal_z)
                           )
                 {continue;}

                 double dix=(*tgt).points[ii*width_ds+jj].x;
                 double diy=(*tgt).points[ii*width_ds+jj].y;
                 double diz=(*tgt).points[ii*width_ds+jj].z;

                 float sq = ( dix-six) * (dix-six)+
                          (diy-siy) * (diy-siy)+
                          (diz-siz) * (diz-siz);

                 if (sq>0.01) {continue;}

                 double nix=(*tgt).points[ii*width_ds+jj].normal_x;
                 double niy=(*tgt).points[ii*width_ds+jj].normal_y;
                 double niz=(*tgt).points[ii*width_ds+jj].normal_z;

                 double ai1=niz*siy-niy*siz;
                 double ai2=nix*siz-niz*six;
                 double ai3=niy*six-nix*siy;

                 A(q,0)=ai1;
                 A(q,1)=ai2;
                 A(q,2)=ai3;
                 A(q,3)=nix;
                 A(q,4)=niy;
                 A(q,5)=niz;

                 b(q,0)=nix*dix+niy*diy+niz*diz-nix*six-niy*siy-niz*siz;

                 q++;
            }
        }

        Eigen::MatrixXd mA(q,6);
        Eigen::MatrixXd mb(q,1);
        mA=A.block(0,0,q,6);
        mb=b.block(0,0,q,1);

       // std::cout<<"rows:" <<mA.rows();

        MatrixXd AA=mA.transpose() * mA;
        VectorXd bb=mA.transpose() * mb;
        VectorXd result;

      /*  std::cout <<"AT*A"<<std::endl;
        std::cout << AA;

        std::cout <<"AT*b"<<std::endl;
        std::cout << bb;
*/
        //AA.svd().solve(bb, &result);
        // result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
         result = AA.jacobiSvd(ComputeThinU | ComputeThinV).solve(bb);

       //  result = AA.llt().solve(bb);
  //       std::cout<<"\n\n result \n"<< result;

     Eigen::Matrix<float,4,4> M;
     M  <<  1.,-result(2),result(1),result(3),
            result(2),1.,-result(0),result(4),
            -result(1),-result(0),1.,result(5),
            0,0,0,1.;

  //   std::cout<< M<< std::endl;

  //   std::cout<< Rinc<< std::endl;

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

     float alpha = result (0);
     float beta  = result (1);
     float gamma = result (2);
     Eigen::Matrix3f Rinc = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
     RT.topLeftCorner(3,3)=Rinc;
     RT(0,3)=result(3);
     RT(1,3)=result(4);
     RT(2,3)=result(5);

     GlobalTransf=RT*GlobalTransf;

 //    std::cout<<"!!!!!!!!!!!!"<< RT<<std::endl;

    PointCloudWithNormals::Ptr transf (new PointCloudWithNormals);

      pcl::transformPointCloud (*src, *transf,  RT);
    //  std::cout<< "afterTransformSource"<<std::endl;
      src=transf;

  /*   Vector3f tinc = result.tail<3> ();
     //compose
     tcurr = Rinc * tcurr + tinc;
     Rcurr = Rinc * Rcurr;
*/

    }

    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
    cout<<"\n Time of AT*A: "<<diff(time1,time2).tv_sec<<":"<<diff(time1,time2).tv_nsec<<endl;
 LOGI("Time of AT*A:%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
/*
    std::cout<<"\n GlobalTransform\n "<< GlobalTransf <<std::endl;

    viewer.removePointCloud ("vp1");
    viewer.removePointCloud ("vp2");
    pcl::visualization::PointCloudColorHandlerCustom<PointNormalT> src_h (src, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<PointNormalT> tgt_h (tgt, 255, 0, 0);
    viewer.addPointCloud (tgt, tgt_h, "vp1");
    viewer.addPointCloud (src,src_h, "vp2");
    viewer.spin();
*/

    for(int  i=0;i<height_ds;i++)
    {
        for(int j=0;j<width_ds;j++)
        {
            (*src).points[i*width_ds+j].curvature=1;
        }
    }

    //////////////////registration////////////////////////

    //points_with_normals_src
    //points_with_normals_tgt

    //Eigen::Matrix4f TM = Eigen::Matrix4f::Identity ();

    /*
     Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
     Eigen::Matrix<double, 6, 1> b;

     A.transpose;
     Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
            float alpha = result (0);
            float beta  = result (1);
            float gamma = result (2);

    */

/*
        Eigen::Matrix<double,4,4> twist;
        twist << 0., -ksi_ptr[2], ksi_ptr[1], ksi_ptr[3],
                 ksi_ptr[2], 0., -ksi_ptr[0], ksi_ptr[4],
                 -ksi_ptr[1], ksi_ptr[0], 0, ksi_ptr[5],
                 0., 0., 0., 0.;
        g = twist.exp();

        eigen2cv(g, Rt);
    #else
        // TODO: check computeProjectiveMatrix when there is not eigen library,
        // because it gives less accurate pose of the camera
        Rt = Mat::eye(4, 4, CV_64FC1);

        Mat R = Rt(Rect(0,0,3,3));
        Mat rvec = ksi.rowRange(0,3);

        Rodrigues(rvec, R);

        Rt.at<double>(0,3) = ksi.at<double>(3);
        Rt.at<double>(1,3) = ksi.at<double>(4);
        Rt.at<double>(2,3) = ksi.at<double>(5);
    #endif
    }
*/

    //////////////////end registration////////////////////////

}


#include "VisionSolverMPC.h"
#include "../convexMPC/common_types.h"
#include "VisionMPC_interface.h"
#include "VisionRobotState.h"
#include <Eigen/Dense>
#include <cmath>
#include <unsupported/Eigen/MatrixFunctions>
#include <qpOASES.hpp>
#include <stdio.h>
#include <sys/time.h>

#define V_BIG_NUMBER 5e10
//big enough to act like infinity, small enough to avoid numerical weirdness.

VisionRobotState v_rs;
using std::cout;
using std::endl;
using Eigen::Dynamic;

Matrix<fpt,Dynamic,13> vA_qp;
Matrix<fpt,Dynamic,Dynamic> vB_qp;
Matrix<fpt,13,12> vBdt;
Matrix<fpt,13,13> vAdt;
Matrix<fpt,25,25> vABc,v_expmm;
Matrix<fpt,Dynamic,Dynamic> vS;
Matrix<fpt,Dynamic,1> vX_d;
Matrix<fpt,Dynamic,1> vU_b;
Matrix<fpt,Dynamic,Dynamic> v_fmat;

Matrix<fpt,Dynamic,Dynamic> v_qH;
Matrix<fpt,Dynamic,1> v_qg;

Matrix<fpt,Dynamic,Dynamic> v_eye_12h;

qpOASES::real_t* vH_qpoases;
qpOASES::real_t* vg_qpoases;
qpOASES::real_t* vA_qpoases;
qpOASES::real_t* vlb_qpoases;
qpOASES::real_t* vub_qpoases;
qpOASES::real_t* vq_soln;

qpOASES::real_t* vH_red;
qpOASES::real_t* vg_red;
qpOASES::real_t* vA_red;
qpOASES::real_t* vlb_red;
qpOASES::real_t* vub_red;
qpOASES::real_t* vq_red;
u8 v_real_allocated = 0;

char v_var_elim[2000];
char v_con_elim[2000];

mfp* vision_get_q_soln() {
  return vq_soln;
}

s8 v_near_zero(fpt a)
{
  return (a < 0.01 && a > -.01) ;
}

s8 v_near_one(fpt a)
{
  return v_near_zero(a-1);
}
void v_matrix_to_real(qpOASES::real_t* dst, Matrix<fpt,Dynamic,Dynamic> src, s16 rows, s16 cols)
{
  s32 a = 0;
  for(s16 r = 0; r < rows; r++)
  {
    for(s16 c = 0; c < cols; c++)
    {
      dst[a] = src(r,c);
      a++;
    }
  }
}

void vision_c2qp(Matrix<fpt,13,13> Ac, Matrix<fpt,13,12> Bc,fpt dt,s16 horizon)
{
  vABc.setZero();
  vABc.block(0,0,13,13) = Ac;
  vABc.block(0,13,13,12) = Bc;
  vABc = dt*vABc;
  v_expmm = vABc.exp();
  vAdt = v_expmm.block(0,0,13,13);
  vBdt = v_expmm.block(0,13,13,12);
  if(horizon > 19) {
    throw std::runtime_error("horizon is too long!");
  }

  Matrix<fpt,13,13> powerMats[20];
  powerMats[0].setIdentity();
  for(int i = 1; i < horizon+1; i++) {
    powerMats[i] = vAdt * powerMats[i-1];
  }

  for(s16 r = 0; r < horizon; r++)
  {
    vA_qp.block(13*r,0,13,13) = powerMats[r+1];
    for(s16 c = 0; c < horizon; c++)
    {
      if(r >= c)
      {
        s16 a_num = r-c;
        vB_qp.block(13*r,12*c,13,12) = powerMats[a_num] * vBdt;
      }
    }
  }

}

void vision_resize_qp_mats(s16 horizon)
{
  int mcount = 0;
  int h2 = horizon*horizon;

  vA_qp.resize(13*horizon, Eigen::NoChange);
  mcount += 13*horizon*1;

  vB_qp.resize(13*horizon, 12*horizon);
  mcount += 13*h2*12;

  vS.resize(13*horizon, 13*horizon);
  mcount += 13*13*h2;

  vX_d.resize(13*horizon, Eigen::NoChange);
  mcount += 13*horizon;

  vU_b.resize(20*horizon, Eigen::NoChange);
  mcount += 20*horizon;

  v_fmat.resize(20*horizon, 12*horizon);
  mcount += 20*12*h2;

  v_qH.resize(12*horizon, 12*horizon);
  mcount += 12*12*h2;

  v_qg.resize(12*horizon, Eigen::NoChange);
  mcount += 12*horizon;

  v_eye_12h.resize(12*horizon, 12*horizon);
  mcount += 12*12*horizon;

  //printf("realloc'd %d floating point numbers.\n",mcount);
  mcount = 0;

  vA_qp.setZero();
  vB_qp.setZero();
  vS.setZero();
  vX_d.setZero();
  vU_b.setZero();
  v_fmat.setZero();
  v_qH.setZero();
  v_eye_12h.setIdentity();

  //TODO: use realloc instead of free/malloc on size changes

  if(v_real_allocated)
  {

    free(vH_qpoases);
    free(vg_qpoases);
    free(vA_qpoases);
    free(vlb_qpoases);
    free(vub_qpoases);
    free(vq_soln);
    free(vH_red);
    free(vg_red);
    free(vA_red);
    free(vlb_red);
    free(vub_red);
    free(vq_red);
  }

  vH_qpoases = (qpOASES::real_t*)malloc(12*12*horizon*horizon*sizeof(qpOASES::real_t));
  mcount += 12*12*h2;
  vg_qpoases = (qpOASES::real_t*)malloc(12*1*horizon*sizeof(qpOASES::real_t));
  mcount += 12*horizon;
  vA_qpoases = (qpOASES::real_t*)malloc(12*20*horizon*horizon*sizeof(qpOASES::real_t));
  mcount += 12*20*h2;
  vlb_qpoases = (qpOASES::real_t*)malloc(20*1*horizon*sizeof(qpOASES::real_t));
  mcount += 20*horizon;
  vub_qpoases = (qpOASES::real_t*)malloc(20*1*horizon*sizeof(qpOASES::real_t));
  mcount += 20*horizon;
  vq_soln = (qpOASES::real_t*)malloc(12*horizon*sizeof(qpOASES::real_t));
  mcount += 12*horizon;

  vH_red = (qpOASES::real_t*)malloc(12*12*horizon*horizon*sizeof(qpOASES::real_t));
  mcount += 12*12*h2;
  vg_red = (qpOASES::real_t*)malloc(12*1*horizon*sizeof(qpOASES::real_t));
  mcount += 12*horizon;
  vA_red = (qpOASES::real_t*)malloc(12*20*horizon*horizon*sizeof(qpOASES::real_t));
  mcount += 12*20*h2;
  vlb_red = (qpOASES::real_t*)malloc(20*1*horizon*sizeof(qpOASES::real_t));
  mcount += 20*horizon;
  vub_red = (qpOASES::real_t*)malloc(20*1*horizon*sizeof(qpOASES::real_t));
  mcount += 20*horizon;
  vq_red = (qpOASES::real_t*)malloc(12*horizon*sizeof(qpOASES::real_t));
  mcount += 12*horizon;
  v_real_allocated = 1;

  //printf("malloc'd %d floating point numbers.\n",mcount);

#ifdef K_DEBUG
  printf("RESIZED MATRICES FOR HORIZON: %d\n",horizon);
#endif
}

inline Matrix<fpt,3,3> cross_mat(Matrix<fpt,3,3> I_inv, Matrix<fpt,3,1> r)
{
  Matrix<fpt,3,3> cm;
  cm << 0.f, -r(2), r(1),
     r(2), 0.f, -r(0),
     -r(1), r(0), 0.f;
  return I_inv * cm;
}
//continuous time state space matrices.
void vision_ct_ss_mats(Matrix<fpt,3,3> vI_world, fpt m, Matrix<fpt,3,4> r_feet,
    Matrix<fpt,3,3> R_yaw, Matrix<fpt,13,13>& A, Matrix<fpt,13,12>& B, float x_drag)
{
  A.setZero();
  A(3,9) = 1.f;
  A(9,9) = x_drag;
  A(4,10) = 1.f;
  A(5,11) = 1.f;

  A(11,12) = 1.f;
  A.block(0,6,3,3) = R_yaw.transpose();

  B.setZero();
  Matrix<fpt,3,3> I_inv = vI_world.inverse();

  for(s16 b = 0; b < 4; b++)
  {
    B.block(6,b*3,3,3) = cross_mat(I_inv,r_feet.col(b));
    B.block(9,b*3,3,3) = Matrix<fpt,3,3>::Identity() / m;
  }
}

void vision_quat_to_rpy(Quaternionf q, Matrix<fpt,3,1>& rpy)
{
  //from my MATLAB implementation

  //edge case!
  fpt as = vision_t_min(-2.*(q.x()*q.z()-q.w()*q.y()),.99999);
  rpy(0) = atan2(2.f*(q.x()*q.y()+q.w()*q.z()),vision_sq(q.w()) + vision_sq(q.x()) - vision_sq(q.y()) - vision_sq(q.z()));
  rpy(1) = asin(as);
  rpy(2) = atan2(2.f*(q.y()*q.z()+q.w()*q.x()),vision_sq(q.w()) - vision_sq(q.x()) - vision_sq(q.y()) + vision_sq(q.z()));

}

Matrix<fpt,13,1> v_x_0;
Matrix<fpt,3,3> vI_world;
Matrix<fpt,13,13> vA_ct;
Matrix<fpt,13,12> vB_ct_r;

void vision_solve_mpc(vision_mpc_update_data_t* update, vision_mpc_problem_setup* setup)
{
  v_rs.set(update->p, update->v, update->q, update->w, update->r, update->yaw);

  //roll pitch yaw
  Matrix<fpt,3,1> rpy;
  vision_quat_to_rpy(v_rs.q,rpy);

  //initial state (13 state representation)
  v_x_0 << rpy(2), rpy(1), rpy(0), v_rs.p , v_rs.w, v_rs.v, -9.8f;
  vI_world = v_rs.R_yaw * v_rs.I_body * v_rs.R_yaw.transpose(); //original
  vision_ct_ss_mats(vI_world,v_rs.m,v_rs.r_feet,v_rs.R_yaw,vA_ct,vB_ct_r, update->x_drag);

  //QP matrices
  vision_c2qp(vA_ct,vB_ct_r,setup->dt,setup->horizon);

  //weights
  Matrix<fpt,13,1> full_weight;
  for(u8 i = 0; i < 12; i++)
    full_weight(i) = update->weights[i];
  full_weight(12) = 0.f;
  vS.diagonal() = full_weight.replicate(setup->horizon,1);

  //trajectory
  for(s16 i = 0; i < setup->horizon; i++)
  {
    for(s16 j = 0; j < 12; j++)
      vX_d(13*i+j,0) = update->traj[12*i+j];
  }
  //cout<<"XD:\n"<<vX_d<<endl;

  //note - I'm not doing the shifting here.
  s16 k = 0;
  for(s16 i = 0; i < setup->horizon; i++)
  {
    for(s16 j = 0; j < 4; j++)
    {
      vU_b(5*k + 0) = V_BIG_NUMBER;
      vU_b(5*k + 1) = V_BIG_NUMBER;
      vU_b(5*k + 2) = V_BIG_NUMBER;
      vU_b(5*k + 3) = V_BIG_NUMBER;
      vU_b(5*k + 4) = update->gait[i*4 + j] * setup->f_max;
      k++;
    }
  }

  fpt mu = 1.f/setup->mu;
  Matrix<fpt,5,3> f_block;

  f_block <<  mu, 0,  1.f,
          -mu, 0,  1.f,
          0,  mu, 1.f,
          0, -mu, 1.f,
          0,   0, 1.f;

  for(s16 i = 0; i < setup->horizon*4; i++)
  {
    v_fmat.block(i*5,i*3,5,3) = f_block;
  }

  v_qH = 2*(vB_qp.transpose()*vS*vB_qp + update->alpha*v_eye_12h);
  v_qg = 2*vB_qp.transpose()*vS*(vA_qp*v_x_0 - vX_d);

  v_matrix_to_real(vH_qpoases,v_qH,setup->horizon*12, setup->horizon*12);
  v_matrix_to_real(vg_qpoases,v_qg,setup->horizon*12, 1);
  v_matrix_to_real(vA_qpoases,v_fmat,setup->horizon*20, setup->horizon*12);
  v_matrix_to_real(vub_qpoases,vU_b,setup->horizon*20, 1);

  for(s16 i = 0; i < 20*setup->horizon; i++)
    vlb_qpoases[i] = 0.0f;

  s16 num_constraints = 20*setup->horizon;
  s16 num_variables = 12*setup->horizon;

  qpOASES::int_t nWSR = 100;

  int new_vars = num_variables;
  int new_cons = num_constraints;

  for(int i =0; i < num_constraints; i++)
    v_con_elim[i] = 0;

  for(int i = 0; i < num_variables; i++)
    v_var_elim[i] = 0;

  for(int i = 0; i < num_constraints; i++)
  {
    if(! (v_near_zero(vlb_qpoases[i]) && v_near_zero(vub_qpoases[i]))) continue;
    double* c_row = &vA_qpoases[i*num_variables];
    for(int j = 0; j < num_variables; j++)
    {
      if(v_near_one(c_row[j]))
      {
        new_vars -= 3;
        new_cons -= 5;
        int cs = (j*5)/3 -3;
        v_var_elim[j-2] = 1;
        v_var_elim[j-1] = 1;
        v_var_elim[j  ] = 1;
        v_con_elim[cs] = 1;
        v_con_elim[cs+1] = 1;
        v_con_elim[cs+2] = 1;
        v_con_elim[cs+3] = 1;
        v_con_elim[cs+4] = 1;
      }
    }
  }
  //if(new_vars != num_variables)
  if(1==1)
  {
    int var_ind[new_vars];
    int v_con_ind[new_cons];
    int vc = 0;
    for(int i = 0; i < num_variables; i++)
    {
      if(!v_var_elim[i])
      {
        if(!(vc<new_vars))
        {
          printf("BAD ERROR 1\n");
        }
        var_ind[vc] = i;
        vc++;
      }
    }
    vc = 0;
    for(int i = 0; i < num_constraints; i++)
    {
      if(!v_con_elim[i])
      {
        if(!(vc<new_cons))
        {
          printf("BAD ERROR 1\n");
        }
        v_con_ind[vc] = i;
        vc++;
      }
    }
    for(int i = 0; i < new_vars; i++)
    {
      int olda = var_ind[i];
      vg_red[i] = vg_qpoases[olda];
      for(int j = 0; j < new_vars; j++)
      {
        int oldb = var_ind[j];
        vH_red[i*new_vars + j] = vH_qpoases[olda*num_variables + oldb];
      }
    }

    for (int con = 0; con < new_cons; con++)
    {
      for(int st = 0; st < new_vars; st++)
      {
        float cval = vA_qpoases[(num_variables*v_con_ind[con]) + var_ind[st] ];
        vA_red[con*new_vars + st] = cval;
      }
    }
    for(int i = 0; i < new_cons; i++)
    {
      int old = v_con_ind[i];
      vub_red[i] = vub_qpoases[old];
      vlb_red[i] = vlb_qpoases[old];
    }

    qpOASES::QProblem problem_red (new_vars, new_cons);
    qpOASES::Options op;
    op.setToMPC();
    op.printLevel = qpOASES::PL_NONE;
    problem_red.setOptions(op);
    //int_t nWSR = 50000;

    int rval = problem_red.init(vH_red, vg_red, vA_red, NULL, NULL, vlb_red, vub_red, nWSR);
    (void)rval;
    int rval2 = problem_red.getPrimalSolution(vq_red);
    if(rval2 != qpOASES::SUCCESSFUL_RETURN)
      printf("failed to solve!\n");

    // printf("solve time: %.3f ms, size %d, %d\n", solve_timer.getMs(), new_vars, new_cons);

    vc = 0;
    for(int i = 0; i < num_variables; i++)
    {
      if(v_var_elim[i])
      {
        vq_soln[i] = 0.0f;
      }
      else
      {
        vq_soln[i] = vq_red[vc];
        vc++;
      }
    }
  }
}


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

#include <iostream>
#include <gtest/gtest.h>
#include <boost/foreach.hpp>
#include "../kalman_filter.h"
#include "../constant_speed.h"
#include <Eigen/Dense>

namespace snark{

namespace test
{

// basic test for constant speed model
// give position measurement that are approximately increasing,
// expect the state increasing
// expect the covariance increasing after predict, decreasing after update
TEST( constant_speed, simple )
{
    constant_speed<2>::model model( 0.2 );

    kalman_filter< constant_speed<2>::state, constant_speed<2>::model > filter( constant_speed<2>::state(), model );

    double deltaT = 0.1; // interval between measurements

    std::vector< Eigen::Vector2d > measurements( 10 );
    measurements[0] << 1,1;
    measurements[1] << 0.8,0.8;
    measurements[2] << 1.1,1.1;
    measurements[3] << 1,1;
    measurements[4] << 1.5,1.5;
    measurements[5] << 1.2,1.2;
    measurements[6] << 1.9,1.9;
    measurements[7] << 1.3,1.3;
    measurements[8] << 2.2,2.2;
    measurements[9] << 1.9,1.9;

    Eigen::Vector4d previousstate = Eigen::Vector4d::Zero();
    Eigen::Matrix4d previouscovariance = Eigen::Matrix4d::Zero();
    BOOST_FOREACH( const Eigen::Vector2d& meas, measurements )
    {
        constant_speed<2>::position m( meas, 0.3 );
        filter.predict( deltaT );
        EXPECT_GT( filter.state.covariance.determinant(), previouscovariance.determinant() );
        previouscovariance = filter.state.covariance;
        filter.update(m);
        EXPECT_LT( filter.state.covariance.determinant(), previouscovariance.determinant() );
        previouscovariance = filter.state.covariance;
        Eigen::Vector4d diff = filter.state.state_vector - previousstate;
        EXPECT_TRUE( diff.array().abs().isApprox( diff.array() ) );
        previousstate = filter.state.state_vector;
    }
}

} }

int main(int argc, char *argv[])
{
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/**
 * \file
 */
#ifndef MOOVE_BASIC_OP_PKGS_HPP
#define MOOVE_BASIC_OP_PKGS_HPP

#include "except.hpp"

#include <boost/cast.hpp>

namespace Moove {

template<class VariantType, class ValueType = typename VariantType::value_type>
struct set_value {
    void operator () (const VariantType& var, const ValueType& value)const
    { var.setValue(value); }
};

template<class ResultVariant, class GetValue, class SetValue = set_value<ResultVariant> >
class NumericOperatorPackage : public OperatorPackage {
private:
    GetValue    m_getValue;
    SetValue    m_setValue;

public:
    Reply doBinaryOp(Opcode op, std::unique_ptr<Variant> left, std::unique_ptr<Variant> right)const
    {
        using namespace boost;

	shared_ptr<ResultVariant> resultVar(polymorphic_cast<ResultVariant*>(ResultVariant::classFactory().create()));
	MOOVE_ASSERT(resultVar, "invalid type");

	switch(op) {
	    case OP_ADD:
                m_setValue(*resultVar, m_getValue(*left) + m_getValue(*right));
		break;

	    case OP_SUB:
		m_setValue(*resultVar, m_getValue(*left) - m_getValue(*right));
		break;

	    case OP_MUL:
		m_setValue(*resultVar, m_getValue(*left) * m_getValue(*right));
		break;

	    case OP_DIV:
		m_setValue(*resultVar, m_getValue(*left) / m_getValue(*right));
		break;

	    default:
		return OperatorPackage::doBinaryOp(op, left, right);
	}

	return Reply(Reply::NORMAL, resultVar);
    }
};

template<class ResultVariant, class Cast, class SetValue
}       //namespace Moove

#endif


/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include <iostream>
#include <limits>
#include <cmath>
#include "galois/DistGalois.h"
#include "galois/gstl.h"
#include "DistBenchStart.h"

#include "galois/DReducible.h"
#include "galois/AtomicWrapper.h"
#include "galois/ArrayWrapper.h"
#include "galois/runtime/Tracer.h"

#include "galois/graphs/DistributedGraphLoader.h"
#include <boost/serialization/vector.hpp>
#include <boost/serialization/array.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
using namespace boost::archive;
// For resilience
#include "resilience.h"

#ifdef __GALOIS_HET_CUDA__
#include "galois/cuda/cuda_device.h"
#include "gen_cuda.h"
struct CUDA_Context* cuda_ctx;
#endif

constexpr static const char* const regionname = "SGD";

/******************************************************************************/
/* Declaration of command line arguments */
/******************************************************************************/

namespace cll = llvm::cl;

static cll::opt<unsigned int>
    maxIterations("maxIterations",
                  cll::desc("Maximum iterations: Default 10000"),
                  cll::init(10000));
static cll::opt<bool> bipartite(
    "bipartite",
    cll::desc("Is graph bipartite? if yes, it expects first N nodes to have "
              "edges."),
    cll::init(false));
static cll::opt<double>
    LEARNING_RATE("LEARNING_RATE",
                  cll::desc("Learning rate (GAMMA): Default 0.00001"),
                  cll::init(0.00001));
static cll::opt<double> LAMBDA("LAMBDA", cll::desc("LAMBDA: Default 0.0001"),
                               cll::init(0.0001));
static cll::opt<double>
    DECAY_RATE("DECAY_RATE",
               cll::desc("Decay rate to be used in step size function "
                         "(DECAY_RATE): Default 0.9"),
               cll::init(0.9));
static cll::opt<double> tolerance(
    "tolerance",
    cll::desc("rms normalized tolerance for convergence:Default 0.01"),
    cll::init(0.01));

/******************************************************************************/
/* Graph structure declarations + helper functions + other initialization */
/******************************************************************************/

#define LATENT_VECTOR_SIZE 20
// static const double LEARNING_RATE = 0.00001; // GAMMA, Purdue: 0.01 Intel:
// 0.001 static const double DECAY_RATE = 0.9; // STEP_DEC, Purdue: 0.1 Intel:
// 0.9 static const double LAMBDA = 0.0001; // Purdue: 1.0 Intel: 0.001
static const double MINVAL = -1e+100;
static const double MAXVAL = 1e+100;

const unsigned int infinity = std::numeric_limits<unsigned int>::max() / 4;

struct NodeData {

  // galois::CopyableArray<galois::CopyableAtomic<double>, LATENT_VECTOR_SIZE>
  // residual_latent_vector; galois::CopyableArray<double, LATENT_VECTOR_SIZE>
  // latent_vector;

  std::vector<double> latent_vector;
  std::vector<galois::CopyableAtomic<double>> residual_latent_vector;

  template <class Archive>
  void serialize(Archive& ar, const unsigned int version) {
    ar& boost::serialization::make_array(latent_vector.data(),
                                         LATENT_VECTOR_SIZE);
    // ar & boost::serialization::make_array(residual_latent_vector.data(),
    // LATENT_VECTOR_SIZE);
  }
};

galois::DynamicBitSet bitset_latent_vector;
galois::DynamicBitSet bitset_residual_latent_vector;

typedef galois::graphs::DistGraph<NodeData, double> Graph;
// typedef galois::graphs::DistGraph<NodeData, uint32_t> Graph;
typedef typename Graph::GraphNode GNode;

#include "gen_sync.hh"
// TODO: Set seed
static double genRand() {
  // generate a random double in (-1,1)
  return 2.0 * ((double)std::rand() / (double)RAND_MAX) - 1.0;
}

static double genVal(uint32_t n) {
  return 2.0 * ((double)n / (double)RAND_MAX) - 1.0;
}

// Purdue learning function
double getstep_size(unsigned int round) {
  return LEARNING_RATE * 1.5 / (1.0 + DECAY_RATE * pow(round + 1, 1.5));
}

/**
 * Prediction of edge weight based on 2 latent vectors
 */
double calcPrediction(const NodeData& movie_data, const NodeData& user_data) {
  double pred = galois::innerProduct(movie_data.latent_vector,
                                     user_data.latent_vector, 0.0);
  double p    = pred;

  pred = std::min(MAXVAL, pred);
  pred = std::max(MINVAL, pred);

#ifndef NDEBUG
  if (p != pred)
    std::cerr << "clamped " << p << " to " << pred << "\n";
#endif

  return pred;
}

/******************************************************************************/
/* Algorithm structures */
/******************************************************************************/

struct InitializeGraph {
  Graph* graph;

  InitializeGraph(Graph* _graph) : graph(_graph) {}

  void static go(Graph& _graph) {
    auto& allNodes = _graph.allNodesRange();

#ifdef __GALOIS_HET_CUDA__
    if (personality == GPU_CUDA) {
      std::string impl_str(_graph.get_run_identifier("InitializeGraph"));
      galois::StatTimer StatTimer_cuda(impl_str.c_str());
      StatTimer_cuda.start();
      InitializeGraph_cuda(*allNodes.begin(), *allNodes.end(), cuda_ctx);
      StatTimer_cuda.stop();
    } else if (personality == CPU)
#endif
      galois::do_all(galois::iterate(allNodes.begin(), allNodes.end()),
                     InitializeGraph{&_graph},
                     galois::loopname("InitializeGraph"));

    // due to latent_vector being generated randomly, it should be sync'd
    // to 1 consistent version across all hosts
    _graph.sync<writeSource, readAny, Reduce_set_latent_vector,
                Broadcast_latent_vector>("InitializeGraph");
  }

  void operator()(GNode src) const {
    NodeData& sdata = graph->getData(src);

    // resize vectors
    sdata.latent_vector.resize(LATENT_VECTOR_SIZE);
    sdata.residual_latent_vector.resize(LATENT_VECTOR_SIZE);

    for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
      sdata.latent_vector[i] = genVal(src); // randomly create latent vector
      sdata.residual_latent_vector[i] = 0;  // randomly create latent vector

#ifndef NDEBUG
      if (!std::isnormal(sdata.latent_vector[i]))
        galois::gDebug("GEN for ", i, " ", sdata.latent_vector[i]);
#endif
    }
  }
};

struct setMasterBitset {
  Graph* graph;

  setMasterBitset(Graph* _graph) : graph(_graph) {}

  void static go(Graph& _graph) {
    galois::do_all(galois::iterate(_graph.masterNodesRange().begin(),
                                   _graph.masterNodesRange().end()),
                   setMasterBitset{&_graph},
                   galois::loopname("InitializeGraph_crashed_setMasterBiset"));
  }

  void operator()(GNode src) const { bitset_latent_vector.set(src); }
};

struct InitializeGraph_crashed {
  Graph* graph;

  InitializeGraph_crashed(Graph* _graph) : graph(_graph) {}

  void static go(Graph& _graph) {

    setMasterBitset::go(_graph);

    auto& allNodes = _graph.allNodesRange();

#ifdef __GALOIS_HET_CUDA__
    if (personality == GPU_CUDA) {
      std::string impl_str(_graph.get_run_identifier("InitializeGraph"));
      galois::StatTimer StatTimer_cuda(impl_str.c_str());
      StatTimer_cuda.start();
      InitializeGraph_cuda(*allNodes.begin(), *allNodes.end(), cuda_ctx);
      StatTimer_cuda.stop();
    } else if (personality == CPU)
#endif
      galois::do_all(galois::iterate(allNodes.begin(), allNodes.end()),
                     InitializeGraph_crashed{&_graph},
                     galois::loopname("InitializeGraph_crashed"));

    _graph.sync<writeAny, readAny, Reduce_set_latent_vector,
                Broadcast_latent_vector, Bitset_latent_vector>(
        "InitializeGraph_crashed");
  }

  void operator()(GNode src) const {
    NodeData& sdata = graph->getData(src);

    // resize vectors
    sdata.latent_vector.resize(LATENT_VECTOR_SIZE);
    sdata.residual_latent_vector.resize(LATENT_VECTOR_SIZE);

    for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
      sdata.latent_vector[i] = genVal(src); // randomly create latent vector
      sdata.residual_latent_vector[i] = 0;  // randomly create latent vector

#ifndef NDEBUG
      if (!std::isnormal(sdata.latent_vector[i]))
        galois::gDebug("GEN for ", i, " ", sdata.latent_vector[i]);
#endif
    }
  }
};

struct InitializeGraph_healthy {
  Graph* graph;

  InitializeGraph_healthy(Graph* _graph) : graph(_graph) {}

  void static go(Graph& _graph) {
    auto& allNodes = _graph.allNodesRange();

#ifdef __GALOIS_HET_CUDA__
    if (personality == GPU_CUDA) {
      std::string impl_str(_graph.get_run_identifier("InitializeGraph"));
      galois::StatTimer StatTimer_cuda(impl_str.c_str());
      StatTimer_cuda.start();
      InitializeGraph_cuda(*allNodes.begin(), *allNodes.end(), cuda_ctx);
      StatTimer_cuda.stop();
    } else if (personality == CPU)
#endif
      galois::do_all(galois::iterate(allNodes.begin(), allNodes.end()),
                     InitializeGraph_healthy{&_graph},
                     galois::loopname("InitializeGraph_healthy"));

    _graph.sync<writeAny, readAny, Reduce_set_latent_vector,
                Broadcast_latent_vector, Bitset_latent_vector>(
        "InitializeGraph_healthy");
  }

  void operator()(GNode src) const {
    NodeData& sdata = graph->getData(src);

    for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
      sdata.residual_latent_vector[i] = 0; // randomly create latent vector
    }
    bitset_latent_vector.set(src);
  }
};

/* Recovery to be called by resilience based fault tolerance
 * It is a NoOp
 */
struct recovery {
  Graph* graph;

  recovery(Graph* _graph) : graph(_graph) {}

  void static go(Graph& _graph) {}
};

struct SGD_mergeResidual {
  Graph* graph;

  SGD_mergeResidual(Graph* _graph) : graph(_graph) {}

  void static go(Graph& _graph) {

    auto& allNodes = _graph.allNodesRange();

#ifdef __GALOIS_HET_CUDA__
    if (personality == GPU_CUDA) {
      std::string impl_str("SGD_" + (_graph.get_run_identifier()));
      galois::StatTimer StatTimer_cuda(impl_str.c_str());
      StatTimer_cuda.start();
      int __retval = 0;
      SGD_all_cuda(__retval, cuda_ctx);
      // DGAccumulator_accum += __retval;
      StatTimer_cuda.stop();
    } else if (personality == CPU)
#endif

      galois::do_all(
          galois::iterate(allNodes.begin(), allNodes.end()),
          SGD_mergeResidual{&_graph},
          galois::loopname(_graph.get_run_identifier("SGD_merge").c_str()),
          galois::steal(), galois::no_stats());
  }

  void operator()(GNode src) const {
    NodeData& sdata              = graph->getData(src);
    auto& latent_vector          = sdata.latent_vector;
    auto& residual_latent_vector = sdata.residual_latent_vector;

    for (int i = 0; i < LATENT_VECTOR_SIZE; ++i) {
      latent_vector[i] += residual_latent_vector[i];
      residual_latent_vector[i] = 0;

#ifndef NDEBUG
      if (!std::isnormal(sdata.latent_vector[i]))
        galois::gDebug("GEN for ", i, " ", sdata.latent_vector[i]);
#endif
    }
  }
};

struct SGD {
  Graph* graph;
  double step_size;
  galois::DGAccumulator<double>& DGAccumulator_accum;

  SGD(Graph* _graph, double _step_size, galois::DGAccumulator<double>& _dga)
      : graph(_graph), step_size(_step_size), DGAccumulator_accum(_dga) {}

  void static go(Graph& _graph, galois::DGAccumulator<double>& dga) {
    unsigned _num_iterations              = 0;
    unsigned _num_iterations_stepSize     = 0;
    unsigned _num_iterations_checkpointed = 0;
    double rms_normalized                 = 0.0;
    double last                           = -1.0;
    double last_checkpointed              = -1.0;
    auto& nodesWithEdges                  = _graph.allNodesWithEdgesRange();
    do {
      // Checkpointing the all the node data
      if (enableFT && recoveryScheme == CP) {
        saveCheckpointToDisk(_num_iterations, _graph);
        // Saving other state variables
        if (_num_iterations % checkpointInterval == 0) {
          last_checkpointed            = last;
          _num_iterations_checkpointed = _num_iterations;
        }
      }

      auto step_size = getstep_size(_num_iterations_stepSize);
      dga.reset();
      galois::do_all(galois::iterate(nodesWithEdges),
                     SGD(&_graph, step_size, dga),
                     galois::loopname(_graph.get_run_identifier("SGD").c_str()),
                     galois::steal(), galois::no_stats());

      _graph.sync<writeDestination, readAny,
                  Reduce_pair_wise_add_array_residual_latent_vector,
                  Broadcast_residual_latent_vector,
                  Bitset_residual_latent_vector>("SGD");

      SGD_mergeResidual::go(_graph);

      /**************************CRASH SITE : start
       * *****************************************/
      if (enableFT && (_num_iterations == crashIteration)) {
        crashSite<recovery, InitializeGraph_crashed, InitializeGraph_healthy>(
            _graph);
        ++_num_iterations;
        if (recoveryScheme == CP) {
          _num_iterations_stepSize = _num_iterations_checkpointed;
          last                     = last_checkpointed;
        }

        continue;
      }
      /**************************CRASH SITE : end
       * *****************************************/

      // calculate root mean squared error
      // Divide by 2 since for symmetric graph it is counted twice
      double error   = dga.reduce() / 2;
      rms_normalized = std::sqrt(error / _graph.globalSizeEdges());

      double error_change = std::abs((last - error) / last);
      if (galois::runtime::getSystemNetworkInterface().ID == 0) {
        galois::gPrint("ITERATION : ", _num_iterations, "\n");
        galois::gDebug("RMS Normalized : ", rms_normalized);
        galois::gPrint("RMS : ", rms_normalized, "\n");
        galois::gPrint("abs(last - error/last) : ", error_change, "\n");
      }

      if (error_change < tolerance) {
        break;
      }
      last = error;
      ++_num_iterations;
      ++_num_iterations_stepSize;
    } while ((_num_iterations < maxIterations));

    if (galois::runtime::getSystemNetworkInterface().ID == 0) {
      galois::runtime::reportStat_Single(
          regionname, "NumIterations_" + std::to_string(_graph.get_run_num()),
          (unsigned long)_num_iterations);
    }
  }

  void operator()(GNode src) const {
    NodeData& sdata           = graph->getData(src);
    auto& movie_node          = sdata.latent_vector;
    auto& residual_movie_node = sdata.residual_latent_vector;

    for (auto jj = graph->edge_begin(src), ej = graph->edge_end(src); jj != ej;
         ++jj) {
      GNode dst   = graph->getEdgeDst(jj);
      auto& ddata = graph->getData(dst);

      auto& user_node          = ddata.latent_vector;
      auto& residual_user_node = ddata.residual_latent_vector;
      // auto& sdata_up = sdata.updates;

      double edge_rating = graph->getEdgeData(jj);

      // doGradientUpdate
      double old_dp = galois::innerProduct(user_node, movie_node, double(0));

      double cur_error = edge_rating - old_dp;
      DGAccumulator_accum += (cur_error * cur_error);

      assert(cur_error < 10000 && cur_error > -10000);

      bool setBit = false;
      // update both vectors based on error derived from 2 previous vectors
      for (int i = 0; i < LATENT_VECTOR_SIZE; ++i) {

        double prevUser  = user_node[i];
        double prevMovie = movie_node[i];

        // Only update the destination
        galois::atomicAdd(
            residual_user_node[i],
            double(step_size * (cur_error * prevMovie - LAMBDA * prevUser)));
        // galois::gPrint("val : ", residual_user_node[i], "\n");
        assert(std::isnormal(residual_user_node[i]));
        if (!setBit && std::abs(residual_user_node[i]) > 0.1)
          setBit = true;

        // galois::atomicAdd(residual_movie_node[i],  double(step_size *
        // (cur_error * prevUser - LAMBDA * prevMovie)));
        // assert(std::isnormal(residual_movie_node[i]));
      }
      if (setBit)
        bitset_residual_latent_vector.set(dst);
    }
  }
};

/******************************************************************************/
/* Main */
/******************************************************************************/
constexpr static const char* const name = "SGD - Distributed Heterogeneous";
constexpr static const char* const desc = "SGD on Distributed Galois.";
constexpr static const char* const url  = 0;

int main(int argc, char** argv) {
  galois::DistMemSys G;
  DistBenchStart(argc, argv, name, desc, url);

  const auto& net = galois::runtime::getSystemNetworkInterface();
  if (net.ID == 0) {
    galois::runtime::reportParam(regionname, "Max Iterations",
                                 (unsigned long)maxIterations);

    galois::runtime::reportParam(regionname, "ENABLE_FT", (enableFT));
  }

  galois::StatTimer StatTimer_total("TimerTotal", regionname);

  StatTimer_total.start();
#ifdef __GALOIS_HET_CUDA__
  Graph* hg = distGraphInitialization<NodeData, double>(&cuda_ctx);
#else
  Graph* hg = distGraphInitialization<NodeData, double>();
#endif

  // bitset comm setup
  bitset_latent_vector.resize(hg->size());
  bitset_residual_latent_vector.resize(hg->size());

  galois::gPrint("[", net.ID, "] InitializeGraph::go called\n");

  galois::StatTimer StatTimer_init("TIMER_GRAPH_INIT", regionname);
  StatTimer_init.start();
  InitializeGraph::go((*hg));
  StatTimer_init.stop();

  galois::runtime::getHostBarrier().wait();

  // accumulators for use in operators
  galois::DGAccumulator<double> DGAccumulator_accum;
  // galois::DGAccumulator<uint64_t> DGAccumulator_sum;
  // galois::DGAccumulator<uint32_t> DGAccumulator_max;
  // galois::GReduceMax<uint32_t> m;

  for (auto run = 0; run < numRuns; ++run) {
    galois::gPrint("[", net.ID, "] SGD::go run ", run, " called\n");
    std::string timer_str("Timer_" + std::to_string(run));
    galois::StatTimer StatTimer_main(timer_str.c_str(), regionname);

    StatTimer_main.start();
    SGD::go((*hg), DGAccumulator_accum);
    StatTimer_main.stop();

    if ((run + 1) != numRuns) {
#ifdef __GALOIS_HET_CUDA__
      if (personality == GPU_CUDA) {
        // bitset_dist_current_reset_cuda(cuda_ctx);
      } else
#endif
        bitset_latent_vector.reset();
      bitset_residual_latent_vector.reset();

      (*hg).set_num_run(run + 1);
      InitializeGraph::go((*hg));
      galois::runtime::getHostBarrier().wait();
    }
  }

  StatTimer_total.stop();

  return 0;
}


#include "rawInput.hpp"
#include "ClpBALPInterface.hpp"
#include <boost/scoped_ptr.hpp>
#include <cstdlib>

using boost::scoped_ptr; // replace with unique_ptr for C++11
using namespace std;

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

	MPI_Init(&argc, &argv);

	int mype;
	MPI_Comm_rank(MPI_COMM_WORLD,&mype);

	if (argc < 3) {
		if (mype == 0) printf("Usage: %s [rawdump root name] [num scenarios] [solution output root name] [starting basis root name]\n",argv[0]);
		return 1;
	}

	string datarootname(argv[1]);
	int nscen = atoi(argv[2]);

	scoped_ptr<rawInput> s(new rawInput(datarootname,nscen));

	BAContext ctx(MPI_COMM_WORLD);
	ctx.initializeAssignment(nscen);

	ClpBALPInterface solver(*s, ctx, ClpBALPInterface::useDual);
	s.reset(0); // free memory
	if (argc == 5) {
		solver.loadStatus(argv[4]);
	}
	//solver.setDumpFrequency(5000,argv[3]);
	solver.setPrimalTolerance(1e-6);
	solver.setDualTolerance(1e-6);
	solver.go();

	if (argc >= 4 && argv[3][0] != '-') {
		if (mype == 0) printf("Writing solution\n");
		solver.writeStatus(argv[3]);
		if (mype == 0) printf("Finished writing solution\n");
	}

	MPI_Finalize();

	return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Regression

#include <fstream>
#include <iostream>

#include <boost/test/unit_test.hpp>

#include <xolotl/core/diffusion/Diffusion1DHandler.h>
#include <xolotl/core/network/PSIReactionNetwork.h>
#include <xolotl/options/Options.h>
#include <xolotl/test/CommandLine.h>
#include <xolotl/util/MPIUtils.h>

using namespace std;
using namespace xolotl;
using namespace core;
using namespace diffusion;

using Kokkos::ScopeGuard;
BOOST_GLOBAL_FIXTURE(ScopeGuard);

/**
 * This suite is responsible for testing the Diffusion1DHandler.
 */
BOOST_AUTO_TEST_SUITE(Diffusion1DHandler_testSuite)

/**
 * Method checking the initialization of the off-diagonal part of the Jacobian,
 * and the compute diffusion methods.
 */
BOOST_AUTO_TEST_CASE(checkDiffusion)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=8 0 0 1 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	util::mpiInit(cl.argc, cl.argv);
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	std::vector<double> temperatures;
	for (int l = 0; l < 5; l++) {
		grid.push_back((double)l);
		temperatures.push_back(1000.0);
	}

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create the diffusion handler
	Diffusion1DHandler diffusionHandler(opts.getMigrationThreshold());

	// Create a collection of advection handlers
	std::vector<advection::IAdvectionHandler*> advectionHandlers;

	// Create ofill
	network::IReactionNetwork::SparseFillMap ofill;

	// Initialize it
	diffusionHandler.initializeOFill(network, ofill);
	diffusionHandler.initializeDiffusionGrid(advectionHandlers, grid, 5, 0);

	// Test which cluster diffuses
	BOOST_REQUIRE_EQUAL(ofill[1][0], 1); // He_1
	BOOST_REQUIRE_EQUAL(ofill[3][0], 3); // He_2
	BOOST_REQUIRE_EQUAL(ofill[5][0], 5); // He_3
	BOOST_REQUIRE_EQUAL(ofill[7][0], 7); // He_4
	BOOST_REQUIRE_EQUAL(ofill[9][0], 9); // He_5
	BOOST_REQUIRE_EQUAL(ofill[11][0], 11); // He_6
	BOOST_REQUIRE_EQUAL(ofill[13][0], 13); // He_7
	BOOST_REQUIRE_EQUAL(ofill[0][0], 0); // V_1

	// Check the total number of diffusing clusters
	BOOST_REQUIRE_EQUAL(diffusionHandler.getNumberOfDiffusing(), 8);

	// The size parameter in the x direction
	double hx = 1.0;

	// The arrays of concentration
	double concentration[3 * dof];
	double newConcentration[3 * dof];

	// Initialize their values
	for (int i = 0; i < 3 * dof; i++) {
		concentration[i] = (double)i * i;
		newConcentration[i] = 0.0;
	}

	// Set the temperature to 1000K to initialize the diffusion coefficients
	network.setTemperatures(temperatures);
	network.syncClusterDataOnHost();

	// Get pointers
	double* conc = &concentration[0];
	double* updatedConc = &newConcentration[0];

	// Get the offset for the grid point in the middle
	// Supposing the 3 grid points are laid-out as follow:
	// 0 | 1 | 2
	double* concOffset = conc + dof;
	double* updatedConcOffset = updatedConc + dof;

	// Fill the concVector with the pointer to the middle, left, and right grid
	// points
	double* concVector[3]{};
	concVector[0] = concOffset; // middle
	concVector[1] = conc; // left
	concVector[2] = conc + 2 * dof; // right

	// Compute the diffusion at this grid point
	diffusionHandler.computeDiffusion(
		network, concVector, updatedConcOffset, hx, hx, 0);

	// Check the new values of updatedConcOffset
	BOOST_REQUIRE_CLOSE(updatedConcOffset[1], 3.7081e+12, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[3], 1.8160e+12, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[5], 7.3065e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[7], 9.6476e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[9], 7.1800e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[11], 1.7783e+10, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[13], 2.7860e+09, 0.01);
	BOOST_REQUIRE_CLOSE(
		updatedConcOffset[15], 0.0, 0.01); // He_8 does not diffuse
	BOOST_REQUIRE_CLOSE(updatedConcOffset[0], 2.9207e+08, 0.01);

	// Initialize the indices and values to set in the Jacobian
	int nDiff = diffusionHandler.getNumberOfDiffusing();
	IdType indices[nDiff];
	double val[3 * nDiff];
	// Get the pointer on them for the compute diffusion method
	IdType* indicesPointer = &indices[0];
	double* valPointer = &val[0];

	// Compute the partial derivatives for the diffusion a the grid point 1
	diffusionHandler.computePartialsForDiffusion(
		network, valPointer, indicesPointer, hx, hx, 0);

	// Check the values for the indices
	BOOST_REQUIRE_EQUAL(indices[0], 0);
	BOOST_REQUIRE_EQUAL(indices[1], 1);
	BOOST_REQUIRE_EQUAL(indices[2], 3);
	BOOST_REQUIRE_EQUAL(indices[3], 5);
	BOOST_REQUIRE_EQUAL(indices[4], 7);
	BOOST_REQUIRE_EQUAL(indices[5], 9);
	BOOST_REQUIRE_EQUAL(indices[6], 11);
	BOOST_REQUIRE_EQUAL(indices[7], 13);

	// Check some values
	BOOST_REQUIRE_CLOSE(val[1], 505312, 0.01);
	BOOST_REQUIRE_CLOSE(val[4], 6415444736, 0.01);
	BOOST_REQUIRE_CLOSE(val[5], 6415444736, 0.01);
	BOOST_REQUIRE_CLOSE(val[6], -6283827232, 0.01);
	BOOST_REQUIRE_CLOSE(val[9], -2528210084, 0.01);

	// Finalize MPI
	MPI_Finalize();
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/integer_traits.hpp>