// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015, 2019.
// Modifications copyright (c) 2015-2019, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP

#include <limits>

#include <boost/core/ignore_unused.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/make_unsigned.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>
#include <boost/geometry/util/math.hpp>

#ifdef BOOST_GEOMETRY_SIDE_OF_INTERSECTION_DEBUG
#include <boost/math/common_factor_ct.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

namespace detail
{

// A tool for multiplication of integers avoiding overflow
// It's a temporary workaround until we can use Multiprecision
// The algorithm is based on Karatsuba algorithm
// see: http://en.wikipedia.org/wiki/Karatsuba_algorithm
template <typename T>
struct multiplicable_integral
{
    // Currently this tool can't be used with non-integral coordinate types.
    // Also side_of_intersection strategy sign_of_product() and sign_of_compare()
    // functions would have to be modified to properly support floating-point
    // types (comparisons and multiplication).
    BOOST_STATIC_ASSERT(boost::is_integral<T>::value);

    static const std::size_t bits = CHAR_BIT * sizeof(T);
    static const std::size_t half_bits = bits / 2;
    typedef typename boost::make_unsigned<T>::type unsigned_type;
    static const unsigned_type base = unsigned_type(1) << half_bits; // 2^half_bits

    int m_sign;
    unsigned_type m_ms;
    unsigned_type m_ls;

    multiplicable_integral(int sign, unsigned_type ms, unsigned_type ls)
        : m_sign(sign), m_ms(ms), m_ls(ls)
    {}

    explicit multiplicable_integral(T const& val)
    {
        unsigned_type val_u = val > 0 ?
                                unsigned_type(val)
                              : val == (std::numeric_limits<T>::min)() ?
                                  unsigned_type((std::numeric_limits<T>::max)()) + 1
                                : unsigned_type(-val);
        // MMLL -> S 00MM 00LL
        m_sign = math::sign(val);
        m_ms = val_u >> half_bits; // val_u / base
        m_ls = val_u - m_ms * base;
    }

    friend multiplicable_integral operator*(multiplicable_integral const& a,
                                            multiplicable_integral const& b)
    {
        // (S 00MM 00LL) * (S 00MM 00LL) -> (S Z2MM 00LL)
        unsigned_type z2 = a.m_ms * b.m_ms;
        unsigned_type z0 = a.m_ls * b.m_ls;
        unsigned_type z1 = (a.m_ms + a.m_ls) * (b.m_ms + b.m_ls) - z2 - z0;
        // z0 may be >= base so it must be normalized to allow comparison
        unsigned_type z0_ms = z0 >> half_bits; // z0 / base
        return multiplicable_integral(a.m_sign * b.m_sign,
                                      z2 * base + z1 + z0_ms,
                                      z0 - base * z0_ms);
    }

    friend bool operator<(multiplicable_integral const& a,
                          multiplicable_integral const& b)
    {
        if ( a.m_sign == b.m_sign )
        {
            bool u_less = a.m_ms < b.m_ms
                      || (a.m_ms == b.m_ms && a.m_ls < b.m_ls);
            return a.m_sign > 0 ? u_less : (! u_less);
        }
        else
        {
            return a.m_sign < b.m_sign;
        }
    }

    friend bool operator>(multiplicable_integral const& a,
                          multiplicable_integral const& b)
    {
        return b < a;
    }

#ifdef BOOST_GEOMETRY_SIDE_OF_INTERSECTION_DEBUG
    template <typename CmpVal>
    void check_value(CmpVal const& cmp_val) const
    {
        unsigned_type b = base; // a workaround for MinGW - undefined reference base
        CmpVal val = CmpVal(m_sign) * (CmpVal(m_ms) * CmpVal(b) + CmpVal(m_ls));
        BOOST_GEOMETRY_ASSERT(cmp_val == val);
    }
#endif // BOOST_GEOMETRY_SIDE_OF_INTERSECTION_DEBUG
};

} // namespace detail

// Calculates the side of the intersection-point (if any) of
// of segment a//b w.r.t. segment c
// This is calculated without (re)calculating the IP itself again and fully
// based on integer mathematics; there are no divisions
// It can be used for either integer (rescaled) points, and also for FP
class side_of_intersection
{
private :
    template <typename T, typename U>
    static inline
    int sign_of_product(T const& a, U const& b)
    {
        return a == 0 || b == 0 ? 0
            : a > 0 && b > 0 ? 1
            : a < 0 && b < 0 ? 1
            : -1;
    }

    template <typename T>
    static inline
    int sign_of_compare(T const& a, T const& b, T const& c, T const& d)
    {
        // Both a*b and c*d are positive
        // We have to judge if a*b > c*d

        using side::detail::multiplicable_integral;
        multiplicable_integral<T> ab = multiplicable_integral<T>(a)
                                     * multiplicable_integral<T>(b);
        multiplicable_integral<T> cd = multiplicable_integral<T>(c)
                                     * multiplicable_integral<T>(d);

        int result = ab > cd ? 1
                   : ab < cd ? -1
                   : 0
                   ;

#ifdef BOOST_GEOMETRY_SIDE_OF_INTERSECTION_DEBUG
        using namespace boost::multiprecision;
        cpp_int const lab = cpp_int(a) * cpp_int(b);
        cpp_int const lcd = cpp_int(c) * cpp_int(d);

        ab.check_value(lab);
        cd.check_value(lcd);

        int result2 = lab > lcd ? 1
                    : lab < lcd ? -1
                    : 0
                    ;
        BOOST_GEOMETRY_ASSERT(result == result2);
#endif

        return result;
    }

    template <typename T>
    static inline
    int sign_of_addition_of_two_products(T const& a, T const& b, T const& c, T const& d)
    {
        // sign of a*b+c*d, 1 if positive, -1 if negative, else 0
        int const ab = sign_of_product(a, b);
        int const cd = sign_of_product(c, d);
        if (ab == 0)
        {
            return cd;
        }
        if (cd == 0)
        {
            return ab;
        }

        if (ab == cd)
        {
            // Both positive or both negative
            return ab;
        }

        // One is positive, one is negative, both are non zero
        // If ab is positive, we have to judge if a*b > -c*d (then 1 because sum is positive)
        // If ab is negative, we have to judge if c*d > -a*b (idem)
        return ab == 1
            ? sign_of_compare(a, b, -c, d)
            : sign_of_compare(c, d, -a, b);
    }

public :

    // Calculates the side of the intersection-point (if any) of
    // of segment a//b w.r.t. segment c
    // This is calculated without (re)calculating the IP itself again and fully
    // based on integer mathematics
    template <typename T, typename Segment, typename Point>
    static inline T side_value(Segment const& a, Segment const& b,
                Segment const& c, Point const& fallback_point)
    {
        // The first point of the three segments is reused several times
        T const ax = get<0, 0>(a);
        T const ay = get<0, 1>(a);
        T const bx = get<0, 0>(b);
        T const by = get<0, 1>(b);
        T const cx = get<0, 0>(c);
        T const cy = get<0, 1>(c);

        T const dx_a = get<1, 0>(a) - ax;
        T const dy_a = get<1, 1>(a) - ay;

        T const dx_b = get<1, 0>(b) - bx;
        T const dy_b = get<1, 1>(b) - by;

        T const dx_c = get<1, 0>(c) - cx;
        T const dy_c = get<1, 1>(c) - cy;

        // Cramer's rule: d (see cart_intersect.hpp)
        T const d = geometry::detail::determinant<T>
                    (
                        dx_a, dy_a,
                        dx_b, dy_b
                    );

        T const zero = T();
        if (d == zero)
        {
            // There is no IP of a//b, they are collinear or parallel
            // Assuming they intersect (this method should be called for
            // segments known to intersect), they are collinear and overlap.
            // They have one or two intersection points - we don't know and
            // have to rely on the fallback intersection point

            Point c1, c2;
            geometry::detail::assign_point_from_index<0>(c, c1);
            geometry::detail::assign_point_from_index<1>(c, c2);
            return side_by_triangle<>::apply(c1, c2, fallback_point);
        }

        // Cramer's rule: da (see cart_intersect.hpp)
        T const da = geometry::detail::determinant<T>
                    (
                        dx_b,    dy_b,
                        ax - bx, ay - by
                    );

        // IP is at (ax + (da/d) * dx_a, ay + (da/d) * dy_a)
        // Side of IP is w.r.t. c is: determinant(dx_c, dy_c, ipx-cx, ipy-cy)
        // We replace ipx by expression above and multiply each term by d

#ifdef BOOST_GEOMETRY_SIDE_OF_INTERSECTION_DEBUG
        T const result1 = geometry::detail::determinant<T>
                    (
                        dx_c * d,                   dy_c * d,
                        d * (ax - cx) + dx_a * da,  d * (ay - cy) + dy_a * da
                    );

        // Note: result / (d * d)
        // is identical to the side_value of side_by_triangle
        // Therefore, the sign is always the same as that result, and the
        // resulting side (left,right,collinear) is the same

        // The first row we divide again by d because of determinant multiply rule
        T const result2 = d * geometry::detail::determinant<T>
                    (
                        dx_c,                   dy_c,
                        d * (ax - cx) + dx_a * da,  d * (ay - cy) + dy_a * da
                    );
        // Write out:
        T const result3 = d * (dx_c * (d * (ay - cy) + dy_a * da)
                             - dy_c * (d * (ax - cx) + dx_a * da));
        // Write out in braces:
        T const result4 = d * (dx_c * d * (ay - cy) + dx_c * dy_a * da
                             - dy_c * d * (ax - cx) - dy_c * dx_a * da);
        // Write in terms of d * XX + da * YY
        T const result5 = d * (d * (dx_c * (ay - cy) - dy_c * (ax - cx))
                             + da * (dx_c * dy_a - dy_c * dx_a));

        boost::ignore_unused(result1, result2, result3, result4, result5);
        //return result;
#endif

        // We consider the results separately
        // (in the end we only have to return the side-value 1,0 or -1)

        // To avoid multiplications we judge the product (easy, avoids *d)
        // and the sign of p*q+r*s (more elaborate)
        T const result = sign_of_product
            (
                d,
                sign_of_addition_of_two_products
                    (
                        d, dx_c * (ay - cy) - dy_c * (ax - cx),
                        da, dx_c * dy_a - dy_c * dx_a
                    )
                );
        return result;

    }

    template <typename Segment, typename Point>
    static inline int apply(Segment const& a, Segment const& b,
            Segment const& c,
            Point const& fallback_point)
    {
        typedef typename geometry::coordinate_type<Segment>::type coordinate_type;
        coordinate_type const s = side_value<coordinate_type>(a, b, c, fallback_point);
        coordinate_type const zero = coordinate_type();
        return math::equals(s, zero) ? 0
            : s > zero ? 1
            : -1;
    }

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP


#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 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);
}


#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 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;
}


#define BIORBD_API_EXPORTS
#include "RigidBody/Integrator.h"

#include <Eigen/Dense>
#include <boost/numeric/odeint.hpp>
#include <rbdl/Dynamics.h>
#include "Utils/Error.h"
#include "Utils/String.h"
#include "RigidBody/GeneralizedCoordinates.h"
#include "RigidBody/Joints.h"

biorbd::rigidbody::Integrator::Integrator() :
    m_nbre(std::shared_ptr<unsigned int>()),
    m_steps(std::shared_ptr<unsigned int>()),
    m_model(std::shared_ptr<RigidBodyDynamics::Model>()),
    m_x_vec(std::shared_ptr<std::vector<state_type>>()),
    m_times(std::shared_ptr<std::vector<double>>()),
    m_u(std::shared_ptr<biorbd::utils::Vector>())

{

}

biorbd::rigidbody::Integrator biorbd::rigidbody::Integrator::DeepCopy() const
{
    biorbd::rigidbody::Integrator copy;
    copy.DeepCopy(*this);
    return copy;
}

void biorbd::rigidbody::Integrator::DeepCopy(const biorbd::rigidbody::Integrator &other)
{
    *m_nbre = *other.m_nbre;
    *m_steps = *other.m_steps;
    *m_model = *other.m_model;
    m_x_vec->resize(other.m_x_vec->size());
    for (unsigned int i=0; i<other.m_x_vec->size(); ++i)
        (*m_x_vec)[i] = (*other.m_x_vec)[i];
    m_times->resize(other.m_times->size());
    for (unsigned int i=0; i<other.m_times->size(); ++i)
        (*m_times)[i] = (*other.m_times)[i];
    *m_u = *other.m_u;
}

void biorbd::rigidbody::Integrator::operator() (
        const state_type &x ,
        state_type &dxdt ,
        double ){
    // Équation différentielle : x/xdot => xdot/xddot
    biorbd::rigidbody::GeneralizedCoordinates Q(*m_nbre);
    biorbd::rigidbody::GeneralizedCoordinates QDot(*m_nbre);
    biorbd::rigidbody::GeneralizedCoordinates QDDot(biorbd::utils::Vector(*m_nbre).setZero());
    for (unsigned int i=0; i<*m_nbre; i++){
        Q(i) = x[i];
        QDot(i) = x[i+*m_nbre];
    }

    RigidBodyDynamics::ForwardDynamics (*m_model, Q, QDot, *m_u, QDDot);

    // Faire sortir xdot/xddot
    for (unsigned int i=0; i<*m_nbre; i++){
        dxdt[i] = QDot[i];
        dxdt[i + *m_nbre] = QDDot[i];
    }

}

void biorbd::rigidbody::Integrator::showAll(){
    std::cout << "Test:" << std::endl;
    for (unsigned int i=0; i<=*m_steps; i++){
        std::cout << (*m_times)[i];
        for (unsigned int j=0; j<*m_nbre; j++)
            std::cout << " " << (*m_x_vec)[i][j];
        std::cout << std::endl;
    }
}

unsigned int biorbd::rigidbody::Integrator::steps() const
{
    return *m_steps+1;
}

biorbd::utils::Vector biorbd::rigidbody::Integrator::getX(
        unsigned int idx){
    biorbd::utils::Vector out(*m_nbre*2);
    biorbd::utils::Error::check(idx <= *m_steps, "Trying to get Q outside range");
    for (unsigned int i=0; i<*m_nbre*2; i++){
        out(i) = (*m_x_vec)[idx][i];
        }
    return out;
}

void biorbd::rigidbody::Integrator::integrate(
        biorbd::rigidbody::Joints& model,
        const biorbd::utils::Vector &Q_Qdot,
        const biorbd::utils::Vector &u,
        double t0,
        double tend,
        double timeStep){
    // Stocker le nombre d'élément à traiter
    *m_nbre = static_cast<unsigned int>(Q_Qdot.rows())/2; // Q et Qdot
    *m_u = u; // Copier les effecteurs
    *m_model = model;

    // Remplissage de la variable par les positions et vitesse
    state_type x(*m_nbre*2);
    for (unsigned int i=0; i<*m_nbre*2; i++)
        x[i] = Q_Qdot(i);

    // Choix de l'algorithme et intégration
    boost::numeric::odeint::runge_kutta4< state_type > stepper;
    *m_steps = static_cast<unsigned int>(boost::numeric::odeint::integrate_const( stepper, (*this), x, t0, tend, timeStep, push_back_state_and_time( *m_x_vec , *m_times )));
}


#pragma once
#include "arpackdef.h"

#include <Eigen/Dense>
#include <random>
#include <vector>

extern "C"
{
    void dsaupd_(a_int*, const char*, a_int*, const char*, a_int*, double*, double*, a_int*,
                 double*, a_int*, a_int*, a_int*, double*, double*, a_int*, a_int*);

    void dseupd_(a_int* rvec, const char* howmny, a_int* select, double* d, double* z, a_int* ldz,
                 double* sigma, const char* bmat, a_int* n, const char* which, a_int* nev,
                 double* tol, double* resid, a_int* ncv, double* v, a_int* ldv, a_int* iparam,
                 a_int* inptr, double* workd, double* workl, a_int* lworkl, a_int* info);
}

namespace edlib
{
enum class ErrorType
{
    NormalExit,
    NotConverged,
    IncorrectParams,
    Others
};

template<typename MatrixVectorOp> class ArpackSolver
{
private:
    MatrixVectorOp& op_;
    const a_int dim_;
    std::vector<double> d_;
    std::vector<double> z_;

public:
    explicit ArpackSolver(MatrixVectorOp& op) : op_{op}, dim_{static_cast<a_int>(op.rows())}
    {
        assert(op.rows() == op.cols());
    }

    ErrorType solve(a_int nev, uint32_t max_iter = 1024, double tol = 1e-10)
    {
        a_int dim = dim_;
        if(dim <= 0 || nev <= 0)
        {
            return ErrorType::IncorrectParams;
        }

        a_int ido = 0;
        const char* bmat = "I";
        a_int n = dim;
        const char* which = "SA";
        std::vector<double> resid(dim);

        a_int ncv = 3 * nev;
        std::vector<double> v(static_cast<size_t>(ncv * dim));

        a_int ldv = dim;
        std::array<a_int, 11> iparam = {
            1, // ishift
            0, // levec (not used)
            static_cast<a_int>(max_iter), // maxiter
            1, // nb
            0, // nconv
            0, // iupd (not used)
            1, // mode (1 is usual eigenvalue problem)
            0, // np
            0,
            0,
            0 // only for output
        };

        std::array<a_int, 14> ipntr{};
        std::vector<double> workd(3 * static_cast<size_t>(dim), 0.);

        int lworkl = 3 * ncv * ncv + 6 * ncv;
        std::vector<double> workl(lworkl, 0);

        a_int info = 0;

        // first call
        dsaupd_(&ido, bmat, &n, which, &nev, &tol, resid.data(), &ncv, v.data(), &ldv,
                iparam.data(), ipntr.data(), workd.data(), workl.data(), &lworkl, &info);

        while(ido == -1 || ido == 1)
        {
            op_.perform_op(workd.data() + ipntr[0] - 1, workd.data() + ipntr[1] - 1);

            dsaupd_(&ido, bmat, &n, which, &nev, &tol, resid.data(), &ncv, v.data(), &ldv,
                    iparam.data(), ipntr.data(), workd.data(), workl.data(), &lworkl, &info);
        }

        if(info == 1 || iparam[4] != nev)
        {
            return ErrorType::NotConverged;
        }
        else if(info != 0)
        {
            return ErrorType::IncorrectParams;
        }

        a_int rvec = 1;

        d_.resize(nev + 1);
        z_.resize(static_cast<size_t>(dim + 1) * static_cast<size_t>(nev + 1));
        a_int ldz = dim + 1;
        double sigma = 0.0;

        std::vector<a_int> select(ncv);
        std::fill(select.begin(), select.end(), 1);

        const char* howmny = "All";
        dseupd_(&rvec, howmny, select.data(), d_.data(), z_.data(), &ldz, &sigma, bmat, &n, which,
                &nev, &tol, resid.data(), &ncv, v.data(), &ldv, iparam.data(), ipntr.data(),
                workd.data(), workl.data(), &lworkl, &info);

        return ErrorType::NormalExit;
    }

    [[nodiscard]] auto eigenvalues() const -> Eigen::Map<const Eigen::VectorXd>
    {
        return {d_.data(), static_cast<Eigen::Index>(d_.size() - 1)};
    }

    [[nodiscard]] auto eigenvectors() const
        -> Eigen::Map<const Eigen::MatrixXd, 0, Eigen::OuterStride<Eigen::Dynamic>>
    {
        return {z_.data(), dim_, static_cast<Eigen::Index>(d_.size() - 1), {dim_ + 1}};
    }
};
} // namespace edlib


#ifndef GUNEROTRANSACTIONRECEIVECIRCUIT_H_
#define GUNEROTRANSACTIONRECEIVECIRCUIT_H_

#include <deque>
#include <boost/optional.hpp>
#include <boost/static_assert.hpp>
#include <libff/common/utils.hpp>
#include <libsnark/common/default_types/r1cs_gg_ppzksnark_pp.hpp>
#include <libsnark/common/default_types/r1cs_gg_ppzksnark_pp.hpp>
#include <libsnark/relations/constraint_satisfaction_problems/r1cs/examples/r1cs_examples.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_gg_ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/uscs_ppzksnark/uscs_ppzksnark.hpp>
#include <libff/algebra/fields/field_utils.hpp>
#include <libff/algebra/scalar_multiplication/multiexp.hpp>
#include <libsnark/common/default_types/r1cs_ppzksnark_pp.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libff/algebra/curves/edwards/edwards_pp.hpp>
#include <libff/algebra/curves/mnt/mnt4/mnt4_pp.hpp>
#include <libff/algebra/curves/mnt/mnt6/mnt6_pp.hpp>
#include <libff/common/utils.hpp>

#include <libsnark/gadgetlib1/gadgets/merkle_tree/merkle_tree_check_read_gadget.hpp>
#include <libsnark/gadgetlib1/gadgets/merkle_tree/merkle_tree_check_update_gadget.hpp>

#include "gunerotransactionreceive_gadget.hpp"
#include "GuneroProof.hpp"

using namespace libsnark;

namespace gunero {

class GuneroTransactionReceiveWitness{
public:
    uint256 W;
    uint256 T;
    uint256 V_S;
    uint256 V_R;
    uint256 L;

    GuneroTransactionReceiveWitness() {}
    GuneroTransactionReceiveWitness(
        const uint256& pW,
        const uint256& pT,
        const uint256& pV_S,
        const uint256& pV_R,
        const uint256& pL
    ) : W(pW),
        T(pT),
        V_S(pV_S),
        V_R(pV_R),
        L(pL)
    {
    }
    ~GuneroTransactionReceiveWitness() {}

    ADD_SERIALIZE_METHODS;

    template <typename Stream, typename Operation>
    inline void SerializationOp(Stream& s, Operation ser_action, int nType, int nVersion) {
        READWRITE(W);
        READWRITE(T);
        READWRITE(V_S);
        READWRITE(V_R);
        READWRITE(L);
    }

    friend std::ostream& operator<<(std::ostream &out, const GuneroTransactionReceiveWitness &witness)
    {
        ::Serialize(out, witness, 1, 1);

        return out;
    }

    friend std::istream& operator>>(std::istream &in, GuneroTransactionReceiveWitness &witness)
    {
        ::Unserialize(in, witness, 1, 1);

        return in;
    }
};

///// TRANSACTION RECEIVE PROOF /////
// Public Parameters:
// Authorization Root Hash (W)
// Token UID (T)
// Sender Account View Hash (V_S)
// Receiver Account View Hash (V_R)
// Current Transaction Hash (L)

// Private Parameters:
// Receiver Account Secret Key (s_R)
// Receiver Account View Randomizer (r_R)
// Sender Account Address (A_S)
// Sender Account View Randomizer (r_S)
// Firearm Serial Number (F)
// Firearm View Randomizer (j)
// alt: Receiver Account (A_R)
// alt: Sender Proof Public Key (P_proof_S)

//1) Obtain A_R from s_R through EDCSA operations
//1 alt) Obtain P_proof_R from s_R through PRF operations
//2) Validate V_S == hash(A_S, hash(W, r_S)) (View Hash is consistent for Sender)
//2 alt) Validate V_S == hash(P_proof_S, hash(W, r_S)) (View Hash is consistent for Sender)
//3) Validate V_R == hash(A_R, hash(W, r_R) (View Hash is consistent for Receiver)
//3 alt) Validate V_R == hash(P_proof_R, hash(W, r_R)) (View Hash is consistent for Receiver)
//4) Validate T == hash(F, j) (Both parties know the serial number)
//5) Validate L == hash(A_S, hash(s_R, hash(T, W)) (The send proof is consistent, not forged)
template<typename FieldT, typename BaseT, typename HashT>
class GuneroTransactionReceiveCircuit
{
public:
    GuneroTransactionReceiveCircuit()
    {}
    ~GuneroTransactionReceiveCircuit() {}

    void generate(
        const std::string& r1csPath,
        const std::string& pkPath,
        const std::string& vkPath
    ) {
        protoboard<FieldT> pb;
        gunerotransactionreceive_gadget<FieldT, BaseT, HashT> gunero(pb);

        gunero.generate_r1cs_constraints(r1csPath, pkPath, vkPath);
    }

    bool prove(
        const uint256& pW,
        const uint256& pT,
        const uint256& pV_S,
        const uint256& pV_R,
        const uint256& pL,
        const uint252& ps_R,
        const uint256& pr_R,
        const uint160& pA_S,
        const uint256& pr_S,
        const uint256& pF,
        const uint256& pj,
        const uint160& pA_R,
        const uint256& pP_proof_S,
        const r1cs_ppzksnark_proving_key<BaseT>& pk,
        const r1cs_ppzksnark_verification_key<BaseT>& vk,
        GuneroProof& proof
    )
    {
#ifdef DEBUG
        libff::print_header("Gunero witness (proof)");
#endif

        {
            r1cs_primary_input<FieldT> primary_input;
            r1cs_auxiliary_input<FieldT> aux_input;
            {
                protoboard<FieldT> pb;
                {
#ifdef DEBUG
                    libff::print_header("Gunero gunerotransactionreceive_gadget.load_r1cs_constraints()");
#endif

                    gunerotransactionreceive_gadget<FieldT, BaseT, HashT> gunero(pb);

                    gunero.generate_r1cs_witness(
                        pW,
                        pT,
                        pV_S,
                        pV_R,
                        pL,
                        ps_R,
                        pr_R,
                        pA_S,
                        pr_S,
                        pF,
                        pj,
                        pA_R,
                        pP_proof_S
                    );

#ifdef DEBUG
                    printf("\n"); libff::print_indent(); libff::print_mem("after gunerotransactionreceive_gadget.load_r1cs_constraints()"); libff::print_time("after gunerotransactionreceive_gadget.load_r1cs_constraints()");
#endif
                }

                // The constraint system must be satisfied or there is an unimplemented
                // or incorrect sanity check above. Or the constraint system is broken!
                assert(pb.is_satisfied());

                // TODO: These are copies, which is not strictly necessary.
                primary_input = pb.primary_input();
                aux_input = pb.auxiliary_input();

                // Swap A and B if it's beneficial (less arithmetic in G2)
                // In our circuit, we already know that it's beneficial
                // to swap, but it takes so little time to perform this
                // estimate that it doesn't matter if we check every time.
                // pb.constraint_system.swap_AB_if_beneficial();

                //Test witness_map()
                {
                    r1cs_primary_input<FieldT> primary_input_test = gunerotransactionreceive_gadget<FieldT, BaseT, HashT>::witness_map(
                        pW,
                        pT,
                        pV_S,
                        pV_R,
                        pL
                    );
                    assert(primary_input == primary_input_test);
                }
            }

            r1cs_ppzksnark_proof<BaseT> r1cs_proof = r1cs_ppzksnark_prover<BaseT>(
                pk,
                primary_input,
                aux_input
            );

            proof = GuneroProof(r1cs_proof);

#ifdef DEBUG
            printf("\n"); libff::print_indent(); libff::print_mem("after witness (proof)"); libff::print_time("after witness (proof)");
#endif
        }

        //Verify
        {
            r1cs_primary_input<FieldT> primary_input = gunerotransactionreceive_gadget<FieldT, BaseT, HashT>::witness_map(
                pW,
                pT,
                pV_S,
                pV_R,
                pL
            );

            return r1cs_ppzksnark_verifier_strong_IC<BaseT>(vk, primary_input, proof.to_libsnark_proof<r1cs_ppzksnark_proof<BaseT>>());
        }
    }

    bool verify(
        const uint256& pW,
        const uint256& pT,
        const uint256& pV_S,
        const uint256& pV_R,
        const uint256& pL,
        const GuneroProof& proof,
        const r1cs_ppzksnark_verification_key<BaseT>& vk,
        const r1cs_ppzksnark_processed_verification_key<BaseT>& vk_precomp
        )
    {
        try
        {
            r1cs_primary_input<FieldT> primary_input = gunerotransactionreceive_gadget<FieldT, BaseT, HashT>::witness_map(
                pW,
                pT,
                pV_S,
                pV_R,
                pL
            );

            r1cs_ppzksnark_proof<BaseT> r1cs_proof = proof.to_libsnark_proof<r1cs_ppzksnark_proof<BaseT>>();

            ProofVerifier<BaseT> verifierEnabled = ProofVerifier<BaseT>::Strict();

            bool verified = verifierEnabled.check(
                vk,
                vk_precomp,
                primary_input,
                r1cs_proof
            );

#ifdef DEBUG
            printf("\n"); libff::print_indent(); libff::print_mem("after verify"); libff::print_time("after verify");
#endif

            if (verified)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        catch (...)
        {
            return false;
        }
    }
};

} // end namespace `gunero`

#endif /* GUNEROTRANSACTIONRECEIVECIRCUIT_H_ */


#pragma once

#include <crest/geometry/indexed_mesh.hpp>
#include <crest/geometry/patch.hpp>
#include <crest/geometry/mesh_algorithms.hpp>

#include <boost/range/iterator_range.hpp>

#include <cassert>

namespace crest
{
    namespace detail
    {
        template <typename Index>
        class DescendantMap
        {
        public:
            typedef typename std::vector<Index>::const_iterator          ConstIndexIterator;
            typedef typename boost::iterator_range<ConstIndexIterator>   ConstIndexIteratorRange;

            template <typename Scalar>
            explicit DescendantMap(const IndexedMesh<Scalar, Index> & coarse,
                                   const IndexedMesh<Scalar, Index> & fine)
            {
                _map = std::vector<std::vector<Index>>(coarse.num_elements());
                for (Index k = 0; k < fine.num_elements(); ++k)
                {
                    assert(fine.ancestor_for(k) < coarse.num_elements());
                    _map[fine.ancestor_for(k)].push_back(k);
                }
            }

            ConstIndexIteratorRange descendants_for(Index coarse_element) const
            {
                assert(coarse_element >= Index(0) && coarse_element <= static_cast<Index>(_map.size()));
                return boost::make_iterator_range(_map[coarse_element].begin(), _map[coarse_element].end());
            }

        private:
            std::vector<std::vector<Index>> _map;
        };
    }

    /**
     * Represents a triangulated domain by a "coarse" (usually quasi-uniform) mesh and
     * a "fine" mesh that is formed from the coarse mesh by bisection.
     */
    template <typename Scalar, typename Index>
    class BiscaleMesh
    {
    public:
        struct CoarseTag {};
        struct FineTag {};

        typedef Patch<Scalar, Index, CoarseTag> CoarsePatch;
        typedef Patch<Scalar, Index, FineTag>   FinePatch;

        typedef typename detail::DescendantMap<Index>::ConstIndexIteratorRange ConstIndexIteratorRange;

        explicit BiscaleMesh(IndexedMesh<Scalar, Index> coarse_mesh,
                             IndexedMesh<Scalar, Index> fine_mesh);

        CoarsePatch coarse_element_patch(Index coarse_element, unsigned int max_distance) const;
        FinePatch fine_patch_from_coarse(const CoarsePatch & coarse_patch) const;

        Index                       ancestor_for(Index fine_element) const;
        ConstIndexIteratorRange     descendants_for(Index coarse_element) const;

        const IndexedMesh<Scalar, Index> & coarse_mesh() const;
        const IndexedMesh<Scalar, Index> & fine_mesh() const;

    private:
        IndexedMesh<Scalar, Index>      _coarse;
        IndexedMesh<Scalar, Index>      _fine;
        detail::DescendantMap<Index>    _descendants;
    };

    template <typename Scalar, typename Index>
    BiscaleMesh<Scalar, Index>::BiscaleMesh(IndexedMesh<Scalar, Index> coarse_mesh,
                                            IndexedMesh<Scalar, Index> fine_mesh)
            :   _descendants(detail::DescendantMap<Index>(coarse_mesh, fine_mesh))
    {
        _coarse = std::move(coarse_mesh);
        _fine = std::move(fine_mesh);
        // Check that all coarse elements have at least one descendant, otherwise
        // the fine mesh cannot correspond to a refinement of the coarse mesh.
        for (Index k = 0; k < _coarse.num_elements(); ++k)
        {
            // Since this is a no-op in release mode, the compiler will optimize it away
            assert(!_descendants.descendants_for(k).empty());
        }
    };

    template <typename Scalar, typename Index>
    typename BiscaleMesh<Scalar, Index>::CoarsePatch
    BiscaleMesh<Scalar, Index>::coarse_element_patch(Index coarse_element, unsigned int max_distance) const
    {
        return patch_for_element<CoarseTag>(coarse_mesh(), coarse_element, max_distance);
    };

    template <typename Scalar, typename Index>
    typename BiscaleMesh<Scalar, Index>::FinePatch
    BiscaleMesh<Scalar, Index>::fine_patch_from_coarse(
            const BiscaleMesh<Scalar, Index>::CoarsePatch & coarse_patch) const
    {
        std::vector<Index> fine_patch;
        for (const auto coarse_element : coarse_patch)
        {
            for (const auto fine_element : descendants_for(coarse_element))
            {
                fine_patch.push_back(fine_element);
            }
        }
        fine_patch = algo::sorted_unique(std::move(fine_patch));
        return FinePatch(fine_mesh(), std::move(fine_patch));
    };

    template <typename Scalar, typename Index>
    Index BiscaleMesh<Scalar, Index>::ancestor_for(Index fine_element) const
    {
        return _fine.ancestor_for(fine_element);
    };

    template <typename Scalar, typename Index>
    typename BiscaleMesh<Scalar, Index>::ConstIndexIteratorRange
    BiscaleMesh<Scalar, Index>::descendants_for(Index coarse_element) const
    {
        return _descendants.descendants_for(coarse_element);
    };

    template <typename Scalar, typename Index>
    const IndexedMesh<Scalar, Index> & BiscaleMesh<Scalar, Index>::coarse_mesh() const
    {
        return _coarse;
    };

    template <typename Scalar, typename Index>
    const IndexedMesh<Scalar, Index> & BiscaleMesh<Scalar, Index>::fine_mesh() const
    {
        return _fine;
    };

}


/* Copyright (C) 2019-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

#include <helib/PolyMod.h>
#include <helib/exceptions.h>
#include <NTL/ZZX.h>
#include <NTL/ZZ_p.h>
#include <vector>
#include <helib/NumbTh.h>

#include "io.h"

namespace helib {

PolyMod::PolyMod() : ringDescriptor(nullptr) {}
PolyMod::PolyMod(const std::shared_ptr<PolyModRing>& ringDescriptor) :
    PolyMod(NTL::ZZX(0), ringDescriptor)
{}
PolyMod::PolyMod(long input,
                 const std::shared_ptr<PolyModRing>& ringDescriptor) :
    PolyMod(NTL::ZZX(input), ringDescriptor)
{}
PolyMod::PolyMod(const std::vector<long>& input,
                 const std::shared_ptr<PolyModRing>& ringDescriptor) :
    PolyMod(ringDescriptor)
{
  *this = input;
}
PolyMod::PolyMod(const NTL::ZZX& input,
                 const std::shared_ptr<PolyModRing>& ringDescriptor) :
    ringDescriptor(ringDescriptor), data(input)
{
  this->modularReduce();
}

PolyMod& PolyMod::operator=(long input)
{
  assertValidity(*this);
  this->data = NTL::ZZX(input);
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator=(const std::vector<long>& input)
{
  assertValidity(*this);
  NTL::clear(data); // Make sure higher-degree terms don't remain
  for (std::size_t i = 0; i < input.size(); ++i)
    NTL::SetCoeff(data, i, input[i]);
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator=(const std::initializer_list<long>& input)
{
  assertValidity(*this);
  *this = std::vector<long>(input);
  return *this;
}

PolyMod& PolyMod::operator=(const NTL::ZZX& input)
{
  assertValidity(*this);
  this->data = input;
  this->modularReduce();
  return *this;
}

PolyMod::operator long() const
{
  assertValidity(*this);
  long ret;
  NTL::conv(ret, NTL::ConstTerm(this->data));
  return ret;
}

PolyMod::operator std::vector<long>() const
{
  assertValidity(*this);
  std::vector<long> ret(NTL::deg(ringDescriptor->G));
  for (std::size_t i = 0; i < ret.size(); ++i)
    NTL::conv(ret[i], NTL::coeff(data, i));
  return ret;
}

PolyMod::operator NTL::ZZX() const
{
  assertValidity(*this);
  return getData();
}

bool PolyMod::isValid() const { return ringDescriptor != nullptr; }

long PolyMod::getp2r() const { return ringDescriptor->p2r; }

NTL::ZZX PolyMod::getG() const { return ringDescriptor->G; }

const NTL::ZZX& PolyMod::getData() const
{
  assertValidity(*this);
  return this->data;
}

bool PolyMod::operator==(const PolyMod& rhs) const
{
  if (!isValid() && !rhs.isValid())
    return true;
  else
    return isValid() && rhs.isValid() &&
           *ringDescriptor == *(rhs.ringDescriptor) && data == rhs.data;
}

bool PolyMod::operator==(long rhs) const { return *this == NTL::ZZX(rhs); }

bool PolyMod::operator==(const std::vector<long>& rhs) const
{
  if (!this->isValid()) {
    return false;
  } else {
    PolyMod other(rhs, ringDescriptor);
    return *this == other;
  }
}

bool PolyMod::operator==(const NTL::ZZX& rhs) const
{
  if (!this->isValid()) {
    return false;
  } else {
    PolyMod copy(*this);
    // Using subtraction to ensure modularReduce is called.
    // We are checking for divisibility of difference by G in Z_p rather than
    // direct equality
    copy -= rhs;
    return copy.data == NTL::ZZX(0);
  }
}

PolyMod& PolyMod::negate()
{
  assertValidity(*this);
  *this *= -1;
  return *this;
}

PolyMod PolyMod::operator-() const
{
  assertValidity(*this);
  PolyMod poly(*this);
  poly.negate();
  return poly;
}

PolyMod PolyMod::operator*(const PolyMod& rhs) const
{
  assertInterop(*this, rhs);
  PolyMod result = *this;
  result *= rhs;
  return result;
}

PolyMod PolyMod::operator*(long rhs) const { return operator*(NTL::ZZX{rhs}); }

PolyMod PolyMod::operator*(const NTL::ZZX& rhs) const
{
  PolyMod result(*this);
  PolyMod multiplier(*this);
  multiplier = rhs;
  return result * multiplier;
}

PolyMod PolyMod::operator+(const PolyMod& rhs) const
{
  assertInterop(*this, rhs);
  PolyMod result(*this);
  result += rhs;
  return result;
}

PolyMod PolyMod::operator+(long rhs) const { return operator+(NTL::ZZX{rhs}); }

PolyMod PolyMod::operator+(const NTL::ZZX& rhs) const
{
  PolyMod result(*this);
  PolyMod addend(result);
  addend = rhs;
  return result + addend;
}

PolyMod PolyMod::operator-(const PolyMod& rhs) const
{
  assertInterop(*this, rhs);
  PolyMod result = *this;
  result -= rhs;
  return result;
}

PolyMod PolyMod::operator-(long rhs) const { return operator-(NTL::ZZX{rhs}); }

PolyMod PolyMod::operator-(const NTL::ZZX& rhs) const
{
  PolyMod result(*this);
  PolyMod subtrahend(result);
  subtrahend = rhs;
  return result - subtrahend;
}

PolyMod& PolyMod::operator*=(const PolyMod& otherPoly)
{
  assertInterop(*this, otherPoly);
  this->data *= otherPoly.data;
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator*=(long scalar)
{
  assertValidity(*this);
  this->data *= NTL::ZZX(scalar);
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator*=(const NTL::ZZX& otherPoly)
{
  assertValidity(*this);
  this->data *= otherPoly;
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator+=(const PolyMod& otherPoly)
{
  assertInterop(*this, otherPoly);
  this->data += otherPoly.data;
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator+=(long scalar)
{
  assertValidity(*this);
  this->data += NTL::ZZX(scalar);
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator+=(const NTL::ZZX& otherPoly)
{
  assertValidity(*this);
  this->data += otherPoly;
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator-=(const PolyMod& otherPoly)
{
  assertInterop(*this, otherPoly);
  this->data -= otherPoly.data;
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator-=(long scalar)
{
  assertValidity(*this);
  this->data -= NTL::ZZX(scalar);
  this->modularReduce();
  return *this;
}

PolyMod& PolyMod::operator-=(const NTL::ZZX& otherPoly)
{
  assertValidity(*this);
  this->data -= otherPoly;
  this->modularReduce();
  return *this;
}

void PolyMod::writeToJSON(std::ostream& os) const
{
  PolyMod::assertValidity(*this);
  executeRedirectJsonError<void>([&]() { os << writeToJSON(); });
}

JsonWrapper PolyMod::writeToJSON() const
{
  PolyMod::assertValidity(*this);

  return executeRedirectJsonError<JsonWrapper>(
      [&]() { return wrap(this->data); });
}

PolyMod PolyMod::readFromJSON(
    std::istream& is,
    const std::shared_ptr<PolyModRing>& ringDescriptor)
{
  PolyMod poly(ringDescriptor);
  poly.readJSON(is);
  return poly;
}

PolyMod PolyMod::readFromJSON(
    const JsonWrapper& jw,
    const std::shared_ptr<PolyModRing>& ringDescriptor)
{
  PolyMod poly(ringDescriptor);
  poly.readJSON(jw);
  return poly;
}

void PolyMod::readJSON(std::istream& is)
{
  executeRedirectJsonError<void>([&]() {
    json j;
    is >> j;
    this->readJSON(wrap(j));
  });
}

void PolyMod::readJSON(const JsonWrapper& jw)
{
  auto body = [&]() {
    PolyMod::assertValidity(*this);

    NTL::ZZX poly = unwrap(jw);

    long g_degree = NTL::deg(this->ringDescriptor->G);
    if (deg(poly) >= g_degree) {
      // Too many elements. Raising an error.
      std::stringstream err_msg;
      err_msg << "Cannot deserialize to PolyMod: Degree is too small.  "
              << "Trying to deserialize " << deg(poly) + 1 << " coefficients.  "
              << "Slot modulus degree is " << g_degree << ".";
      throw IOError(err_msg.str());
    }

    NTL::clear(this->data); // Make sure higher-degree terms don't remain
    this->data = poly;

    // Normalization (removal of leading zeros) is done by modularReduce.
    this->modularReduce();
  };

  executeRedirectJsonError<void>(body);
}

std::istream& operator>>(std::istream& is, PolyMod& poly)
{
  PolyMod::assertValidity(poly);

  poly.readJSON(is);
  return is;
}

std::ostream& operator<<(std::ostream& os, const PolyMod& poly)
{
  PolyMod::assertValidity(poly);

  poly.writeToJSON(os);
  return os;
}

void PolyMod::modularReduce()
{
  NTL::ZZ_pContext pContext;
  pContext.save();
  NTL::ZZ_p::init(NTL::ZZ(ringDescriptor->p2r));
  NTL::ZZ_pX poly_mod_p2r;
  NTL::conv(poly_mod_p2r, this->data);
  NTL::ZZ_pX G_mod_p2r;
  NTL::conv(G_mod_p2r, ringDescriptor->G);
  poly_mod_p2r %= G_mod_p2r;
  NTL::conv(this->data, poly_mod_p2r);
  pContext.restore();
  this->data.normalize();
}

void PolyMod::assertValidity(const PolyMod& poly)
{
  if (!poly.isValid()) {
    throw LogicError("Cannot operate on invalid (default constructed) PolyMod");
  }
}

void PolyMod::assertInterop(const PolyMod& lhs, const PolyMod& rhs)
{
  assertValidity(lhs);
  assertValidity(rhs);
  if (*(lhs.ringDescriptor) != *(rhs.ringDescriptor))
    throw LogicError("Ring descriptors are not equal between PolyMod objects");
}

} // namespace helib