//
// Created by eliane on 06/03/19.
//

#include "VinaLikeCovalentReversible.h"

#include <exception>

#include <Structures/Atom.h>

#include <Engines/Internals/InternalsUtilityFunctions.h>

#include <boost/log/trivial.hpp>

#include "VinaLikeCommon.h"
#include "VinaExtendedCommon.h"

namespace SmolDock::Score {

    const std::array<std::string, VinaLikeCovalentReversible::numCoefficients>
    VinaLikeCovalentReversible::coefficientsNames =  {"Gauss1", "Gauss2", "RepulsionExceptCovalent", "Hydrophobic","Hydrogen", "CovalentReversible"};

    template<bool OnlyIntermolecular, bool useNonDefaultCoefficients> // default : false
    double VinaLikeCovalentReversibleIntermolecularScoringFunction(const iConformer &ligand_, iTransform &transform,
                                                 const iProtein &protein,
                                                 std::array<double, VinaLikeCovalentReversible_numCoefficients> nonDefaultCoeffs) {

        BOOST_ASSERT(!ligand_.x.empty());
        BOOST_ASSERT(!protein.x.empty());

        BOOST_ASSERT(transform.bondRotationsAngles.size() == ligand_.num_rotatable_bond);

        if(std::abs(transform.rota.norm() - 1) > 0.1) {
            transform.rota.normalize();
        }

        double score_raw = 0;

        iConformer ligand = ligand_;
        applyBondRotationInPlace(ligand, transform);

        Eigen::Vector3d ProtCenterPosition = {protein.center_x, protein.center_y, protein.center_z};

        if constexpr(!OnlyIntermolecular) // C++17
        {
            for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
                for (unsigned int idxLig2 = idxLig; idxLig2 < ligand.x.size(); idxLig2++) {

                    if(idxLig == idxLig2)
                        continue;

                    Eigen::Vector3d LigDistance = {ligand.x[idxLig] - ligand.x[idxLig2],
                                                   ligand.y[idxLig] - ligand.y[idxLig2],
                                                   ligand.z[idxLig] - ligand.z[idxLig2]};

                    double distance_raw = LigDistance.norm();
                    const double distance = distanceFromRawDistance(distance_raw, ligand.atomicRadius[idxLig],
                                                                    ligand.atomicRadius[idxLig2]);

//                score_raw += VinaClassic::coeff_gauss1      * vinaGaussComponent(distance, 0.0, 0.5);
//                score_raw += VinaClassic::coeff_gauss2      * vinaGaussComponent(distance, 3.0, 2.0);
                    score_raw += VinaClassic::coeff_repulsion   * vinaRepulsionComponent(distance, 0.0);
                }
            }
        }

        for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
            for (unsigned int idxProt = 0; idxProt < protein.x.size(); idxProt++) {

                Eigen::Vector3d LigPosition = {ligand.x[idxLig], ligand.y[idxLig], ligand.z[idxLig]};
                applyRigidTransformInPlace(LigPosition, transform);

                Eigen::Vector3d ProtPosition = {protein.x[idxProt], protein.y[idxProt], protein.z[idxProt]};
                Eigen::Vector3d distToCenterVector = LigPosition - ProtCenterPosition;

                double distanceToProteinCenter = distToCenterVector.norm();

                if (distanceToProteinCenter > (protein.radius - 1)) {
                    score_raw += std::pow((distanceToProteinCenter - protein.radius), 4) + 10;
                    continue;
                }

                Eigen::Vector3d distVect = ProtPosition - LigPosition;

                double rawDist = distVect.norm();

                if (rawDist >= VinaClassic::interaction_cutoff)
                    continue;

                const double distance = distanceFromRawDistance(rawDist, ligand.atomicRadius[idxLig],
                                                                protein.atomicRadius[idxProt]);

                const unsigned int atom1AtomicNumber = ligand.type[idxLig];
                const unsigned int atom1AtomVariant = ligand.variant[idxLig];
                const unsigned int atom2AtomicNumber = protein.type[idxProt];
                const unsigned int atom2AtomVariant = protein.variant[idxProt];

                if constexpr(useNonDefaultCoefficients)
                {
                    score_raw += nonDefaultCoeffs[0]      * vinaGaussComponent(distance, 0.0, 0.5);
                    score_raw += nonDefaultCoeffs[1]     * vinaGaussComponent(distance, 3.0, 2.0);
                    score_raw += nonDefaultCoeffs[2]   * vinaRepulsionComponent(distance, 0.0);
                    score_raw += nonDefaultCoeffs[3] * vinaHydrophobicComponent(distance,
                                                                               atom1AtomicNumber, atom1AtomVariant,
                                                                               atom2AtomicNumber, atom2AtomVariant);

                    score_raw += nonDefaultCoeffs[4]    * vinaHydrogenComponent(distance,
                                                                               atom1AtomicNumber, atom1AtomVariant,
                                                                               atom2AtomicNumber, atom2AtomVariant);
                    score_raw += nonDefaultCoeffs[5] * VinaExtended::covalentReversibleComponent(distance,
                                                                                atom1AtomicNumber, atom1AtomVariant,
                                                                                atom2AtomicNumber, atom2AtomVariant);
                }else {
                    score_raw += VinaClassic::coeff_gauss1      * vinaGaussComponent(distance, 0.0, 0.5);
                    score_raw += VinaClassic::coeff_gauss2      * vinaGaussComponent(distance, 3.0, 2.0);
                    score_raw += VinaClassic::coeff_repulsion   * VinaExtended::RepulsionExceptForCovalentComponent(distance, 0.0,
                                                                                atom1AtomicNumber, atom1AtomVariant,
                                                                                atom2AtomicNumber, atom2AtomVariant);
                    score_raw += VinaClassic::coeff_hydrophobic * vinaHydrophobicComponent(distance,
                                                                                atom1AtomicNumber, atom1AtomVariant,
                                                                                atom2AtomicNumber, atom2AtomVariant);

                    score_raw += VinaClassic::coeff_hydrogen    * vinaHydrogenComponent(distance,
                                                                                atom1AtomicNumber, atom1AtomVariant,
                                                                                atom2AtomicNumber, atom2AtomVariant);

                    score_raw += VinaExtended::coeff_CovalentReversible * VinaExtended::covalentReversibleComponent(distance,
                                                                                atom1AtomicNumber, atom1AtomVariant,
                                                                                atom2AtomicNumber, atom2AtomVariant);
                }

            } // for
        } // for

        double final_score = score_raw / (1 + (VinaClassic::coeff_entropic * ligand.num_rotatable_bond));
        return final_score;
    }

    VinaLikeCovalentReversible::VinaLikeCovalentReversible(const iConformer &startingConformation_,
                       const iProtein &p,
                       const iTransform &initialTransform_,
                       double differential_epsilon_,
                        bool useNonDefaultCoefficient) :
            useNonDefaultCoefficient(useNonDefaultCoefficient),
            startingConformation(startingConformation_),
            prot(p),
            initialTransform(initialTransform_),
            differential_epsilon(differential_epsilon_) {
        this->numberOfRotatableBonds = this->startingConformation.num_rotatable_bond;
        this->numberOfParamInState = 7 + (this->numberOfRotatableBonds);

        if (this->initialTransform.bondRotationsAngles.size() != this->numberOfRotatableBonds) {
            BOOST_LOG_TRIVIAL(error)
                    << "Discrepency between the number of rotatable bonds in the iConformer and iTransform ("
                    << this->numberOfRotatableBonds << " != " << this->initialTransform.bondRotationsAngles.size()
                    << ")";
            std::terminate();
        }

        this->nonDefaultCoefficients = {VinaClassic::coeff_gauss1, VinaClassic::coeff_gauss2,
                                        VinaClassic::coeff_repulsion, VinaClassic::coeff_hydrophobic,
                                        VinaClassic::coeff_hydrogen, VinaExtended::coeff_CovalentReversible };

    }

    double VinaLikeCovalentReversible::Evaluate(const arma::mat &x) {
        BOOST_ASSERT(x.n_rows == this->numberOfParamInState);

        iTransform tr = this->internalToExternalRepr(x);

        tr.doHousekeeping();

        double score_ = VinaLikeCovalentReversibleIntermolecularScoringFunction(this->startingConformation, tr, this->prot);

        return score_;
    }

    double VinaLikeCovalentReversible::EvaluateWithGradient(const arma::mat &x, arma::mat &grad) {

        BOOST_ASSERT(!x.has_nan());
        BOOST_ASSERT(!grad.has_nan());
        BOOST_ASSERT(x.n_rows == this->numberOfParamInState);
        BOOST_ASSERT(grad.n_rows == this->numberOfParamInState);

        iTransform tr = this->internalToExternalRepr(x);
        tr.doHousekeeping();

        double score_ = VinaLikeCovalentReversibleIntermolecularScoringFunction(this->startingConformation, tr, this->prot);

        // Translation
        {
            iTransform transform_dx = tr;
            transform_dx.transl.x() += this->differential_epsilon;
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dx, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dx, this->prot);
            grad[0] = gradScore - score_;
        }

        {
            iTransform transform_dy = tr;
            transform_dy.transl.y() += this->differential_epsilon;
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dy, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dy, this->prot);
            grad[1] = gradScore - score_;
        }

        {
            iTransform transform_dz = tr;
            transform_dz.transl.z() += this->differential_epsilon;
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dz, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dz, this->prot);
            grad[2] = gradScore - score_;
        }

        // Rotation

        {
            iTransform transform_dqs = tr;
            transform_dqs.rota.w() += this->differential_epsilon;
            transform_dqs.doHousekeeping();
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dqs, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dqs, this->prot);
            grad[3] = gradScore - score_;
        }

        {
            iTransform transform_dqx = tr;
            transform_dqx.rota.x() += this->differential_epsilon;
            transform_dqx.doHousekeeping();
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dqx, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dqx, this->prot);
            grad[4] = gradScore - score_;

        }

        {
            iTransform transform_dqy = tr;
            transform_dqy.rota.x() += this->differential_epsilon;
            transform_dqy.doHousekeeping();
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dqy, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dqy, this->prot);
            grad[5] = gradScore - score_;
        }

        {
            iTransform transform_dqz = tr;
            transform_dqz.rota.x() += this->differential_epsilon;
            transform_dqz.doHousekeeping();
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dqz, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dqz, this->prot);
            grad[6] = gradScore - score_;
        }

        for (unsigned int i = 0; i < this->numberOfRotatableBonds; i++) {
            iTransform transform_dbondrot = tr;
            transform_dbondrot.bondRotationsAngles[i] += this->differential_epsilon;
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeCovalentReversibleIntermolecularScoringFunction<false,true>(this->startingConformation, transform_dbondrot, this->prot, this->nonDefaultCoefficients)
                                                                    : VinaLikeCovalentReversibleIntermolecularScoringFunction<false,false>(this->startingConformation, transform_dbondrot, this->prot);
            grad[7 + i] = gradScore - score_;

        }

        /*
        BOOST_LOG_TRIVIAL(debug) << "Transform: " << x.t();
        BOOST_LOG_TRIVIAL(debug) << "Score: " << score_;
        BOOST_LOG_TRIVIAL(debug) << "Gradient";
        BOOST_LOG_TRIVIAL(debug) << "     ds: " << grad[3];
        BOOST_LOG_TRIVIAL(debug) << "     du: " << grad[4] << "   dx: " << grad[0];
        BOOST_LOG_TRIVIAL(debug) << "     dv: " << grad[5] << "   dy: " << grad[1];
        BOOST_LOG_TRIVIAL(debug) << "     dt: " << grad[6] << "   dx: " << grad[2];
        //*/

        BOOST_ASSERT(score_ == score_); // catches NaN
        return score_;
    }

    double VinaLikeCovalentReversible::getDifferentialEpsilon() const {
        return this->differential_epsilon;
    }

    arma::mat VinaLikeCovalentReversible::getStartingConditions() const {
        return this->externalToInternalRepr(this->initialTransform);
    }

    iConformer VinaLikeCovalentReversible::getConformerForParamMatrix(const arma::mat &x) {
        BOOST_ASSERT(x.n_rows == this->numberOfParamInState);

        iTransform tr = this->internalToExternalRepr(x);
        tr.doHousekeeping();

        iConformer ret = this->startingConformation;
        applyBondRotationInPlace(ret, tr);
        applyRigidTransformInPlace(ret, tr);

        return ret;
    }

    unsigned int VinaLikeCovalentReversible::getParamVectorDimension() const {
        return this->numberOfParamInState;
    }

    std::vector<std::tuple<std::string, double>> VinaLikeCovalentReversible::EvaluateSubcomponents(const arma::mat &x) {

        BOOST_ASSERT(x.n_rows == this->numberOfParamInState);

        iTransform tr = this->internalToExternalRepr(x);
        tr.doHousekeeping();

        return VinaLikeCovalentReversibleIntermolecularComponents(this->startingConformation, tr, this->prot);
    }

    double VinaLikeCovalentReversible::EvaluateOnlyIntermolecular(const arma::mat &x) {
        BOOST_ASSERT(x.n_rows == this->numberOfParamInState);

        iTransform tr = this->internalToExternalRepr(x);

        tr.doHousekeeping();

        // Template parameter controls whether onlyIntermolecular interaction are taken into account. Here we want true
        double score_ = VinaLikeCovalentReversibleIntermolecularScoringFunction<true>(this->startingConformation, tr, this->prot);

        return score_;
    }

    unsigned int VinaLikeCovalentReversible::getCoefficientsVectorWidth() {
        return this->numCoefficients;
    }

    std::vector<std::string> VinaLikeCovalentReversible::getCoefficientsNames() {
        return std::vector<std::string>(this->coefficientsNames.begin(), this->coefficientsNames.end());
    }

    std::vector<double> VinaLikeCovalentReversible::getCurrentCoefficients() {
        if(this->useNonDefaultCoefficient)
        {
            return std::vector<double>(this->nonDefaultCoefficients.begin(),this->nonDefaultCoefficients.end());
        }
        return {VinaClassic::coeff_gauss1, VinaClassic::coeff_gauss2,
                VinaClassic::coeff_repulsion, VinaClassic::coeff_hydrophobic,
                VinaClassic::coeff_hydrogen, VinaExtended::coeff_CovalentReversible };

    }

    bool VinaLikeCovalentReversible::setNonDefaultCoefficients(std::vector<double> coeffs) {
        if(coeffs.size() != this->numCoefficients)
        {
            BOOST_LOG_TRIVIAL(error) << "Trying to set " << this->numCoefficients <<" coefficients with vector of " << coeffs.size() << " values.";
            return false;
        }
        if(this->useNonDefaultCoefficient == false)
        {
            BOOST_LOG_TRIVIAL(error) << "Trying to set non default coefficient, but this scoring function was constructed with default coefficients only.";
            BOOST_LOG_TRIVIAL(error) << "Check the parameters passed to the scoring function constructor.";
            return false;
        }
        for (unsigned int j = 0; j < this->nonDefaultCoefficients.size(); ++j) {
            this->nonDefaultCoefficients[j] = coeffs[j];
        }
        return true;
    }

    template<bool useNonDefaultCoefficients> // default : false
    std::vector<std::tuple<std::string, double>> VinaLikeCovalentReversibleIntermolecularComponents(const iConformer &conformer, iTransform &transform,
                                                                                const iProtein &protein,
                                                                                std::array<double, VinaLikeCovalentReversible_numCoefficients> nonDefaultCoeffs)
    {
        BOOST_ASSERT(!conformer.x.empty());
        BOOST_ASSERT(!protein.x.empty());
        BOOST_ASSERT(transform.bondRotationsAngles.size() == conformer.num_rotatable_bond);

        if(std::abs(transform.rota.norm() - 1) > 0.1) {
            transform.rota.normalize();
        }

        std::vector<std::tuple<std::string, double>> ret;

        double gauss1_total = 0.0;
        double gauss2_total = 0.0;
        double repulsion_total = 0.0;
        double repulsion_VinaClassic_total = 0.0;
        double hydrogen_total = 0.0;
        double hydrophobic_total = 0.0;
        double covrev_total = 0.0;
        double score_raw = 0.0;

        double intramolecular_repuls_total = 0.0;
        double intramolecular_score = 0.0;

        iConformer ligand = conformer;
        applyBondRotationInPlace(ligand, transform);

        Eigen::Vector3d ProtCenterPosition = {protein.center_x, protein.center_y, protein.center_z};

        for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
            for (unsigned int idxLig2 = idxLig; idxLig2 < ligand.x.size(); idxLig2++) {

                if(idxLig == idxLig2)
                    continue;

                Eigen::Vector3d LigDistance = {ligand.x[idxLig] - ligand.x[idxLig2],
                                               ligand.y[idxLig] - ligand.y[idxLig2],
                                               ligand.z[idxLig] - ligand.z[idxLig2]};

                double distance_raw = LigDistance.norm();
                const double distance = distanceFromRawDistance(distance_raw, ligand.atomicRadius[idxLig],
                                                                ligand.atomicRadius[idxLig2]);

//                score_raw += VinaClassic::coeff_gauss1      * vinaGaussComponent(distance, 0.0, 0.5);
//                score_raw += VinaClassic::coeff_gauss2      * vinaGaussComponent(distance, 3.0, 2.0);
                intramolecular_repuls_total +=  vinaRepulsionComponent(distance, 0.0);

            }
        }

        intramolecular_score = VinaClassic::coeff_repulsion   * intramolecular_repuls_total;

        for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
            for (unsigned int idxProt = 0; idxProt < protein.x.size(); idxProt++) {

                Eigen::Vector3d LigPosition = {ligand.x[idxLig], ligand.y[idxLig], ligand.z[idxLig]};
                applyRigidTransformInPlace(LigPosition, transform);

                Eigen::Vector3d ProtPosition = {protein.x[idxProt], protein.y[idxProt], protein.z[idxProt]};
                Eigen::Vector3d distToCenterVector = LigPosition - ProtCenterPosition;

                double distanceToProteinCenter = distToCenterVector.norm();

                if (distanceToProteinCenter > (protein.radius - 1)) {
                    score_raw += std::pow((distanceToProteinCenter - protein.radius), 4) + 10;
                    continue;
                }

                Eigen::Vector3d distVect = ProtPosition - LigPosition;

                double rawDist = distVect.norm();

                if (rawDist >= VinaClassic::interaction_cutoff)
                    continue;

                const double distance = distanceFromRawDistance(rawDist, ligand.atomicRadius[idxLig],
                                                          protein.atomicRadius[idxProt]);

                const unsigned int atom1AtomicNumber = ligand.type[idxLig];
                const unsigned int atom1AtomVariant = ligand.variant[idxLig];
                const unsigned int atom2AtomicNumber = protein.type[idxProt];
                const unsigned int atom2AtomVariant = protein.variant[idxProt];

                gauss1_total += vinaGaussComponent(distance, 0.0, 0.5);
                gauss2_total += vinaGaussComponent(distance, 3.0, 2.0);
                repulsion_total += VinaExtended::RepulsionExceptForCovalentComponent(distance, 0.0,
                                                                                atom1AtomicNumber, atom1AtomVariant,
                                                                                atom2AtomicNumber, atom2AtomVariant);
                hydrophobic_total += vinaHydrophobicComponent(distance,
                                                           atom1AtomicNumber, atom1AtomVariant,
                                                           atom2AtomicNumber, atom2AtomVariant);
                hydrogen_total += vinaHydrogenComponent(distance,
                                                           atom1AtomicNumber, atom1AtomVariant,
                                                           atom2AtomicNumber, atom2AtomVariant);
                covrev_total += VinaExtended::covalentReversibleComponent(distance,
                                                                          atom1AtomicNumber, atom1AtomVariant,
                                                                          atom2AtomicNumber, atom2AtomVariant);

                // Not used in the calculation of the score
                repulsion_VinaClassic_total += vinaRepulsionComponent(distance, 0.0);

            } // for
        } // for

        double score_sum = 0.0;
        double score_NoCovRev = 0.0;
        if constexpr(useNonDefaultCoefficients)
        {
            ret.emplace_back(std::make_tuple("NonDefaultCoeffs", 1.0));
            score_sum =   nonDefaultCoeffs[0] * gauss1_total
                                 + nonDefaultCoeffs[1] * gauss2_total
                                 + nonDefaultCoeffs[2] * repulsion_total
                                 + nonDefaultCoeffs[3] * hydrophobic_total
                                 + nonDefaultCoeffs[4] * hydrogen_total
                                 + nonDefaultCoeffs[5] * covrev_total;

            score_NoCovRev =   nonDefaultCoeffs[0] * gauss1_total
                               + nonDefaultCoeffs[1] * gauss2_total
                               + nonDefaultCoeffs[2] * repulsion_total
                               + nonDefaultCoeffs[3] * hydrophobic_total
                               + nonDefaultCoeffs[4] * hydrogen_total;
        }else {
            score_sum =   VinaClassic::coeff_gauss1 * gauss1_total
                                 + VinaClassic::coeff_gauss2 * gauss2_total
                                 + VinaClassic::coeff_repulsion * repulsion_total
                                 + VinaClassic::coeff_hydrophobic * hydrophobic_total
                                 + VinaClassic::coeff_hydrogen * hydrogen_total
                                 + VinaExtended::coeff_CovalentReversible * covrev_total;

            score_NoCovRev =   VinaClassic::coeff_gauss1 * gauss1_total
                                      + VinaClassic::coeff_gauss2 * gauss2_total
                                      + VinaClassic::coeff_repulsion * repulsion_total
                                      + VinaClassic::coeff_hydrophobic * hydrophobic_total
                                      + VinaClassic::coeff_hydrogen * hydrogen_total;
        }

        double final_score = score_sum / (1 + (VinaClassic::coeff_entropic * ligand.num_rotatable_bond));

        double final_score_nocovrev = score_NoCovRev / (1 + (VinaClassic::coeff_entropic * ligand.num_rotatable_bond));

        ret.emplace_back(std::make_tuple("Gauss1", gauss1_total));
        ret.emplace_back(std::make_tuple("Gauss2", gauss2_total));
        ret.emplace_back(std::make_tuple("Repulsion_NotUsedInScore", repulsion_VinaClassic_total));
        ret.emplace_back(std::make_tuple("RepulsionExceptCovalent", repulsion_total));
        ret.emplace_back(std::make_tuple("Hydrophobic", hydrophobic_total));
        ret.emplace_back(std::make_tuple("Hydrogen", hydrogen_total ));
        ret.emplace_back(std::make_tuple("CovalentReversible", covrev_total));
        ret.emplace_back(std::make_tuple("numRot", ligand.num_rotatable_bond));
        ret.emplace_back(std::make_tuple("Intra_Repuls", intramolecular_repuls_total));
        ret.emplace_back(std::make_tuple("Intra_Score", intramolecular_score));
        ret.emplace_back(std::make_tuple("ScoreRaw_NoCovRev", score_NoCovRev));
        ret.emplace_back(std::make_tuple("Score_NoCovRev", final_score_nocovrev));
        ret.emplace_back(std::make_tuple("Score_Raw", score_sum));
        ret.emplace_back(std::make_tuple("Score", final_score));
        return ret;
    }

}


#include "AdRecommender.h"
#include <glog/logging.h>
#include <boost/filesystem.hpp>
#include <fstream>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <3rdparty/am/google/sparsetable.h>
#include <boost/serialization/map.hpp>
#include <boost/serialization/set.hpp>
#include <boost/serialization/unordered_map.hpp>

#define LATENT_VEC_DIM  10

namespace bfs = boost::filesystem;
using namespace boost::numeric::ublas;

namespace sf1r
{
static const double SMOOTH = 0.02;
static const double MAX_NORM = 100;

static double affinity(const AdRecommender::LatentVecT& left, const AdRecommender::LatentVecT& right)
{
    assert(left.size() == right.size());
    double sum = 0;
    for (size_t i = 0; i < left.size(); ++i)
    {
        sum += left[i] * right[i];
    }
    return sum;
}

static int getunviewed(const std::bitset<AdRecommender::MAX_AD_ITEMS>& bits, std::size_t index)
{
    if (index >= bits.count())
        return -1;
    for(std::size_t i = 0; i < bits.size(); ++i)
    {
        if (bits.test(i))
        {
            if (index == 0)
            {
                return i;
            }
            --index;
        }
    }
    return -1;
}

AdRecommender::AdRecommender()
    :use_ad_feature_(true)
{
}

AdRecommender::~AdRecommender()
{
}

void AdRecommender::init(const std::string& data_path, bool use_ad_feature)
{
    srand(time(NULL));
    clicked_num_ = 1;
    impression_num_ = 1000;
    data_path_ = data_path;
    use_ad_feature_ = use_ad_feature;
    bfs::create_directories(data_path_);
    load();
    if (clicked_num_ < 1)
        clicked_num_ = 1;
    if (impression_num_ < clicked_num_)
        impression_num_ = clicked_num_*100;

    learning_rate_ = 1/(double)clicked_num_;
    ratio_ = -1 * (double)clicked_num_ /(double)(impression_num_ - clicked_num_);
    LOG(INFO) << "init clicked_num_: " << clicked_num_ << ", ratio_:" << ratio_;
    default_latent_.resize(LATENT_VEC_DIM, 1);
    //db_ = new MatrixType(10000, data_path + "/rec.db");
}

void AdRecommender::load()
{
    std::ifstream ifs(std::string(data_path_ + "/ad_latent.data").c_str());
    std::string data;
    if (ifs.good())
    {
        std::size_t len = 0;
        ifs.read((char*)&len, sizeof(len));
        data.resize(len);
        ifs.read((char*)&data[0], len);
        izenelib::util::izene_deserialization<LatentVecContainerT> izd(data.data(), data.size());
        izd.read_image(ad_latent_vec_list_);
    }
    LOG(INFO) << "ad latent vec list loaded: " << ad_latent_vec_list_.size();
    ifs.close();

    ifs.open(std::string(data_path_ + "/user_latent.data").c_str());
    if (ifs.good())
    {
        std::size_t len = 0;
        ifs.read((char*)&len, sizeof(len));
        data.resize(len);
        ifs.read((char*)&data[0], len);
        izenelib::util::izene_deserialization<LatentVecContainerT> izd(data.data(), data.size());
        izd.read_image(user_feature_latent_vec_list_);
    }
    LOG(INFO) << "user latent vec list loaded: " << user_feature_latent_vec_list_.size();
    ifs.close();

    ifs.open(std::string(data_path_ + "/history_data.data").c_str());
    if (ifs.good())
    {
        ifs.read((char*)&clicked_num_, sizeof(clicked_num_));
        ifs.read((char*)&impression_num_, sizeof(impression_num_));
    }
    LOG(INFO) << "history data: clicked " << clicked_num_ << ", impression " << impression_num_;
    ifs.close();

    ifs.open(std::string(data_path_ + "/ad_feature_info.data").c_str());
    if(ifs.good())
    {
        std::size_t len = 0;
        ifs.read((char*)&len, sizeof(len));
        data.resize(len);
        ifs.read((char*)&data[0], len);
        izenelib::util::izene_deserialization<std::vector<std::string> > izd(data.data(), data.size());
        izd.read_image(ad_feature_value_list_);
        len = 0;
        ifs.read((char*)&len, sizeof(len));
        data.resize(len);
        ifs.read((char*)&data[0], len);
        izenelib::util::izene_deserialization<AdFeatureContainerT> izd_2(data.data(), data.size());
        izd_2.read_image(ad_features_map_);
        for(size_t i = 0; i < ad_feature_value_list_.size(); ++i)
        {
            ad_feature_value_id_list_[ad_feature_value_list_[i]] = i;
        }
    }
    LOG(INFO) << "ad feature items loaded: " << ad_feature_value_list_.size();
}

void AdRecommender::save()
{
    std::ofstream ofs(std::string(data_path_ + "/ad_latent.data").c_str());
    std::size_t len = 0;
    char* buf = NULL;
    {
        boost::shared_lock<boost::shared_mutex> lock(ad_latent_lock_);
        izenelib::util::izene_serialization<LatentVecContainerT> izs(ad_latent_vec_list_);
        izs.write_image(buf, len);
        ofs.write((const char*)&len, sizeof(len));
        ofs.write(buf, len);
        ofs.flush();
        ofs.close();
    }

    {
        boost::shared_lock<boost::shared_mutex> lock(user_latent_lock_);
        ofs.open(std::string(data_path_ + "/user_latent.data").c_str());
        len = 0;
        izenelib::util::izene_serialization<LatentVecContainerT> izs(user_feature_latent_vec_list_);
        izs.write_image(buf, len);
        ofs.write((const char*)&len, sizeof(len));
        ofs.write(buf, len);
        ofs.flush();
        ofs.close();
    }

    {
        ofs.open(std::string(data_path_ + "/history_data.data").c_str());
        ofs.write((const char*)&clicked_num_, sizeof(clicked_num_));
        ofs.write((const char*)&impression_num_, sizeof(impression_num_));
        ofs.flush();
        ofs.close();
    }

    boost::shared_lock<boost::shared_mutex> lock(ad_feature_lock_);
    ofs.open(std::string(data_path_ + "/ad_feature_info.data").c_str());
    {
        len = 0;
        izenelib::util::izene_serialization<std::vector<std::string> > izs(ad_feature_value_list_);
        izs.write_image(buf, len);
        ofs.write((const char*)&len, sizeof(len));
        ofs.write(buf, len);
    }

    {
        len = 0;
        izenelib::util::izene_serialization<AdFeatureContainerT> izs(ad_features_map_);
        izs.write_image(buf, len);
        ofs.write((const char*)&len, sizeof(len));
        ofs.write(buf, len);
    }
    ofs.flush();
    ofs.close();
}

//void AdRecommender::setMaxAdDocId(docid_t max_docid)
//{
//    ad_latent_vec_list_.resize(max_docid);
//}

void AdRecommender::getUserLatentVecKeys(const FeatureT& user_info, std::vector<std::string>& user_latentvec_keys)
{
    user_latentvec_keys.resize(user_info.size());
    std::size_t i = 0;
    for(FeatureT::const_iterator it = user_info.begin(); it != user_info.end(); ++it)
    {
        user_latentvec_keys[i++] = it->first + it->second;
    }
}

void AdRecommender::updateAdFeatures(const std::string& ad_docid, const std::vector<std::string>& features)
{
    boost::unique_lock<boost::shared_mutex> lock(ad_feature_lock_);
    std::set<uint32_t>& ad_feature_id_list = ad_features_map_[ad_docid];
    ad_feature_id_list.clear();
    std::vector<std::string> expand_features = features;
    if (!use_ad_feature_ || features.empty())
    {
        expand_features.push_back(ad_docid);
    }
    for (std::size_t i = 0; i < expand_features.size(); ++i)
    {
        uint32_t id = 0;
        const std::string& feature_str = expand_features[i];
        boost::unordered_map<std::string, uint32_t>::iterator it = ad_feature_value_id_list_.find(feature_str);
        if (it == ad_feature_value_id_list_.end())
        {
            id = ad_feature_value_list_.size();
            ad_feature_value_list_.push_back(feature_str);
            ad_feature_value_id_list_[feature_str] = id;
            unviewed_items_.set(id);
        }
        else
        {
            id = it->second;
        }
        ad_feature_id_list.insert(id);
    }
}

void AdRecommender::getAdLatentVecKeys(const std::string& ad_docid, std::vector<std::string>& ad_latentvec_keys)
{
    if (!use_ad_feature_)
    {
        ad_latentvec_keys.resize(1);
        ad_latentvec_keys[0] = ad_docid;
        return;
    }
    AdFeatureContainerT::const_iterator it = ad_features_map_.find(ad_docid);
    if (it != ad_features_map_.end())
    {
        ad_latentvec_keys.resize(it->second.size());
        std::size_t i = 0;
        std::set<uint32_t>::const_iterator value_it = it->second.begin();
        for(; value_it != it->second.end(); ++value_it)
        {
            ad_latentvec_keys[i++] = ad_feature_value_list_[*value_it];
        }
    }
}

void AdRecommender::getCombinedUserLatentVec(const std::vector<std::string>& latentvec_keys,
    const std::vector<double>& weight_list, LatentVecT& latent_vec)
{
    latent_vec.clear();
    latent_vec.resize(LATENT_VEC_DIM, 0);
    for (std::size_t i = 0; i < latentvec_keys.size(); ++i)
    {
        LatentVecContainerT::const_iterator it = user_feature_latent_vec_list_.find(latentvec_keys[i]);
        if (it == user_feature_latent_vec_list_.end())
            continue;
        double w = weight_list[i];
        for(std::size_t j = 0; j < latent_vec.size(); ++j)
        {
            latent_vec[j] += it->second[j]*w;
        }
    }
    for(std::size_t j = 0; j < latent_vec.size(); ++j)
    {
        if (latentvec_keys.size() == 0)
            latent_vec[j] = 1;
        else
            latent_vec[j] /= latentvec_keys.size();
    }
}

void AdRecommender::getCombinedUserLatentVec(const std::vector<std::string>& latentvec_keys,
    LatentVecT& latent_vec, std::size_t k)
{
    for (std::size_t i = 0; i < latentvec_keys.size(); ++i)
    {
        LatentVecContainerT::const_iterator it = user_feature_latent_vec_list_.find(latentvec_keys[i]);
        if (it == user_feature_latent_vec_list_.end())
            continue;
        latent_vec[k] += it->second[k];
    }
    if (latentvec_keys.size() == 0)
        latent_vec[k] = 1;
    else
        latent_vec[k] /= latentvec_keys.size();
}

void AdRecommender::getCombinedUserLatentVec(const std::vector<std::string>& latentvec_keys, LatentVecT& latent_vec)
{
    latent_vec.clear();
    latent_vec.resize(LATENT_VEC_DIM, 0);
    for (std::size_t i = 0; i < latentvec_keys.size(); ++i)
    {
        LatentVecContainerT::const_iterator it = user_feature_latent_vec_list_.find(latentvec_keys[i]);
        if (it == user_feature_latent_vec_list_.end())
            continue;
        for(std::size_t j = 0; j < latent_vec.size(); ++j)
        {
            latent_vec[j] += it->second[j];
        }
    }
    for(std::size_t j = 0; j < latent_vec.size(); ++j)
    {
        if (latentvec_keys.size() == 0)
            latent_vec[j] = 1;
        else
            latent_vec[j] /= latentvec_keys.size();
    }
}

// get the user latent by combined the features of the user.
// Linear combined or non-linear combined both work. Some weighted can be considered.
void AdRecommender::getCombinedUserLatentVec(const std::vector<LatentVecT>& latentvec_list,
    LatentVecT& latent_vec, std::size_t k)
{
    for (std::size_t i = 0; i < latentvec_list.size(); ++i)
    {
        latent_vec[k] += latentvec_list[i][k];
    }
    if (latentvec_list.size() == 0)
        latent_vec[k] = 1;
    else
        latent_vec[k] /= latentvec_list.size();
}

void AdRecommender::getCombinedUserLatentVec(const std::vector<LatentVecT>& latentvec_list,
    const std::vector<double>& weight_list, LatentVecT& latent_vec)
{
    latent_vec.clear();
    latent_vec.resize(LATENT_VEC_DIM, 0);
    for (std::size_t i = 0; i < latentvec_list.size(); ++i)
    {
        double w = weight_list[i];
        for(std::size_t j = 0; j < latent_vec.size(); ++j)
        {
            latent_vec[j] += latentvec_list[i][j] * w;
        }
    }
    for(std::size_t j = 0; j < latent_vec.size(); ++j)
    {
        if (latentvec_list.size() == 0)
            latent_vec[j] = 1;
        else
            latent_vec[j] /= latentvec_list.size();
    }
}

void AdRecommender::doRecommend(const std::string& user_str_id,
    const FeatureT& user_info, std::size_t max_return,
    std::vector<std::string>& recommended_items,
    std::vector<double>& score_list, bool rec_for_unview)
{
    std::vector<std::string> user_keys;
    getUserLatentVecKeys(user_info, user_keys);
    LatentVecT user_latent_vec;

    {
        boost::shared_lock<boost::shared_mutex> lock(user_latent_lock_);
        getCombinedUserLatentVec(user_keys, user_latent_vec);
    }

    bool has_unviewed_item = rec_for_unview && unviewed_items_.any();

    ScoreSortedAdQueue hit_list(max_return);
    {
        boost::shared_lock<boost::shared_mutex> lock(ad_latent_lock_);
        if (recommended_items.empty())
        {
            LatentVecContainerT::const_iterator it = ad_latent_vec_list_.begin();
            while (it != ad_latent_vec_list_.end())
            {
                if (!(it->second.empty()))
                {
                    ScoredAdItem item;
                    item.score = affinity(user_latent_vec, it->second);
                    item.key = it->first;
                    hit_list.insert(item);
                }
                ++it;
            }
        }
        else
        {
            for(size_t i = 0; i < recommended_items.size(); ++i)
            {
                LatentVecContainerT::const_iterator it = ad_latent_vec_list_.find(recommended_items[i]);
                if (it == ad_latent_vec_list_.end())
                    continue;
                if (it->second.empty())
                    continue;
                ScoredAdItem item;
                item.score = affinity(user_latent_vec, it->second);
                item.key = it->first;
                hit_list.insert(item);
            }
        }
    }
    double default_score = 0;
    if (has_unviewed_item)
        default_score = affinity(user_latent_vec, default_latent_);

    size_t scoresize = hit_list.size();
    recommended_items.resize(hit_list.size());
    score_list.resize(hit_list.size());
    for(int i = scoresize - 1; i >= 0; --i)
    {
        const ScoredAdItem& item = hit_list.pop();
        if (has_unviewed_item)
        {
            if (item.score < default_score)
            {
                boost::shared_lock<boost::shared_mutex> lock(ad_feature_lock_);
                // chose from unviewed items.
                int unview_index = getunviewed(unviewed_items_, scoresize - 1 - i);
                if (unview_index != -1 && unview_index < (int)ad_feature_value_list_.size())
                {
                    recommended_items[i] = ad_feature_value_list_[unview_index];
                    score_list[i] = default_score;
                    //LOG(INFO) << "recommend from unviewed items : " << recommended_items[i];
                    continue;
                }
            }
        }
        recommended_items[i] = item.key;
        score_list[i] = item.score;
    }
}

void AdRecommender::recommendFromCand(const std::string& user_str_id,
    const FeatureT& user_info, std::size_t max_return,
    std::vector<std::string>& recommended_items,
    std::vector<double>& score_list)
{
    doRecommend(user_str_id, user_info, max_return, recommended_items, score_list, false);
}

void AdRecommender::recommend(const std::string& user_str_id,
    const FeatureT& user_info, std::size_t max_return,
    std::vector<std::string>& recommended_items,
    std::vector<double>& score_list)
{
    //LOG(INFO) << "begin do the recommend for user : " << user_str_id;
    doRecommend(user_str_id, user_info, max_return, recommended_items, score_list, true);
    //LOG(INFO) << "finished the ad recommend, size: " << recommended_items.size();
}

void AdRecommender::update(const std::string& user_str_id,
    const FeatureT& user_info, const std::string& ad_docid, bool is_clicked)
{
    impression_num_++;
    if (is_clicked)
    {
        clicked_num_++;
    }
    if (impression_num_ % 50000 == 0)
    {
        learning_rate_ = 1/(double)clicked_num_;
        ratio_ = -1*SMOOTH*(double)clicked_num_/(double)(impression_num_ - clicked_num_) + (1 - SMOOTH)*ratio_;
        LOG(INFO) << " impression_num_ : " << impression_num_ << ", clicked_num_: " << clicked_num_
            << ", ratio_ : " << ratio_;
        dumpUserLatent();
    }

    std::vector<std::string> user_keys;
    getUserLatentVecKeys(user_info, user_keys);
    std::vector<LatentVecT*> user_feature_latent_list;

    {
        boost::unique_lock<boost::shared_mutex> lock_user(user_latent_lock_);
        for (size_t i = 0; i < user_keys.size(); ++i)
        {
            std::pair<LatentVecContainerT::iterator, bool> it_pair = user_feature_latent_vec_list_.insert(std::make_pair(user_keys[i], default_latent_));
            if (it_pair.second)
            {
                // a new feature value
                LatentVecT new_vec(LATENT_VEC_DIM, 0);
                for(std::size_t j = 0; j < LATENT_VEC_DIM; ++j)
                {
                    new_vec[j] = random() % 10;
                }
                it_pair.first->second.swap(new_vec);
                if (!is_clicked)
                    continue;
            }
            LatentVecT& feature_latent = it_pair.first->second;
            user_feature_latent_list.push_back(&feature_latent);
        }
    }

    std::vector<LatentVecT> tmp_user_feature_latent_list(user_feature_latent_list.size());
    for(std::size_t i = 0; i < user_feature_latent_list.size(); ++i)
    {
        tmp_user_feature_latent_list[i] = *(user_feature_latent_list[i]);
    }

    std::vector<std::string> ad_keys;
    std::vector<LatentVecT*> ad_feature_latent_list;

    {
        boost::unique_lock<boost::shared_mutex> lock_ad(ad_latent_lock_);
        {
            boost::shared_lock<boost::shared_mutex> lock_feature(ad_feature_lock_);
            getAdLatentVecKeys(ad_docid, ad_keys);
            for(size_t i = 0; i < ad_keys.size(); ++i)
            {
                std::pair<LatentVecContainerT::iterator, bool> it_pair = ad_latent_vec_list_.insert(std::make_pair(ad_keys[i], default_latent_));
                if (it_pair.second)
                {
                    // a new feature value
                    LatentVecT new_vec(LATENT_VEC_DIM, 0);
                    for(std::size_t j = 0; j < LATENT_VEC_DIM; ++j)
                    {
                        new_vec[j] = random() % 10;
                    }
                    it_pair.first->second.swap(new_vec);
                }

                ad_feature_latent_list.push_back(&(it_pair.first->second));
                unviewed_items_.reset(ad_feature_value_id_list_[ad_keys[i]]);
            }
        }
    }
    std::vector<LatentVecT> tmp_ad_feature_latent_list(ad_feature_latent_list.size());
    for(std::size_t i = 0; i < ad_feature_latent_list.size(); ++i)
    {
        tmp_ad_feature_latent_list[i] = *(ad_feature_latent_list[i]);
    }

    double gradient = learning_rate_;
    double new_max_norm = 0;
    if (!is_clicked)
        gradient = ratio_ * learning_rate_;
    for (size_t i = 0; i < tmp_ad_feature_latent_list.size(); ++i)
    {
        LatentVecT& ad_latent = tmp_ad_feature_latent_list[i];
        LatentVecT combined_user_latent(LATENT_VEC_DIM, 0);
        assert(ad_latent.size() == user_latent.size());
        for (size_t k = 0; k < ad_latent.size(); ++k)
        {
            getCombinedUserLatentVec(tmp_user_feature_latent_list, combined_user_latent, k);
            ad_latent[k] += gradient * combined_user_latent[k];
            new_max_norm = std::max(new_max_norm, std::fabs(ad_latent[k]));
            for (size_t j = 0; j < tmp_user_feature_latent_list.size(); ++j)
            {
                LatentVecT& user_latent = tmp_user_feature_latent_list[j];
                //user_latent[k] += gradient*ad_latent[k]*(combined_user_latent[k]/user_latent[k]);
                user_latent[k] += gradient*ad_latent[k];
                new_max_norm = std::max(new_max_norm, std::fabs(user_latent[k]));
            }
        }
    }
    {
        boost::unique_lock<boost::shared_mutex> lock_user(user_latent_lock_);
        for(std::size_t i = 0; i < user_feature_latent_list.size(); ++i)
        {
            tmp_user_feature_latent_list[i].swap(*(user_feature_latent_list[i]));
        }
        if (new_max_norm > MAX_NORM)
        {
            // scale the vectors to obey the norm constrain.
            double scale = MAX_NORM/new_max_norm;
            for (LatentVecContainerT::iterator it = user_feature_latent_vec_list_.begin();
                it != user_feature_latent_vec_list_.end(); ++it)
            {
                for (size_t j = 0; j < it->second.size(); ++j)
                {
                    it->second[j] *= scale;
                }
            }
        }
    }
    {
        boost::unique_lock<boost::shared_mutex> lock_ad(ad_latent_lock_);
        for(std::size_t i = 0; i < ad_feature_latent_list.size(); ++i)
        {
            tmp_ad_feature_latent_list[i].swap(*(ad_feature_latent_list[i]));
        }
        if (new_max_norm > MAX_NORM)
        {
            // scale the vectors to obey the norm constrain.
            double scale = MAX_NORM/new_max_norm;
            for (LatentVecContainerT::iterator it = ad_latent_vec_list_.begin();
                it != ad_latent_vec_list_.end(); ++it)
            {
                for(size_t j = 0; j < it->second.size(); ++j)
                {
                    it->second[j] *= scale;
                }
            }
        }
    }
    if (is_clicked)
    {
        //LOG(INFO) << "clicked ad : " << ad_docid << ", user: " << user_str_id;
    }
}

void AdRecommender::dumpUserLatent()
{
    LOG(INFO) << "ad feature value size: " << ad_feature_value_list_.size()
        << ", " << ad_feature_value_id_list_.size() << ", " << ad_latent_vec_list_.size();
    LOG(INFO) << "currently unviewed_items : " << unviewed_items_.count();
    std::ofstream ofs(std::string(data_path_ + "/dumped_userlatent.txt").c_str());
    for (LatentVecContainerT::const_iterator it = user_feature_latent_vec_list_.begin();
        it != user_feature_latent_vec_list_.end(); ++it)
    {
        ofs << "key: " << it->first << ", latent vector: ";
        for (size_t i = 0; i < it->second.size(); ++i)
        {
            ofs << it->second[i] << ",";
        }
        ofs << std::endl;
    }
    ofs << "dumped ad latent: " << std::endl;
    for (LatentVecContainerT::const_iterator it = ad_latent_vec_list_.begin();
        it != ad_latent_vec_list_.end(); ++it)
    {
        ofs << "key: " << it->first << ", latent vector: ";
        for (size_t i = 0; i < it->second.size(); ++i)
        {
            ofs << it->second[i] << ",";
        }
        ofs << std::endl;
    }

    ofs.flush();
}

}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file ored/portfolio/creditdefaultswapoption.hpp
    \brief credit default swap option trade data model and serialization
    \ingroup tradedata
 */

#pragma once

#include <ored/portfolio/creditdefaultswap.hpp>
#include <ored/portfolio/optiondata.hpp>
#include <boost/optional/optional.hpp>

/*! Serializable Credit Default Swap Option
    \ingroup tradedata
*/
namespace ore {
namespace data {

class CreditDefaultSwapOption : public Trade {
public:
    /*! Hold information on a default that has occured and for which an auction has been held.

        If the CDS option has knockout set to false, a default payment will be made on expiry of the option in the
        event of a default. Also, if knockout is set to true, we would still need to know this amount between the
        auction date and the auction settlement date, typically 3 business days, to assign a value to the option
        trade. Between the default date and the auction date, the recovery rate still trades so there should be enough
        information in the market data to price the trade using the CDS option engine.
    */
    class AuctionSettlementInformation : public XMLSerializable {
    public:
        //! Default constructor
        AuctionSettlementInformation();

        //! Detailed constructor
        AuctionSettlementInformation(const QuantLib::Date& auctionSettlementDate,
            QuantLib::Real& auctionFinalPrice);

        //! \name Inspectors
        //@{
        const QuantLib::Date& auctionSettlementDate() const;
        QuantLib::Real auctionFinalPrice() const;
        //@}

        //! \name Serialisation
        //@{
        void fromXML(XMLNode* node) override;
        XMLNode* toXML(XMLDocument& doc) override;
        //@}

    private:
        QuantLib::Date auctionSettlementDate_;
        QuantLib::Real auctionFinalPrice_;
    };

    //! Default constructor
    CreditDefaultSwapOption();

    //! Constructor
    CreditDefaultSwapOption(const Envelope& env,
        const OptionData& option,
        const CreditDefaultSwapData& swap,
        QuantLib::Real strike = QuantLib::Null<QuantLib::Real>(),
        const std::string& strikeType = "Spread",
        bool knockOut = true,
        const std::string& term = "",
        const boost::optional<AuctionSettlementInformation>& asi = boost::none);

    //! \name Trade interface
    //@{
    void build(const boost::shared_ptr<EngineFactory>& ef) override;
    //@}

    //! \name Inspectors
    //@{
    const OptionData& option() const;
    const CreditDefaultSwapData& swap() const;
    QuantLib::Real strike() const;
    const std::string& strikeType() const;
    bool knockOut() const;
    const std::string& term() const;
    const boost::optional<AuctionSettlementInformation>& auctionSettlementInformation() const;
    //@}

    //! \name Serialisation
    //@{
    void fromXML(XMLNode* node) override;
    XMLNode* toXML(XMLDocument& doc) override;
    //@}

private:
    OptionData option_;
    CreditDefaultSwapData swap_;
    QuantLib::Real strike_;
    std::string strikeType_;
    bool knockOut_;
    std::string term_;
    boost::optional<AuctionSettlementInformation> asi_;

    //! Build CDS option given that no default
    void buildNoDefault(const boost::shared_ptr<EngineFactory>& ef);

    //! Build instrument given that default has occured
    void buildDefaulted(const boost::shared_ptr<EngineFactory>& ef);

    //! Add the premium payment
    void addPremium(const boost::shared_ptr<EngineFactory>& ef,
        const QuantLib::Currency& tradeCurrency,
        const std::string& marketConfig,
        std::vector<boost::shared_ptr<QuantLib::Instrument>>& additionalInstruments,
        std::vector<QuantLib::Real>& additionalMultipliers);
};

}
}


#include "Kriging_binding.hpp"

#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

#include <armadillo>
#include <carma>
#include <libKriging/Kriging.hpp>
#include <random>

PyKriging::PyKriging(const std::string& kernel) : m_internal{new Kriging{kernel}} {}

PyKriging::~PyKriging() {}

void PyKriging::fit(const py::array_t<double>& y,
                    const py::array_t<double>& X,
                    const Kriging::RegressionModel& regmodel,
                    bool normalize,
                    const std::string& optim,
                    const std::string& objective,
                    const Kriging::Parameters& parameters) {
  arma::mat mat_y = carma::arr_to_col<double>(y);
  arma::mat mat_X = carma::arr_to_mat<double>(X);
  m_internal->fit(mat_y, mat_X, regmodel, normalize, optim, objective, parameters);
}

std::tuple<py::array_t<double>, py::array_t<double>, py::array_t<double>>
PyKriging::predict(const py::array_t<double>& X, bool withStd, bool withCov) {
  arma::mat mat_X = carma::arr_to_mat<double>(X);
  auto [y_predict, y_stderr, y_cov] = m_internal->predict(mat_X, withStd, withCov);
  return std::make_tuple(
      carma::col_to_arr(y_predict, true), carma::col_to_arr(y_stderr, true), carma::mat_to_arr(y_cov, true));
}

py::array_t<double> PyKriging::simulate(const int nsim, const int seed, const py::array_t<double>& Xp) {
  arma::mat mat_X = carma::arr_to_mat<double>(Xp);
  auto result = m_internal->simulate(nsim, seed, mat_X);
  return carma::mat_to_arr(result, true);
}

void PyKriging::update(const py::array_t<double>& newy, const py::array_t<double>& newX, bool normalize) {
  arma::mat mat_y = carma::arr_to_col<double>(newy);
  arma::mat mat_X = carma::arr_to_mat<double>(newX);
  m_internal->update(mat_y, mat_X, normalize);
}

std::string PyKriging::describeModel() const {
  return m_internal->describeModel();
}

std::tuple<double, py::array_t<double>> PyKriging::leaveOneOutEval(const py::array_t<double>& theta,
                                                                   const bool want_grad) {
  arma::vec vec_theta = carma::arr_to_col<double>(theta);
  auto [llo, grad] = m_internal->leaveOneOutEval(vec_theta, want_grad);
  return {llo, carma::col_to_arr(grad)};
}

std::tuple<double, py::array_t<double>, py::array_t<double>>
PyKriging::logLikelihoodEval(const py::array_t<double>& theta, const bool want_grad, const bool want_hess) {
  arma::vec vec_theta = carma::arr_to_col<double>(theta);
  auto [llo, grad, hess] = m_internal->logLikelihoodEval(vec_theta, want_grad, want_hess);
  return {
      llo,
      carma::col_to_arr(grad),
      // carma::mat_to_arr(hess)  // FIXME error in hessian transmission
      {}  //
  };
}

std::tuple<double, py::array_t<double>> PyKriging::logMargPostEval(const py::array_t<double>& theta,
                                                                   const bool want_grad) {
  arma::vec vec_theta = carma::arr_to_col<double>(theta);
  auto [lmp, grad] = m_internal->logMargPostEval(vec_theta, want_grad);
  return {lmp, carma::col_to_arr(grad)};
}


#pragma once

#include <vector>
#include <array>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace vgl {
struct Vertex {
    Eigen::Vector4f pos{};
    Eigen::Vector4f normal{};
    Eigen::Vector4f tangent{};
    Eigen::Vector4f uv{};
};

inline bool operator==(const Vertex& v, const Eigen::Vector4f const& p) { return v.pos == p; }

struct Bounds;
Bounds calc_bounds(const std::vector<Vertex>& vertices);

struct Bounds {
    Eigen::Vector4f min = Eigen::Vector4f::Constant(std::numeric_limits<float>::max());
    Eigen::Vector4f max = Eigen::Vector4f::Constant(std::numeric_limits<float>::lowest());

    inline void join(const Bounds& b) {
        min = min.cwiseMin(b.min);
        max = max.cwiseMax(b.max);
    }

    inline void join(const std::vector<Vertex>& vertices) {
        Bounds b = calc_bounds(vertices);
        min = min.cwiseMin(b.min);
        max = max.cwiseMax(b.max);
    }
};

inline Bounds calc_bounds(const std::vector<Vertex>& vertices) {
    Bounds bounds{};
    for (auto& v : vertices) {
        bounds.min = bounds.min.cwiseMin(v.pos);
        bounds.max = bounds.max.cwiseMax(v.pos);
    }
    return bounds;
}
namespace geo {
inline std::array<Eigen::Vector3f, 14> unit_cube() {
    return {Eigen::Vector3f(-0.5, 0.5, 0.5),  Eigen::Vector3f(0.5, 0.5, 0.5),    Eigen::Vector3f(-0.5, -0.5, 0.5),
            Eigen::Vector3f(0.5, -0.5, 0.5),  Eigen::Vector3f(0.5, -0.5, -0.5),  Eigen::Vector3f(0.5, 0.5, 0.5),
            Eigen::Vector3f(0.5, 0.5, -0.5),  Eigen::Vector3f(-0.5, 0.5, 0.5),   Eigen::Vector3f(-0.5, 0.5, -0.5),
            Eigen::Vector3f(-0.5, -0.5, 0.5), Eigen::Vector3f(-0.5, -0.5, -0.5), Eigen::Vector3f(0.5, -0.5, -0.5),
            Eigen::Vector3f(-0.5, 0.5, -0.5), Eigen::Vector3f(0.5, 0.5, -0.5)};
}
} // namespace geo
} // namespace vgl


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generates constant log10_ehi.This constant is coupled with Log10_elo and is
    used in the float logarithms computations
    We have double(log10_ehi<float>())+double(Log10_elo<float>()) == Log_10(e)

    @par Semantic:

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

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct log10_ehi_ : boost::dispatch::constant_value_<log10_ehi_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,log10_ehi_,boost::dispatch::constant_value_<log10_ehi_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0x3ede0000UL, 0x3fdbcb7b1526e510ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, log10_ehi_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::log10_ehi_,log10_ehi);
  }

  template<typename T> BOOST_FORCEINLINE auto Log10_ehi()
  BOOST_NOEXCEPT_DECLTYPE(detail::log10_ehi( boost::dispatch::as_<T>{}))
  {
    return detail::log10_ehi( boost::dispatch::as_<T>{} );
  }
} }

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

#endif


/**
 * @file stability.cpp
 *
 */

#include "stability.h"
#include "forecast_time.h"
#include "level.h"
#include "logger.h"
#include "metutil.h"
#include "plugin_factory.h"
#include <algorithm>  // for std::transform
#include <boost/thread.hpp>
#include <functional>  // for std::plus

#include "fetcher.h"
#include "hitool.h"

using namespace std;
using namespace himan;
using namespace himan::plugin;

vector<double> Shear(shared_ptr<const plugin_configuration> conf, const forecast_time& ftime, const param& wantedParam,
                     double lowerHeight, double upperHeight);

#ifdef DEBUG
void DumpVector(const vector<double>& vec);
#endif

// Required source and target parameters and levels

const param TParam("T-K");
const param TDParam("TD-K");
const param HParam("Z-M2S2");
const params PParam({param("P-HPA"), param("P-PA")});
const param KIParam("KINDEX-N", 80, 0, 7, 2);
const param VTIParam("VTI-N", 4754);
const param CTIParam("CTI-N", 4751);
const param TTIParam("TTI-N", 4755, 0, 7, 4);
const param SIParam("SI-N", 4750, 0, 7, 13);
const param LIParam("LI-N", 4751, 0, 7, 192);
const param BS01Param("WSH-1-KT", 4771);  // knots!
const param BS06Param("WSH-KT", 4770);    // knots!
const param SRH01Param("HLCY-1-M2S2", 4773);
const param SRH03Param("HLCY-M2S2", 4772, 0, 7, 8);

const level P850Level(himan::kPressure, 850, "PRESSURE");
const level P700Level(himan::kPressure, 700, "PRESSURE");
const level P500Level(himan::kPressure, 500, "PRESSURE");
level groundLevel(himan::kHeight, 0, "HEIGHT");

void T500mSearch(shared_ptr<const plugin_configuration> conf, const forecast_time& ftime, vector<double>& result);
void TD500mSearch(shared_ptr<const plugin_configuration> conf, const forecast_time& ftime, vector<double>& result);

stability::stability() : itsLICalculation(false), itsBSCalculation(false), itsSRHCalculation(false)
{
	itsLogger = logger("stability");
}

void stability::Process(std::shared_ptr<const plugin_configuration> conf)
{
	Init(conf);

	vector<param> theParams;

	// Kindex
	theParams.push_back(KIParam);

	// Cross totals index
	theParams.push_back(CTIParam);

	// Vertical Totals index
	theParams.push_back(VTIParam);

	// Total Totals index
	theParams.push_back(TTIParam);

	if (itsConfiguration->Exists("li") && itsConfiguration->GetValue("li") == "true")
	{
		// Lifted index

		itsLICalculation = true;
		theParams.push_back(LIParam);

		// Showalter Index
		theParams.push_back(SIParam);
	}

	if (itsConfiguration->Exists("bs") && itsConfiguration->GetValue("bs") == "true")
	{
		itsBSCalculation = true;

		// Bulk shear 0 .. 1 km
		theParams.push_back(BS01Param);

		// Bulk shear 0 .. 6 km

		theParams.push_back(BS06Param);
	}

	if (itsConfiguration->Exists("srh") && itsConfiguration->GetValue("srh") == "true")
	{
		// Storm relative helicity 0 .. 1 km
		theParams.push_back(SRH01Param);

		// Storm relative helicity 0 .. 3 km
		theParams.push_back(SRH03Param);
	}

	SetParams(theParams);

	Start();
}

/*
 * Calculate()
 *
 * This function does the actual calculation.
 */

void stability::Calculate(shared_ptr<info> myTargetInfo, unsigned short theThreadIndex)
{
	vector<double> T500mVector, TD500mVector, P500mVector, U01Vector, V01Vector, U06Vector, V06Vector, UidVector,
	    VidVector;

	auto myThreadedLogger = logger("stabilityThread #" + to_string(theThreadIndex));

	forecast_time forecastTime = myTargetInfo->Time();
	level forecastLevel = myTargetInfo->Level();

	info_t T850Info, T700Info, T500Info, TD850Info, TD700Info;

	myThreadedLogger.Info("Calculating time " + static_cast<string>(forecastTime.ValidDateTime()) + " level " +
	                      static_cast<string>(forecastLevel));

	bool LICalculation = itsLICalculation;
	bool BSCalculation = itsBSCalculation;
	bool SRHCalculation = itsSRHCalculation;

	if (!GetSourceData(T850Info, T700Info, T500Info, TD850Info, TD700Info, myTargetInfo,
	                   itsConfiguration->UseCudaForPacking()))
	{
		myThreadedLogger.Warning("Skipping step " + to_string(forecastTime.Step()) + ", level " +
		                         static_cast<string>(forecastLevel));
		return;
	}

	if (LICalculation)
	{
		if (!GetLISourceData(myTargetInfo, T500mVector, TD500mVector, P500mVector))
		{
			myThreadedLogger.Warning("Source data not found for param LI");
			LICalculation = false;
		}
	}

	if (BSCalculation)
	{
		if (!GetWindShearSourceData(myTargetInfo, U01Vector, V01Vector, U06Vector, V06Vector))
		{
			myThreadedLogger.Warning("Source data not found for param BulkShear");
			BSCalculation = false;
		}
	}

	if (SRHCalculation)
	{
		if (!GetSRHSourceData(myTargetInfo, UidVector, VidVector))
		{
			myThreadedLogger.Warning("Source data not found for param SRH");
			SRHCalculation = false;
		}
	}

	string deviceType = "CPU";

#ifdef HAVE_CUDA

	if (itsConfiguration->UseCuda())
	{
		deviceType = "GPU";

		unique_ptr<stability_cuda::options> opts(new stability_cuda::options);

		opts->t500 = T500Info->ToSimple();
		opts->t700 = T700Info->ToSimple();
		opts->t850 = T850Info->ToSimple();
		opts->td700 = TD700Info->ToSimple();
		opts->td850 = TD850Info->ToSimple();

		myTargetInfo->Param(param("KINDEX-N"));
		opts->ki = myTargetInfo->ToSimple();

		myTargetInfo->Param(param("VTI-N"));
		opts->vti = myTargetInfo->ToSimple();

		myTargetInfo->Param(param("CTI-N"));
		opts->cti = myTargetInfo->ToSimple();

		myTargetInfo->Param(param("TTI-N"));
		opts->tti = myTargetInfo->ToSimple();

		if (LICalculation)
		{
			opts->t500m = &T500mVector[0];
			opts->td500m = &TD500mVector[0];
			opts->p500m = &P500mVector[0];

			myTargetInfo->Param(param("LI-N"));
			opts->li = myTargetInfo->ToSimple();

			myTargetInfo->Param(param("SI-N"));
			opts->si = myTargetInfo->ToSimple();
		}

		if (BSCalculation)
		{
			opts->u01 = &U01Vector[0];
			opts->v01 = &V01Vector[0];
			opts->u06 = &U06Vector[0];
			opts->v06 = &V06Vector[0];

			myTargetInfo->Param(BS01Param);
			opts->bs01 = myTargetInfo->ToSimple();
			myTargetInfo->Param(BS06Param);
			opts->bs06 = myTargetInfo->ToSimple();
		}

		opts->N = opts->t500->size_x * opts->t500->size_y;

		stability_cuda::Process(*opts);
	}
	else
#endif
	{
		LOCKSTEP(myTargetInfo, T850Info, T700Info, T500Info, TD850Info, TD700Info)
		{
			double T850 = T850Info->Value();
			double T700 = T700Info->Value();
			double T500 = T500Info->Value();
			double TD850 = TD850Info->Value();
			double TD700 = TD700Info->Value();

			assert(T850 > 0);
			assert(T700 > 0);
			assert(T500 > 0);
			assert(TD850 > 0);
			assert(TD700 > 0);

			double value = kFloatMissing;

			if (T850 == kFloatMissing || T700 == kFloatMissing || T500 == kFloatMissing || TD850 == kFloatMissing ||
			    TD700 == kFloatMissing)
			{
				continue;
			}

			value = metutil::KI_(T850, T700, T500, TD850, TD700);
			myTargetInfo->Param(KIParam);
			myTargetInfo->Value(value);

			value = metutil::CTI_(T500, TD850);
			myTargetInfo->Param(CTIParam);
			myTargetInfo->Value(value);

			value = metutil::VTI_(T850, T500);
			myTargetInfo->Param(VTIParam);
			myTargetInfo->Value(value);

			value = metutil::TTI_(T850, T500, TD850);
			myTargetInfo->Param(TTIParam);
			myTargetInfo->Value(value);

			if (LICalculation)
			{
				size_t locationIndex = myTargetInfo->LocationIndex();

				double T500m = T500mVector[locationIndex];
				double TD500m = TD500mVector[locationIndex];
				double P500m = P500mVector[locationIndex];

				assert(T500m != kFloatMissing);
				assert(TD500m != kFloatMissing);
				assert(P500m != kFloatMissing);

				if (T500m != kFloatMissing && TD500m != kFloatMissing && P500m != kFloatMissing)
				{
					value = metutil::LI_(T500, T500m, TD500m, P500m);

					myTargetInfo->Param(LIParam);
					myTargetInfo->Value(value);
				}

				value = metutil::SI_(T850, T500, TD850);
				myTargetInfo->Param(SIParam);
				myTargetInfo->Value(value);
			}

			if (BSCalculation)
			{
				size_t locationIndex = myTargetInfo->LocationIndex();

				double U01 = U01Vector[locationIndex];
				double V01 = V01Vector[locationIndex];
				double U06 = U06Vector[locationIndex];
				double V06 = V06Vector[locationIndex];

				assert(U01 != kFloatMissing);
				assert(V01 != kFloatMissing);

				assert(U06 != kFloatMissing);
				assert(V06 != kFloatMissing);

				if (U01 != kFloatMissing && V01 != kFloatMissing)
				{
					value = metutil::BulkShear_(U01, V01);

					myTargetInfo->Param(BS01Param);
					myTargetInfo->Value(value);
				}

				if (U06 != kFloatMissing && V01 != kFloatMissing)
				{
					value = metutil::BulkShear_(U06, V06);

					myTargetInfo->Param(BS06Param);
					myTargetInfo->Value(value);
				}
			}

			if (SRHCalculation)
			{
				size_t locationIndex = myTargetInfo->LocationIndex();

				double Uid = UidVector[locationIndex];
				double Vid = VidVector[locationIndex];

				assert(Uid != kFloatMissing);
				assert(Vid != kFloatMissing);

				if (Uid != kFloatMissing && Vid != kFloatMissing)
				{
				}
			}
		}
	}

	myThreadedLogger.Info("[" + deviceType + "] Missing values: " + to_string(myTargetInfo->Data().MissingCount()) +
	                      "/" + to_string(myTargetInfo->Data().Size()));
}

void T500mSearch(shared_ptr<const plugin_configuration> conf, const forecast_time& ftime, vector<double>& result)
{
	auto h = dynamic_pointer_cast<hitool>(plugin_factory::Instance()->Plugin("hitool"));

	h->Configuration(conf);
	h->Time(ftime);

	result = h->VerticalAverage(param("T-K"), 0, 500);

#ifdef DEBUG
	for (size_t i = 0; i < result.size(); i++)
	{
		assert(result[i] != kFloatMissing);
	}
#endif
}

void TD500mSearch(shared_ptr<const plugin_configuration> conf, const forecast_time& ftime, vector<double>& result)
{
	auto h = dynamic_pointer_cast<hitool>(plugin_factory::Instance()->Plugin("hitool"));

	h->Configuration(conf);
	h->Time(ftime);

	result = h->VerticalAverage(param("TD-K"), 0, 500);
}
#if 0
inline
double stability::StormRelativeHelicity(double UID, double VID, double U_lower, double U_higher, double V_lower, double V_higher)
{
	return ((UID - U_lower) * (V_lower - V_higher)) - ((VID - V_lower) * (U_lower - U_higher));
}
#endif

bool stability::GetSourceData(shared_ptr<info>& T850Info, shared_ptr<info>& T700Info, shared_ptr<info>& T500Info,
                              shared_ptr<info>& TD850Info, shared_ptr<info>& TD700Info,
                              const shared_ptr<info>& myTargetInfo, bool useCudaInThisThread)
{
	bool ret = true;

	if (!T850Info)
	{
		T850Info = Fetch(myTargetInfo->Time(), P850Level, TParam, myTargetInfo->ForecastType(), useCudaInThisThread);
	}

	if (!T700Info)
	{
		T700Info = Fetch(myTargetInfo->Time(), P700Level, TParam, myTargetInfo->ForecastType(), useCudaInThisThread);
	}

	if (!T500Info)
	{
		T500Info = Fetch(myTargetInfo->Time(), P500Level, TParam, myTargetInfo->ForecastType(), useCudaInThisThread);
	}

	if (!TD850Info)
	{
		TD850Info = Fetch(myTargetInfo->Time(), P850Level, TDParam, myTargetInfo->ForecastType(), useCudaInThisThread);
	}

	if (!TD700Info)
	{
		TD700Info = Fetch(myTargetInfo->Time(), P700Level, TDParam, myTargetInfo->ForecastType(), useCudaInThisThread);
	}

	if (!T850Info || !T700Info || !T500Info || !TD850Info || !TD700Info)
	{
		ret = false;
	}

	return ret;
}

bool stability::GetLISourceData(const shared_ptr<info>& myTargetInfo, vector<double>& T500mVector,
                                vector<double>& TD500mVector, vector<double>& P500mVector)
{
	auto h = dynamic_pointer_cast<hitool>(plugin_factory::Instance()->Plugin("hitool"));

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());

// Fetch Z uncompressed since it is not transferred to cuda
#if 0
	auto HInfo = Fetch(myTargetInfo->Time(), groundLevel, HParam, false);

	if (!HInfo)
	{
		return false;
	}

	vector<double> H0mVector = HInfo->Grid()->Data().Values();
	vector<double> H500mVector(HInfo->SizeLocations());

	for (size_t i = 0; i < H500mVector.size(); i++)
	{
		// H0mVector contains the height of ground (compared to MSL). Height can be negative
		// (maybe even in real life (Netherlands?)), but in our case we use 0 as smallest height.
		// TODO: check how it is in smarttools

		if (H0mVector[i] == kFloatMissing)
		{
			continue;
		}

		H0mVector[i] *= constants::kIg;
		H0mVector[i] = fmax(0, H0mVector[i]);

		H500mVector[i] = H0mVector[i] + 500.;
	}

#endif

	// Fetch average values of T, TD and P over vertical height range 0 ... 500m OVER GROUND

	boost::thread t1(&T500mSearch, itsConfiguration, myTargetInfo->Time(), boost::ref(T500mVector));
	boost::thread t2(&TD500mSearch, itsConfiguration, myTargetInfo->Time(), boost::ref(TD500mVector));

	P500mVector = h->VerticalAverage(PParam, 0., 500.);

	assert(P500mVector[0] != kFloatMissing);

	if (P500mVector[0] < 1500)
	{
		transform(P500mVector.begin(), P500mVector.end(), P500mVector.begin(),
		          bind1st(multiplies<double>(), 100));  // hPa to Pa
	}

	t1.join();
	t2.join();

	return true;
}

bool stability::GetWindShearSourceData(const shared_ptr<info>& myTargetInfo, vector<double>& U01Vector,
                                       vector<double>& V01Vector, vector<double>& U06Vector, vector<double>& V06Vector)
{
	auto h = dynamic_pointer_cast<hitool>(plugin_factory::Instance()->Plugin("hitool"));

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());

	// BS 0-6
	U06Vector = Shear(itsConfiguration, myTargetInfo->Time(), param("U-MS"), 0, 6000);
	V06Vector = Shear(itsConfiguration, myTargetInfo->Time(), param("V-MS"), 0, 6000);

#ifdef DEBUG
	DumpVector(U06Vector);
	DumpVector(V06Vector);
#endif

	// BS 0-1

	U01Vector = Shear(itsConfiguration, myTargetInfo->Time(), param("U-MS"), 0, 1000);
	V01Vector = Shear(itsConfiguration, myTargetInfo->Time(), param("V-MS"), 0, 1000);

#ifdef DEBUG
	DumpVector(U01Vector);
	DumpVector(V01Vector);
#endif

	return true;
}

vector<double> Shear(shared_ptr<const plugin_configuration> conf, const forecast_time& ftime, const param& wantedParam,
                     double lowerHeight, double upperHeight)
{
	auto h = dynamic_pointer_cast<hitool>(plugin_factory::Instance()->Plugin("hitool"));

	auto lowerValues = h->VerticalValue(wantedParam, lowerHeight);
	auto upperValues = h->VerticalValue(wantedParam, upperHeight);

	vector<double> ret(lowerValues.size(), kFloatMissing);

#ifdef YES_WE_HAVE_GCC_WHICH_SUPPORTS_LAMBDAS
	transform(lowerValues.begin(), lowerValues.end(), upperValues.begin(), back_inserter(U),
	          [](double l, double u) { return (u == kFloatMissing || l == kFloatMissing) ? kFloatMissing : u - l; });
	transform(lowerValues.begin(), lowerValues.end(), upperValues.begin(), back_inserter(V),
	          [](double l, double u) { return (u == kFloatMissing || l == kFloatMissing) ? kFloatMissing : u - l; });
#else

	for (size_t i = 0; i < lowerValues.size(); i++)
	{
		double l = lowerValues[i];
		double u = upperValues[i];

		if (u == kFloatMissing || l == kFloatMissing)
		{
			continue;
		}

		ret[i] = u - l;
	}
#endif

	return ret;
}

bool stability::GetSRHSourceData(const shared_ptr<info>& myTargetInfo, vector<double>& Uid, vector<double>& Vid)
{
	// NOTES COPIED FROM SMARTTOOLS-LIBRARY

	/* // **********  SRH calculation help from Pieter Groenemeijer ******************

	Some tips here on how tyo calculate storm-relative helciity

	How to calculate storm-relative helicity

	Integrate the following from p = p_surface to p = p_top (or in case of height coordinates from h_surface to h_top):

	storm_rel_helicity -= ((u_ID-u[p])*(v[p]-v[p+1]))-((v_ID - v[p])*(u[p]-u[p+1]));

	Here, u_ID and v_ID are the forecast storm motion vectors calculated with the so-called ID-method. These can be calculated as follows:

	where

	/average wind
	u0_6 = average 0_6 kilometer u-wind component
	v0_6 = average 0_6 kilometer v-wind component
	(you should use a pressure-weighted average in case you work with height coordinates)

	/shear
	shr_0_6_u = u_6km - u_surface;
	shr_0_6_v = v_6km - v_surface;

	/ shear unit vector
	shr_0_6_u_n = shr_0_6_u / ((shr_0_6_u^2 + shr_0_6_v^2)**0.5);
	shr_0_6_v_n = shr_0_6_v / ((shr_0_6_u^2 + shr_0_6_v^2)** 0.5);

	/id-vector components
	u_ID = u0_6 + shr_0_6_v_n * 7.5;
	v_ID = v0_6 - shr_0_6_u_n * 7.5;

	(7.5 are meters per second... watch out when you work with knots instead)

	*/  // **********  SRH calculation help from Pieter Groenemeijer ******************

	auto h = dynamic_pointer_cast<hitool>(plugin_factory::Instance()->Plugin("hitool"));

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());

	// average wind
	auto Uavg = h->VerticalAverage(param("U-MS"), 0, 6000);
	auto Vavg = h->VerticalAverage(param("V-MS"), 0, 6000);

	// shear
	auto Ushear = Shear(itsConfiguration, myTargetInfo->Time(), param("U-MS"), 0, 6000);
	auto Vshear = Shear(itsConfiguration, myTargetInfo->Time(), param("V-MS"), 0, 6000);

	// shear unit vectors
	Uid.resize(Ushear.size(), kFloatMissing);
	Vid.resize(Vshear.size(), kFloatMissing);

	assert(Uid.size() == Vid.size());
	assert(Uid.size() == Uavg.size());

	for (size_t i = 0; i < Ushear.size(); i++)
	{
		double u = Ushear[i];
		double v = Vshear[i];

		if (u == kFloatMissing || v == kFloatMissing)
		{
			continue;
		}

		double Uunit = u / sqrt(u * u + v * v);
		double Vunit = v / sqrt(u * u + v * v);

		Uid[i] = Uavg[i] - Vunit * 7.5;
		Vid[i] = Vavg[i] - Uunit * 7.5;
	}

	return true;
}

#ifdef DEBUG
void DumpVector(const vector<double>& vec)
{
	double min = 1e38, max = -1e38, sum = 0;
	size_t count = 0, missing = 0;

	for (double val : vec)
	{
		if (val == kFloatMissing)
		{
			missing++;
			continue;
		}

		min = (val < min) ? val : min;
		max = (val > max) ? val : max;
		count++;
		sum += val;
	}

	double mean = numeric_limits<double>::quiet_NaN();

	if (count > 0)
	{
		mean = sum / static_cast<double>(count);
	}

	cout << "min " << min << " max " << max << " mean " << mean << " count " << count << " missing " << missing << endl;
}

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2015 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::RowMajor;

static void test_tanh()
{
  Tensor<float, 1> vec1(6);
  vec1.setRandom();

  Tensor<float, 1> vec2 = vec1.tanh();

  for (int i = 0; i < 6; ++i) {
    VERIFY_IS_APPROX(vec2(i), tanhf(vec1(i)));
  }
}

static void test_sigmoid()
{
  Tensor<float, 1> vec1(6);
  vec1.setRandom();

  Tensor<float, 1> vec2 = vec1.sigmoid();

  for (int i = 0; i < 6; ++i) {
    VERIFY_IS_APPROX(vec2(i), 1.0f / (1.0f + std::exp(-vec1(i))));
  }
}

void test_cxx11_tensor_math()
{
  CALL_SUBTEST(test_tanh());
  CALL_SUBTEST(test_sigmoid());
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <iostream>
#include <cmath>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{

namespace unit_tests
{

Eigen::Matrix3d getTestRotationMatrix(
        const double psiPerturbation,
        const double thetaPerturbation,
        const double phiPerturbation )
{
    return ( Eigen::AngleAxisd( 1.2 + psiPerturbation, Eigen::Vector3d::UnitZ( ) ) *
             Eigen::AngleAxisd( 0.3 + thetaPerturbation, Eigen::Vector3d::UnitX( ) ) *
             Eigen::AngleAxisd( -0.4 + phiPerturbation, Eigen::Vector3d::UnitZ( ) ) ).toRotationMatrix( );
}

Eigen::Quaterniond perturbQuaternionEntry(
        const Eigen::Quaterniond originalQuaternion,
        const int index,
        const double perturbation )
{
    double perturbedValue, newq0;
    Eigen::Quaterniond newQuaternion;
    if( index == 1 )
    {
        perturbedValue = originalQuaternion.x( ) + perturbation;
        newq0 = std::sqrt( 1.0 - perturbedValue * perturbedValue -
                           originalQuaternion.y( ) * originalQuaternion.y( ) -
                           originalQuaternion.z( ) * originalQuaternion.z( ) );
        newQuaternion = Eigen::Quaterniond( newq0, perturbedValue, originalQuaternion.y( ), originalQuaternion.z( ) );

    }
    else if( index == 2 )
    {
        perturbedValue = originalQuaternion.y( ) + perturbation;
        newq0 = std::sqrt( 1.0 - perturbedValue * perturbedValue -
                           originalQuaternion.x( ) * originalQuaternion.x( ) -
                           originalQuaternion.z( ) * originalQuaternion.z( ) );
        newQuaternion = Eigen::Quaterniond( newq0, originalQuaternion.x( ), perturbedValue, originalQuaternion.z( ) );
    }
    else if( index == 3 )
    {
        perturbedValue = originalQuaternion.z( ) + perturbation;
        newq0 = std::sqrt( 1.0 - perturbedValue * perturbedValue -
                           originalQuaternion.y( ) * originalQuaternion.y( ) -
                           originalQuaternion.x( ) * originalQuaternion.x( ) );
        newQuaternion = Eigen::Quaterniond( newq0, originalQuaternion.x( ), originalQuaternion.y( ), perturbedValue );

    }
    return newQuaternion;
}

Eigen::Matrix3d getTestRotationMatrix(
        const double perturbation,
        const int angleIndex )
{
    if( angleIndex == 0 )
    {
        return getTestRotationMatrix( perturbation, 0.0, 0.0 );
    }
    else if( angleIndex == 1 )
    {
        return getTestRotationMatrix( 0.0, perturbation, 0.0 );
    }
    else if( angleIndex == 2 )
    {
        return getTestRotationMatrix( 0.0, 0.0, perturbation );
    }
    else
    {
        return Eigen::Matrix3d::Zero( );
    }
}

//! Test suite for unit conversion functions.
BOOST_AUTO_TEST_SUITE( test_unit_conversions )

//! Test if quaternion multiplication is computed correctly. The expected results are computed with the
//! built-in MATLAB function quatmultiply.
BOOST_AUTO_TEST_CASE( testQuaternionMultiplication )
{
    // Define tolerance
    double tolerance = 10.0 * std::numeric_limits< double >::epsilon( );

    // Test 1
    {
        // Define quaternions to multiply
        Eigen::Vector4d firstQuaternion;
        firstQuaternion << 1.0, 0.0, 0.0, 0.0;
        Eigen::Vector4d secondQuaternion;
        secondQuaternion << 0.062246819542533, -0.210426307554654, -0.276748204905855, 0.935551459636048;

        // Multiply quaternions
        Eigen::Vector4d computedResultantQuaternion = linear_algebra::quaternionProduct( firstQuaternion,
                                                                                secondQuaternion );

        // Expected result
        Eigen::Vector4d expectedResultantQuaternion;
        expectedResultantQuaternion << 0.062246819542533, -0.210426307554654, -0.276748204905855, 0.935551459636048;

        // Check if result is correct
        for ( unsigned int i = 0; i < 4; i++ )
        {
            BOOST_CHECK_CLOSE_FRACTION( computedResultantQuaternion[ i ], expectedResultantQuaternion[ i ],
                                        tolerance );
        }
    }

    // Test 2
    {
        // Define quaternions to multiply
        Eigen::Vector4d firstQuaternion;
        firstQuaternion << 0.808563085773298, -0.184955787789735, -0.218853569995773, 0.513926266878768;
        Eigen::Vector4d secondQuaternion;
        secondQuaternion << 0.062246819542533, -0.276748204905855, 0.935551459636048, -0.210426307554654;

        // Multiply quaternions
        Eigen::Vector4d computedResultantQuaternion = linear_algebra::quaternionProduct( firstQuaternion,
                                                                                secondQuaternion );

        // Expected result
        Eigen::Vector4d expectedResultantQuaternion;
        expectedResultantQuaternion << 0.312036681781879, -0.670033212581165, 0.561681701127148, -0.371755658840092;

        // Check if result is correct
        for ( unsigned int i = 0; i < 4; i++ )
        {
            BOOST_CHECK_CLOSE_FRACTION( computedResultantQuaternion[ i ], expectedResultantQuaternion[ i ],
                                        tolerance );
        }
    }

    // Test 3
    {
        // Define quaternions to multiply
        Eigen::Vector4d firstQuaternion;
        firstQuaternion << -0.522659045487757, -0.363000140684436, 0.699634559308423, -0.324915225026798;
        Eigen::Vector4d secondQuaternion;
        secondQuaternion << 0.242385025550030, 0.655897240236433, 0.670554074388965, -0.247801418397274;

        // Multiply quaternions
        Eigen::Vector4d computedResultantQuaternion = linear_algebra::quaternionProduct( firstQuaternion,
                                                                                secondQuaternion );

        // Expected result
        Eigen::Vector4d expectedResultantQuaternion;
        expectedResultantQuaternion << -0.438251193562246, -0.386293632078142, -0.483953161080297, -0.651538532273827;

        // Check if result is correct
        for ( unsigned int i = 0; i < 4; i++ )
        {
            BOOST_CHECK_CLOSE_FRACTION( computedResultantQuaternion[ i ], expectedResultantQuaternion[ i ],
                                        tolerance );
        }
    }
}

//! Test if angle between vectors is computed correctly.
BOOST_AUTO_TEST_CASE( testAngleBetweenVectorFunctions )
{
    // Using declarations.
    using std::cos;
    using std::sqrt;
    using linear_algebra::computeAngleBetweenVectors;
    using linear_algebra::computeCosineOfAngleBetweenVectors;

    // Four tests are executed. First, the equality of the caluclated cosineOfAngle and the cosine
    // of the calculated angle is checked. Subsequently, the values of the angle and cosineOfAngle
    // are checked against reference values, which are analytical in the first two cases and
    // taken from Matlab results in the third. The first three tests are written for vectors of length
    // 3. The fourth test is written for a vector of length 5.

    // Test 1: Test values for two equal vectors of length 3.
    {
        Eigen::Vector3d testVector1_ = Eigen::Vector3d( 3.0, 2.1, 4.6 );
        Eigen::Vector3d testVector2_ = Eigen::Vector3d( 3.0, 2.1, 4.6 );

        double angle = computeAngleBetweenVectors( testVector1_, testVector2_ );
        double cosineOfAngle = computeCosineOfAngleBetweenVectors( testVector1_, testVector2_ );

        // Check if computed angle and cosine-of-angle are correct.
        BOOST_CHECK_SMALL( cos( angle ) - cosineOfAngle,
                           std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_GE( cosineOfAngle, std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( cosineOfAngle - 1.0, std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_LE( angle, std::sqrt( std::numeric_limits< double >::epsilon( ) ) );
    }

    // Test 2: Test values for two equal, but opposite vectors of length 3.
    {
        Eigen::Vector3d testVector1_ = Eigen::Vector3d( 3.0, 2.1, 4.6 );
        Eigen::Vector3d testVector2_ = Eigen::Vector3d( -3.0, -2.1, -4.6 );

        double angle = computeAngleBetweenVectors( testVector1_, testVector2_ );
        double cosineOfAngle = computeCosineOfAngleBetweenVectors( testVector1_, testVector2_ );

        // Check if computed angle and cosine-of-angle are correct.
        BOOST_CHECK_SMALL( cos( angle ) - cosineOfAngle,
                           std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK( cosineOfAngle < std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( cosineOfAngle + 1.0, std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( angle, mathematical_constants::PI,
                                    std::sqrt( std::numeric_limits< double >::epsilon( ) ) );
    }

    // Test 3: Test values for two vectors of length 3, benchmark values computed using Matlab.
    {
        Eigen::Vector3d testVector1_ = Eigen::Vector3d( 1.0, 2.0, 3.0 );
        Eigen::Vector3d testVector2_ = Eigen::Vector3d( -3.74, 3.7, -4.6 );

        double angle = computeAngleBetweenVectors( testVector1_, testVector2_ );
        double cosineOfAngle = computeCosineOfAngleBetweenVectors( testVector1_, testVector2_ );

        // Check if computed angle and cosine-of-angle are correct.
        BOOST_CHECK_SMALL( cos( angle ) - cosineOfAngle,
                           std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK( cosineOfAngle < std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( cosineOfAngle, -0.387790156029810,
                                    std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( angle, 1.969029256915446,
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: Test values for two vectors of length 5, benchmark values computed using Matlab.
    {
        Eigen::VectorXd testVector1_( 5 );
        testVector1_ << 3.26, 8.66, 1.09, 4.78, 9.92;
        Eigen::VectorXd testVector2_( 5 );
        testVector2_ << 1.05, 0.23, 9.01, 3.25, 7.74;

        double angle = computeAngleBetweenVectors( testVector1_, testVector2_ );
        double cosineOfAngle = computeCosineOfAngleBetweenVectors( testVector1_, testVector2_ );

        // Check if computed angle and cosine-of-angle are correct.
        BOOST_CHECK_SMALL( cos( angle ) - cosineOfAngle,
                           std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK( cosineOfAngle > std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( cosineOfAngle, 0.603178944723925,
                                    std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( angle, 0.923315587553074, 1.0e-15 );
    }
}

//! Test whether rotation matrix partial derivatives are correctly implemented
BOOST_AUTO_TEST_CASE( testRotationMatrixDerivatives )
{
    using namespace tudat::linear_algebra;
    Eigen::Matrix3d nominalMatrix = getTestRotationMatrix( 0.3, 0.2, 0.6 );
    Eigen::Quaterniond nominalQuaternion = Eigen::Quaterniond( nominalMatrix );
    Eigen::Vector4d nominalQuaternionVector = convertQuaternionToVectorFormat(
                nominalQuaternion );

    std::vector< Eigen::Matrix3d > partialDerivatives;
    partialDerivatives.resize( 4 );
    computePartialDerivativeOfRotationMatrixWrtQuaternion(
                nominalQuaternionVector, partialDerivatives );

    double quaternionPerturbation = 1.0E-7;

    for( unsigned int i = 1; i < 4; i++ )
    {
        Eigen::Quaterniond perturbedQuaternion = perturbQuaternionEntry(
                    nominalQuaternion, i, quaternionPerturbation );
        Eigen::Vector4d perturbedQuaternionVector = convertQuaternionToVectorFormat(
                    perturbedQuaternion );
        Eigen::Matrix3d perturbedRotationMatrix = perturbedQuaternion.toRotationMatrix( );

        Eigen::Matrix3d rotationMatrixChange = perturbedRotationMatrix - nominalMatrix;

        Eigen::Matrix3d computedRotationMatrixChange =
                partialDerivatives[ 0 ] * ( perturbedQuaternionVector[ 0 ] - nominalQuaternionVector[ 0 ] ) +
                partialDerivatives[ i ] * ( perturbedQuaternionVector[ i ] - nominalQuaternionVector[ i ] );

        Eigen::Matrix3d rotationMatrixChangeError = rotationMatrixChange - computedRotationMatrixChange;

        for( unsigned int j = 0; j < 3; j++ )
        {
            for( unsigned int k = 0; k < 3; k++ )
            {
                BOOST_CHECK_SMALL( std::fabs( rotationMatrixChangeError( j, k ) ), 1.0E-6 * quaternionPerturbation );
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include <iostream>
#include <memory>
#include "conex/cone_program.h"
#include "conex/constraint.h"
#include "conex/dense_lmi_constraint.h"
#include "conex/quadratic_cone_constraint.h"
#include "conex/soc_constraint.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {

using DenseMatrix = Eigen::MatrixXd;

int DoMain() {
  int n = 3;
  SolverConfiguration config;
  config.inv_sqrt_mu_max = 10000;

  std::vector<Eigen::MatrixXd> A;
  // 1 x1 x3 x3
  // x1 1
  // x2   1
  // x3     1
  DenseMatrix Wsqrt = Eigen::MatrixXd::Random(n, n);

  Eigen::MatrixXd C(n + 1, n + 1);
  C.setIdentity();
  for (int i = 1; i < n + 1; i++) {
    Eigen::MatrixXd Ai(n + 1, n + 1);
    Ai.setZero();
    Ai.bottomLeftCorner(n, 1) = Wsqrt.col(i - 1);
    Ai.topRightCorner(1, n) = Wsqrt.col(i - 1).transpose();
    A.push_back(Ai);
  }
  DenseLMIConstraint lmi_constraint{n + 1, A, C};

  Eigen::MatrixXd b(n, 1);

  DenseMatrix As(n + 1, n);
  As.setZero();
  As.bottomRightCorner(n, n) = Wsqrt;
  DenseMatrix Cs(n + 1, 1);
  Cs.setConstant(0.000);
  Cs(0) = 1;
  SOCConstraint soc_constraint_with_squareroot(As, Cs);

  QuadraticConstraint quad_constraint_with_squareroot(As, Cs);

  DenseMatrix Q = Wsqrt.transpose() * Wsqrt;
  DenseMatrix Aq(n + 1, n);
  Aq.setZero();
  Aq.bottomRightCorner(n, n).setIdentity();
  QuadraticConstraint quad_constraint(Q, Aq, Cs);

  for (int i = -2; i < 2; i++) {
    b.setConstant(i);
    b += Eigen::VectorXd::Random(n, 1) * .02;

    Program prog1(n);
    prog1.AddConstraint(soc_constraint_with_squareroot);
    DenseMatrix y1(n, 1);
    Solve(b, prog1, config, y1.data());

    Program prog2(n);
    prog2.AddConstraint(lmi_constraint);
    DenseMatrix y2(n, 1);
    Solve(b, prog2, config, y2.data());

    EXPECT_NEAR((y1 - y2).norm(), 0, 1e-4);

    Program prog3(n);
    prog3.AddConstraint(quad_constraint);
    DenseMatrix y3(n, 1);
    Solve(b, prog3, config, y3.data());
    EXPECT_NEAR((y1 - y3).norm(), 0, 8e-6);

    Program prog4(n);
    prog4.AddConstraint(quad_constraint_with_squareroot);
    DenseMatrix y4(n, 1);
    Solve(b, prog4, config, y4.data());
    EXPECT_NEAR((y1 - y4).norm(), 0, 8e-6);
  }

  return 0;
}

GTEST_TEST(Constraints, SOCP) {
  srand(1);
  for (int i = 0; i < 10; i++) {
    DoMain();
  }
}

}  // namespace conex