#pragma once
#include <string>
#include <trajopt_sco/modeling.hpp>
#include <boost/function.hpp>
/*
 * Algorithms for non-convex, constrained optimization
 */

namespace sco {

using std::string;
using std::vector;

enum OptStatus {
  OPT_CONVERGED,
  OPT_SCO_ITERATION_LIMIT, // hit iteration limit before convergence
  OPT_PENALTY_ITERATION_LIMIT,
  OPT_FAILED,
  INVALID
};
static const char* OptStatus_strings[]  = {
  "CONVERGED",
  "SCO_ITERATION_LIMIT",
  "PENALTY_ITERATION_LIMIT",
  "FAILED",
  "INVALID"
};
inline string statusToString(OptStatus status) {
  return OptStatus_strings[status];
}

struct OptResults {
  DblVec x; // solution estimate
  OptStatus status;
  double total_cost;
  vector<double> cost_vals;
  DblVec cnt_viols;
  int n_func_evals, n_qp_solves;
  void clear() {
    x.clear();
    status = INVALID;
    cost_vals.clear();
    cnt_viols.clear();
    n_func_evals = 0;
    n_qp_solves = 0;
  }
  OptResults() {clear();}
};
std::ostream& operator<<(std::ostream& o, const OptResults& r);

class Optimizer {
  /*
   * Solves an optimization problem
   */
public:
  virtual OptStatus optimize() = 0;
  virtual ~Optimizer() {}
  virtual void setProblem(OptProbPtr prob) {prob_ = prob;}
  void initialize(const vector<double>& x);
  vector<double>& x() {return results_.x;}
  OptResults& results() {return results_;}

  typedef boost::function<void(OptProb*, DblVec&)> Callback;
  void addCallback(const Callback& f); // called before each iteration
protected:
  vector<Callback> callbacks_;
  void callCallbacks(DblVec& x);
  OptProbPtr prob_;
  OptResults results_;
};

class BasicTrustRegionSQP : public Optimizer {
  /*
   * Alternates between convexifying objectives and constraints and then solving convex subproblem
   * Uses a merit function to decide whether or not to accept the step
   * merit function = objective + merit_err_coeff * | constraint_error |
   * Note: sometimes the convexified objectives and constraints lead to an infeasible subproblem
   * In that case, you should turn them into penalties and solve that problem
   * (todo: implement penalty-based sqp that gracefully handles infeasible constraints)
   */
public:
  double improve_ratio_threshold_, // minimum ratio true_improve/approx_improve to accept step
         min_trust_box_size_, // if trust region gets any smaller, exit and report convergence
         min_approx_improve_, // if model improves less than this, exit and report convergence
         min_approx_improve_frac_, // if model improves less than this, exit and report convergence
         max_iter_,
         trust_shrink_ratio_, // if improvement is less than improve_ratio_threshold, shrink trust region by this ratio
         trust_expand_ratio_, // see above
         cnt_tolerance_, // after convergence of penalty subproblem, if constraint violation is less than this, we're done
         max_merit_coeff_increases_, // number of times that we jack up penalty coefficient
         merit_coeff_increase_ratio_, // ratio that we increate coeff each time
         max_time_ // not yet implemented
         ;
  double merit_error_coeff_, // initial penalty coefficient
         trust_box_size_ // current size of trust region (component-wise)
         ;

  BasicTrustRegionSQP();
  BasicTrustRegionSQP(OptProbPtr prob);
  void setProblem(OptProbPtr prob);
  OptStatus optimize();
protected:
  void adjustTrustRegion(double ratio);
  void setTrustBoxConstraints(const vector<double>& x);
  void initParameters();
  ModelPtr model_;
};

}


/*

CandyPoker
https://github.com/sweeterthancandy/CandyPoker

MIT License

Copyright (c) 2019 Gerry Candy

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

*/
#include "ps/eval/binary_strategy_description.h"
#include <thread>
#include <numeric>
#include <atomic>
#include <bitset>
#include <fstream>
#include <unordered_map>

#include <boost/format.hpp>
#include <boost/assert.hpp>

#include "app/pretty_printer.h"
#include "ps/base/algorithm.h"
#include "ps/base/board_combination_iterator.h"
#include "ps/base/cards.h"
#include "ps/base/cards.h"
#include "ps/base/frontend.h"
#include "ps/base/holdem_board_decl.h"
#include "ps/base/rank_hasher.h"
#include "ps/base/suit_hasher.h"
#include "ps/base/tree.h"

#include "ps/detail/tree_printer.h"

#include "ps/eval/class_cache.h"
#include "ps/eval/pass_mask_eval.h"
#include "ps/eval/instruction.h"

#include "ps/support/config.h"
#include "ps/support/index_sequence.h"

#include <boost/timer/timer.hpp>

#include <boost/log/trivial.hpp>

#include <Eigen/Dense>

#include "ps/support/command.h"

#include <boost/iterator/indirect_iterator.hpp>
#include "ps/eval/holdem_class_vector_cache.h"

namespace ps{

        struct static_event : binary_strategy_description::event_decl{
                explicit static_event(std::string const& key, Eigen::VectorXd vec):
                        key_{key}, vec_{vec}
                {}
                virtual std::string key()const override{ return key_; }
                virtual void expected_value_given_event(Eigen::VectorXd& out, holdem_class_vector const& cv, double p)const override{
                        for(size_t idx=0;idx!=vec_.size();++idx){
                                out[idx] += vec_[idx] * p;
                        }
                }
                virtual std::string to_string()const override{
                        std::stringstream sstr;
                        sstr << "Static{key=" << key_ << ", vec_=" << vector_to_string(vec_) << "}";
                        return sstr.str();
                }
        private:
                std::string key_;
                Eigen::VectorXd vec_;
        };
        struct eval_event : binary_strategy_description::event_decl{
                enum{ Debug = 0 };
                /*
                        perm is the vector of players who are in the hand, ie for hu eval
                        it will always be {0,1}, whilst for three players it colud be
                        {0,1}, {0,2}, {1,2}, or {1,2,3}.
                 */
                explicit
                eval_event( binary_strategy_description::eval_view* eval,
                            std::string const& key,
                           std::vector<size_t> perm,
                           Eigen::VectorXd const& dead_money,
                           Eigen::VectorXd const& active)
                        :eval_{eval}, key_(key), perm_{perm}, dead_money_{dead_money}, active_{active}
                        ,pot_amt_{active_.sum() + dead_money_.sum()}
                {

                        delta_proto_.resize(dead_money_.size()+1);
                        delta_proto_.fill(0);
                        for(size_t idx=0;idx!=active_.size();++idx){
                                delta_proto_[idx] -= active_[idx];
                                delta_proto_[idx] -= dead_money_[idx];
                        }

                        if( Debug ){
                                std::cout << "perm => " << detail::to_string(perm) << "\n"; // __CandyPrint__(cxx-print-scalar,perm)
                                std::cout << "dead_money_ => " << vector_to_string(dead_money_) << "\n"; // __CandyPrint__(cxx-print-scalar,dead_money_)
                                std::cout << "active_ => " << vector_to_string(active_) << "\n"; // __CandyPrint__(cxx-print-scalar,active_)
                                std::cout << "delta_proto_ => " << vector_to_string(delta_proto_) << "\n"; // __CandyPrint__(cxx-print-scalar,delta_proto_)
                                std::cout << "pot_amt_ => " << pot_amt_ << "\n"; // __CandyPrint__(cxx-print-scalar,pot_amt_)
                        }

                }
                virtual std::string key()const override{ return key_; }
                virtual void expected_value_given_event(Eigen::VectorXd& out, holdem_class_vector const& cv, double p)const override{

                        // short circuit for optimization purposes
                        if( std::fabs(p) < 0.001 )
                                return;

                        #if 0
                        struct stride_view{
                                enum{ MaxSize = 9 };
                                void construct(std::vector<size_t> mask_, holdem_id* first, holdem_id* last){
                                        first_ = first;
                                        last_ = last;
                                }
                        private:
                                std::array<size_t, 9> view_;
                        };
                        #endif

                        /*
                                We have a perm of the players involed in the hold, perhaps P = (1,2).
                                What we need to do it then map this perm, so that
                                        CV[S[P[0]]] < CV[S[P[0]]],
                         */

                        std::array<size_t, 9> re_perm;
                        std::copy(perm_.begin(), perm_.end(), re_perm.begin());
                        auto re_perm_end = re_perm.begin() + perm_.size();
                        std::sort(re_perm.begin(), re_perm_end, [&](size_t l, size_t r){
                                return cv[l] < cv[r];
                        });

                        std::array<holdem_class_id, 9> tmp;
                        for(size_t idx=0;idx!=perm_.size();++idx){
                                tmp[idx] = cv[re_perm[idx]];
                        }

                        support::array_view<holdem_class_id> tmp_view(&tmp[0], perm_.size());
                        auto ev_ptr = eval_->eval_no_perm(tmp_view);
                        auto const& ev = *ev_ptr;

                        out += delta_proto_ * p;

                        size_t ev_idx = 0;
                        for(size_t idx=0;idx!=perm_.size();++idx){
                                out[re_perm[idx]] += pot_amt_ * ev[ev_idx] * p;
                                ++ev_idx;
                        }
                }
                virtual std::string to_string()const override{
                        std::stringstream sstr;
                        sstr << std::fixed;
                        sstr << "Eval  {key=" << key_ << ", perm=" << detail::to_string(perm_)
                                << ", dead=" << vector_to_string(dead_money_)
                                << ", active=" << vector_to_string(active_)
                                << ", proto=" << vector_to_string(delta_proto_)
                                << ", pot=" << pot_amt_
                                << "}";
                        return sstr.str();
                }
        private:
                binary_strategy_description::eval_view* eval_;
                std::string key_;
                std::vector<size_t> perm_;
                Eigen::VectorXd dead_money_; // not used
                Eigen::VectorXd active_; // not used
                Eigen::VectorXd delta_proto_;
                double pot_amt_;
        };

        struct heads_up_description : binary_strategy_description{
                heads_up_description(eval_view* eval, double sb, double bb, double eff)
                        : sb_{sb}, bb_{bb}, eff_{eff}
                {
                        eval_ = eval;

                        Eigen::VectorXd v_f_{2};
                        v_f_(0) = -sb_;
                        v_f_(1) =  sb_;
                        auto n_f_ = std::make_shared<static_event>("f", v_f_);
                        events_.push_back(n_f_);

                        Eigen::VectorXd v_pf{2};
                        v_pf(0) =  bb_;
                        v_pf(1) = -bb_;
                        auto n_pf = std::make_shared<static_event>("pf", v_pf);
                        events_.push_back(n_pf);

                        Eigen::VectorXd dead_money = Eigen::VectorXd::Zero(2);
                        Eigen::VectorXd active{2};
                        active[0] = eff_;
                        active[1] = eff_;
                        auto n_pp = std::make_shared<eval_event>(eval_, "pp", std::vector<size_t>{0,1}, dead_money, active);
                        events_.push_back(n_pp);

                        strats_.emplace_back(this, 0,0, "SB Pushing", "");
                        strats_.emplace_back(this, 1,1, "BB Calling, given a SB push", "p");

                        finish();
                }
                virtual strategy_impl_t make_inital_state()const override{
                        Eigen::VectorXd proto(169);
                        proto.fill(0.0);
                        strategy_impl_t vec;
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        return vec;
                }
                virtual double sb()const{ return sb_; }
                virtual double bb()const{ return bb_; }
                virtual double eff()const{ return eff_; }
                virtual size_t num_players()const{ return 2; }
                virtual size_t strat_vector_size()const{ return 2; }

                virtual double probability_of_event(std::string const& key, holdem_class_vector const& cv, strategy_impl_t const& impl)const{
                        auto a = impl[0][cv[0]];
                        auto b = impl[1][cv[1]];
                        //if( key == "p" ){ return a; }
                        if( key == "f" ){ return ( 1.0 - a); }
                        if( key == "pp"){ return a * b; }
                        if( key == "pf"){ return a * (1.0 - b ); }
                        std::stringstream sstr;
                        sstr << "unknown key " << key;
                        throw std::domain_error(sstr.str());
                }
                virtual Eigen::VectorXd expected_value_by_class_id(size_t player_idx, strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(169);
                        result.fill(0);
                        for(auto const& group : *Memory_TwoPlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result(cv[player_idx]) += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
                virtual Eigen::VectorXd expected_value(strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(2);
                        result.fill(0);
                        for(auto const& group : *Memory_TwoPlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        check_probability_of_event(cv, impl);
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result[0] += _.prob * ev[0];
                                        result[1] += _.prob * ev[1];
                                }
                        }
                        return result;
                }
                virtual double expected_value_for_class_id_es(event_set const& es, size_t player_idx, holdem_class_id class_id, strategy_impl_t const& impl)const override{
                        double result = 0.0;
                        holdem_class_vector cv;
                        for(auto const& group : *Memory_TwoPlayerClassVector){
                                if( group.cid != class_id)
                                        continue;
                                for(auto const& _ : group.vec){
                                        cv = _.cv;
                                        if( player_idx != 0 )
                                                std::swap(cv[0], cv[player_idx]);
                                        auto ev = expected_value_of_vector(es, cv, impl);
                                        result += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
        private:
                double sb_;
                double bb_;
                double eff_;
        };

        struct three_player_description : binary_strategy_description{
                three_player_description(eval_view* eval, double sb, double bb, double eff)
                        : sb_{sb}, bb_{bb}, eff_{eff}
                {
                        eval_ = eval;

                        size_t num_players = 3;

                        Eigen::VectorXd stacks{num_players};
                        for(size_t idx=0;idx!=num_players;++idx){
                                stacks[idx] = eff_;
                        }

                        Eigen::VectorXd v_blinds{num_players};
                        v_blinds.fill(0.0);
                        v_blinds[1] = sb_;
                        v_blinds[2] = bb_;

                        auto make_static = [&](std::string const& key, size_t target){
                                Eigen::VectorXd sv = -v_blinds;
                                sv[target] += v_blinds.sum();
                                auto ptr = std::make_shared<static_event>(key, sv);
                                return ptr;
                        };

                        for(unsigned long long mask = ( 1 << num_players ); mask != 0;){
                                --mask;
                                std::bitset<32> bs = {mask};

                                std::string key;
                                std::vector<size_t> perm;
                                Eigen::VectorXd dead_money = Eigen::VectorXd::Zero(3);
                                Eigen::VectorXd active     = Eigen::VectorXd::Zero(3);

                                for(size_t idx=0;idx!= num_players;++idx){
                                        if( bs.test(idx) ){
                                                active[idx] = stacks[idx];
                                                perm.push_back(idx);
                                                key += "p";
                                        } else{
                                                dead_money[idx] = v_blinds[idx];
                                                key += "f";
                                        }
                                }

                                if( bs.count() == 0 )
                                        continue;
                                if( bs.count() == 1 && bs.test(num_players-1) ){
                                        // walk
                                        std::string degenerate_key(num_players-1, 'f');
                                        auto walk = make_static(degenerate_key, num_players-1);
                                        events_.push_back(walk);
                                } else if( bs.count() == 1 ){
                                        // steal
                                        auto steal = make_static(key, perm[0]);
                                        events_.push_back(steal);
                                } else {
                                        // push call

                                        auto allin = std::make_shared<eval_event>(eval_, key, perm, dead_money, active);
                                        events_.push_back(allin);
                                }

                        }

                        strats_.emplace_back(this, 0,0, "BTN Pushing"                        , ""  );
                        strats_.emplace_back(this, 1,1, "SB Calling, given BTN Push"         , "p" );
                        strats_.emplace_back(this, 2,1, "SB Pushing, given BTN Fold"         , "f" );
                        strats_.emplace_back(this, 3,2, "BB Calling, given BTN Push, SB Call", "pp");
                        strats_.emplace_back(this, 4,2, "BB Calling, given BTN Push, SB Fold", "pf");
                        strats_.emplace_back(this, 5,2, "BB Calling, given BTN Fold, SB Push", "fp");

                        finish();
                }
                virtual strategy_impl_t make_inital_state()const override{
                        Eigen::VectorXd proto(169);
                        proto.fill(.5);
                        strategy_impl_t vec;
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        return vec;
                }
                virtual double sb()const{ return sb_; }
                virtual double bb()const{ return bb_; }
                virtual double eff()const{ return eff_; }
                virtual size_t num_players()const{ return 3; }
                virtual size_t strat_vector_size()const{ return 6; }

                /*
                         The index of the strategy vector, for example
                         for hu index 0 is for sb to push, whilst index
                         1 is for bb to call a push (given the action p),
                         ie
                                Index |Player|  key  | Given |   P
                                ------+------+-------+-------+------
                                  0   |   0  |   p   |       | P(p)
                                  1   |   1  |   pp  |   p   | P(p|p)

                        For three player this canonical mapping doesn't
                        apply, we have

                                Index |Player|  Key  | Given |   P
                                ------+------+-------+-------+------
                                  0   |   0  |   p   |       | P(p)
                                  1   |   1  |   pp  |   p   | P(p|p)
                                  2   |   1  |   fp  |   f   | P(p|f)
                                  3   |   2  |   ppp |   pp  | P(p|pp)
                                  4   |   2  |   pfp |   pf  | P(p|pf)
                                  5   |   2  |   fpp |   fp  | P(p|fp)
                */
                virtual double probability_of_event(std::string const& key, holdem_class_vector const& cv, strategy_impl_t const& impl)const{
                        enum{ Debug = 0 };
                        static std::unordered_map<std::string, size_t> reg_alloc = {
                                             //  Player
                                { ""  , 0 }, //   0
                                { "p" , 1 }, //   1
                                { "f" , 2 }, //   1
                                { "pp", 3 }, //   2
                                { "pf", 4 }, //   2
                                { "fp", 5 }  //   2
                        };
                        double result = 1.0;
                        std::string sub;
                        std::stringstream dbg;
                        for(size_t idx=0;idx!=key.size();++idx){
                                if( reg_alloc.count(sub) == 0 ){
                                        throw std::domain_error("bad");
                                }
                                auto reg = reg_alloc[sub];
                                switch(key[idx]){
                                case 'p':
                                case 'P':
                                {
                                        dbg << "P<" << reg << "," << ( impl[reg][cv[idx]] ) << ">";
                                        result *= impl[reg][cv[idx]];
                                        sub += 'p';
                                        break;
                                }
                                case 'f':
                                case 'F':
                                {
                                        dbg << "F<" << reg << "," << ( 1 - impl[reg][cv[idx]] )<<">";
                                        result *= ( 1 - impl[reg][cv[idx]] );
                                        sub += 'f';
                                        break;
                                }}
                        }
                        if( Debug ) std::cout << dbg.str() << "\n";
                        return result;
                }
                virtual Eigen::VectorXd expected_value_by_class_id(size_t player_idx, strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(169);
                        result.fill(0);
                        for(auto const& group : *Memory_ThreePlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result(cv[player_idx]) += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
                virtual Eigen::VectorXd expected_value(strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(3);
                        result.fill(0);
                        for(auto const& group : *Memory_ThreePlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result[0] += _.prob * ev[0];
                                        result[1] += _.prob * ev[1];
                                        result[2] += _.prob * ev[2];
                                }
                        }
                        return result;
                }
                virtual double expected_value_for_class_id_es(event_set const& es, size_t player_idx, holdem_class_id class_id, strategy_impl_t const& impl)const override{
                        double result = 0.0;
                        holdem_class_vector cv;
                        for(auto const& group : *Memory_ThreePlayerClassVector){
                                if( group.cid != class_id)
                                        continue;
                                for(auto const& _ : group.vec){
                                        cv = _.cv;
                                        if( player_idx != 0 )
                                                std::swap(cv[0], cv[player_idx]);
                                        auto ev = expected_value_of_vector(es, cv, impl);
                                        result += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
        private:
                double sb_;
                double bb_;
                double eff_;
        };

        std::shared_ptr<binary_strategy_description> binary_strategy_description::make_hu_description(eval_view* eval, double sb, double bb, double eff){
                return std::make_shared<heads_up_description>(eval, sb, bb, eff);
        }
        std::shared_ptr<binary_strategy_description> binary_strategy_description::make_three_player_description(eval_view* eval, double sb, double bb, double eff){
                return std::make_shared<three_player_description>(eval, sb, bb, eff);
        }

} // end namespace ps


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

#include "NewtonEulerFrom1DLocalFrameR.hpp"
#include <boost/math/quaternion.hpp>
#include "NewtonEulerDS.hpp"
#include "Interaction.hpp"
#include "BlockVector.hpp"
#include <boost/math/quaternion.hpp>

//#define NERI_DEBUG

//#define NEFC3D_DEBUG
// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "debug.h"
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
#ifdef NEFC3D_DEBUG
  printf("contact normal:\n");
  _Nc->display();
  printf("point de contact :\n");
  _Pc1->display();
  printf("center of masse :\n");
  q1->display();
#endif
  _RotationAbsToContactFrame->setValue(0, 0, Nx);
  _RotationAbsToContactFrame->setValue(0, 1, Ny);
  _RotationAbsToContactFrame->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  computeRotationMatrix(q1,_rotationMatrixAbsToBody);
  prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);

  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom1DLocalFrameR jhqt\n");
  _jachqT->display();
#endif
}

void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);

  _RotationAbsToContactFrame->setValue(0, 0, Nx);
  _RotationAbsToContactFrame->setValue(0, 1, Ny);
  _RotationAbsToContactFrame->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  computeRotationMatrix(q1,_rotationMatrixAbsToBody);
  prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);
  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  _NPG2->zero();
  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;

  computeRotationMatrix(q2,_rotationMatrixAbsToBody);
  prod(*_NPG2, *_rotationMatrixAbsToBody, *_AUX1, true);

  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj + 6, -_RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj + 6, -_AUX2->getValue(0, jj - 3));
}

void NewtonEulerFrom1DLocalFrameR::initialize(Interaction& inter)
{
  NewtonEulerR::initialize(inter);
  //proj_with_q  _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));
  unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
  _jachq.reset(new SimpleMatrix(1, qSize));

  /* VA 12/04/2016 All of what follows should be put in WorkM*/
  _RotationAbsToContactFrame.reset(new SimpleMatrix(1, 3));
  _rotationMatrixAbsToBody.reset(new SimpleMatrix(3, 3));
  _AUX1.reset(new SimpleMatrix(3, 3));
  _AUX2.reset(new SimpleMatrix(1, 3));
  _NPG1.reset(new SimpleMatrix(3, 3));
  _NPG2.reset(new SimpleMatrix(3, 3));
  //  _isContact=1;
}

void NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{

  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n");
  DEBUG_PRINTF("with time =  %f\n",time);
  DEBUG_PRINTF("with inter =  %p\n",&inter);

  _jachq->setValue(0, 0, _Nc->getValue(0));
  _jachq->setValue(0, 1, _Nc->getValue(1));
  _jachq->setValue(0, 2, _Nc->getValue(2));
  if (inter.has2Bodies())
  {
    _jachq->setValue(0, 7, -_Nc->getValue(0));
    _jachq->setValue(0, 8, -_Nc->getValue(1));
    _jachq->setValue(0, 9, -_Nc->getValue(2));
  }

  for (unsigned int iDS =0 ; iDS < q0->numberOfBlocks()  ; iDS++)
  {
    SP::SiconosVector q = (q0->getAllVect())[iDS];
    double sign = 1.0;
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq : ds%d->q :", iDS);
    DEBUG_EXPR_WE(q->display(););

    ::boost::math::quaternion<double>    quatGP;
    if (iDS == 0)
    {
      ::boost::math::quaternion<double>    quatAux(0, _Pc1->getValue(0) - q->getValue(0), _Pc1->getValue(1) - q->getValue(1),
                                                   _Pc1->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    else
    {
      sign = -1.0;
      //cout<<"NewtonEulerFrom1DLocalFrameR::computeJachq sign is -1 \n";
      ::boost::math::quaternion<double>    quatAux(0, _Pc2->getValue(0) - q->getValue(0), _Pc2->getValue(1) - q->getValue(1),
                                                   _Pc2->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :GP :%lf, %lf, %lf\n", quatGP.R_component_2(), quatGP.R_component_3(), quatGP.R_component_4());
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :Q :%e,%e, %e, %e\n", q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatQ(q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatcQ(q->getValue(3), -q->getValue(4), -q->getValue(5), -q->getValue(6));
    ::boost::math::quaternion<double>    quat0(1, 0, 0, 0);
    ::boost::math::quaternion<double>    quatBuff;
    ::boost::math::quaternion<double>    _2qiquatGP;
    _2qiquatGP = quatGP;
    _2qiquatGP *= 2 * (q->getValue(3));
    quatBuff = (quatGP * quatQ) + (quatcQ * quatGP) - _2qiquatGP;

    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :quattBuuf : %e,%e,%e \n", quatBuff.R_component_2(), quatBuff.R_component_3(), quatBuff.R_component_4());

    _jachq->setValue(0, 7 * iDS + 3, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                             quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    //cout<<"WARNING NewtonEulerFrom1DLocalFrameR set jachq \n";
    //_jachq->setValue(0,7*iDS+3,0);
    for (unsigned int i = 1; i < 4; i++)
    {
      ::boost::math::quaternion<double>    quatei(0, (i == 1) ? 1 : 0, (i == 2) ? 1 : 0, (i == 3) ? 1 : 0);
      _2qiquatGP = quatGP;
      _2qiquatGP *= 2 * (q->getValue(3 + i));
      quatBuff = quatei * quatcQ * quatGP - quatGP * quatQ * quatei - _2qiquatGP;
      _jachq->setValue(0, 7 * iDS + 3 + i, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                                   quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    }
  }

  DEBUG_EXPR(_jachq->display(););
  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 \n");

}

void NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 )
{
  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 \n")

  if (q0->numberOfBlocks()>1)
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0], (q0->getAllVect())[1]);
  }
  else
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0]);
  }

  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0) \n");

}

double NewtonEulerFrom1DLocalFrameR::distance() const
{
  SiconosVector dpc(*_Pc2 - *_Pc1);
  return dpc.norm2() * (inner_prod(*_Nc, dpc) >= 0 ? -1 : 1);
}

void NewtonEulerFrom1DLocalFrameR::computeh(double time, BlockVector& q0,
                                            SiconosVector &y)
{
  // Contact points and normal are stored as relative to q1 and q2, if
  // no q2 then pc2 and normal are absolute.

  // Update pc1 based on q0 and relPc1
  SP::SiconosVector q1 = (q0.getAllVect())[0];
  ::boost::math::quaternion<double> qq1((*q1)(3), (*q1)(4), (*q1)(5), (*q1)(6));
  ::boost::math::quaternion<double> qpc1(0,(*_relPc1)(0),(*_relPc1)(1),(*_relPc1)(2));

  // apply q1 rotation and add
  qpc1 = qq1 * qpc1 / qq1;
  (*_Pc1)(0) = qpc1.R_component_2() + (*q1)(0);
  (*_Pc1)(1) = qpc1.R_component_3() + (*q1)(1);
  (*_Pc1)(2) = qpc1.R_component_4() + (*q1)(2);

  if (q0.numberOfBlocks() > 1)
  {
    // Update pc2 based on q0 and relPc2
    SP::SiconosVector q2 = (q0.getAllVect())[1];
    ::boost::math::quaternion<double> qq2((*q2)(3), (*q2)(4), (*q2)(5), (*q2)(6));
    ::boost::math::quaternion<double> qpc2(0,(*_relPc2)(0),(*_relPc2)(1),(*_relPc2)(2));

    // apply q2 rotation and add
    qpc2 = qq2 * qpc2 / qq2;
    (*_Pc2)(0) = qpc2.R_component_2() + (*q2)(0);
    (*_Pc2)(1) = qpc2.R_component_3() + (*q2)(1);
    (*_Pc2)(2) = qpc2.R_component_4() + (*q2)(2);

    // same for normal
    ::boost::math::quaternion<double> qnc(0, (*_relNc)(0), (*_relNc)(1), (*_relNc)(2));
    qnc = qq2 * qnc / qq2;
    (*_Nc)(0) = qnc.R_component_2();
    (*_Nc)(1) = qnc.R_component_3();
    (*_Nc)(2) = qnc.R_component_4();
  }
  else
  {
    *_Pc2 = *_relPc2;
    *_Nc = *_relNc;
  }

  NewtonEulerR::computeh(time, q0, y);
}


#include "CollisionHandler.h"
#include "operationVector.h"
#include <boost/numeric/ublas/matrix.hpp>
#include "Turn.h"
#include <SFML/Window.hpp>
#include <SFML/Graphics.hpp>
#include "VectorOfTurn.h"
#include "Game.h"
#include "bw.h"
#include "Position.h"
#include "beta.h"

using namespace std;
using namespace boost::numeric::ublas;

void game();
void test();

int main()
{
	game();
// 	test();
	return 0;
}

void game()
{
	Game game;
	game.init();
	game.mainLoop();
}

void test()
{

	matrix<double> P{8, 8};
	std::vector<double> pie{0, 0, 0, 0, 1, 0, 0, 0};

	fill(P, [&](int l, int c){
		if ( abs(l - c) < 2 || (l == 0 && c == 7) || (l == 7 && c == 0)) {
			return 1/3.0;
		}
		return 0.0;
	});

	matrix<double> Obs(6, 8);
	fill(Obs, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 2 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 3 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 4 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 5 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}
		return 1;
	});

matrix<double> Obs2(3, 8);
	fill(Obs2, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 1 || c == 2)) {
			return 0;
		}

		if (l == 2 && c != 2 ) {
			return 0;

		}

		return 1;
	});

	matrix<double> Obs3(5, 8);
	fill(Obs3, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 2 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 3 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 4 && c!= 5 ) {
			return 0;
		}

		return 1;
	});

		matrix<double> Obs4(9, 8);
	fill(Obs4, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 2 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 3 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 4 && (c == 5 || c == 6 || c == 4)) {
			return 0;
		}

		if (l == 5 && (c == 4 || c == 5 || c == 3)) {
			return 0;
		}

		if (l == 6 && (c == 5 || c == 6 || c == 4)) {
			return 0;
		}

		if (l == 7 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 8 && c!= 7 ) {
			return 0;
		}

		return 1;
	});

	matrix<double> Obs5(3, 8);
	fill(Obs5, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 7 || c == 6)) {
			return 0;
		}

		if (l == 2 && c != 6 ) {
			return 0;

		}

		return 1;
	});

			matrix<double> Obs6(9, 8);
	fill(Obs6, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 1 || c == 2)) {
			return 0;
		}

		if (l == 2 && (c == 2 || c == 1 || c == 3)) {
			return 0;
		}

		if (l == 3 && (c == 3 || c == 2 || c == 4)) {
			return 0;
		}

		if (l == 4 && (c == 5 || c == 3 || c == 4)) {
			return 0;
		}

		if (l == 5 && (c == 4 || c == 5 || c == 3)) {
			return 0;
		}

		if (l == 6 && (c == 5 || c == 6 || c == 4)) {
			return 0;
		}

		if (l == 7 && (c == 5 || c == 4 || c == 3)) {
			return 0;
		}

		if (l == 8 && c!= 3 ) {
			return 0;
		}

		return 1;
	});

	// showMat(Beta(Obs, P, pie));

	showMat(BW({Obs,Obs2,Obs3,Obs4,Obs5,Obs6,Obs5,Obs5,Obs5,Obs5},P,pie,1000));

	//showMat(Obs6);
	//showMat(Position(Obs6,BW({Obs,Obs2,Obs3,Obs4,Obs5},P,pie,10),pie));
	//showMat(Position(Obs6,P,pie));
}


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

#include "VinaLike.h"

#include <exception>

#include <Structures/Atom.h>

#include <Engines/Internals/InternalsUtilityFunctions.h>

#include <boost/log/trivial.hpp>

#include "VinaLikeCommon.h"

namespace SmolDock::Score {

    double force_Instantiate_VinaLikeIntermolecularScoringFunction(const iConformer &conformer, iTransform &transform,
                                                                   const iProtein &protein,
                                                                   std::array<double, VinaLike_numCoefficients> nonDefaultCoeffs) {
        return VinaLikeIntermolecularScoringFunction<true, false>(conformer, transform, protein, nonDefaultCoeffs);
    }

    double force_Instantiate_VinaLikeIntermolecularScoringFunction_vectorized(
            const iConformer_Vectorized &conformer, iTransform &transform,
            const iProtein_vectorized &protein,
            std::array<double, VinaLike_numCoefficients> nonDefaultCoeffs) {
        return VinaLikeIntermolecularScoringFunction_vectorized<true, false>
                (conformer, transform, protein, nonDefaultCoeffs
                );
    }

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

    template<bool OnlyIntermolecular, bool useNonDefaultCoefficients>
    double VinaLikeIntermolecularScoringFunction(const iConformer &ligand_, iTransform &transform,
                                                 const iProtein &protein,
                                                 std::array<double, VinaLike_numCoefficients> nonDefaultCoeffs) {

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

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

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

        double score_raw = 0;

        double distance_total = 0.0;
        double gauss1_total = 0.0;
        double gauss2_total = 0.0;
        double repuls_total = 0.0;
        double hydrophobic_total = 0.0;
        double hydrogen_total = 0.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++) {
                    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;

                const double rawDist = distVect.norm();

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

                const double atomicRadiusLig = ligand.atomicRadius[idxLig];
                const double atomicRadiusProt = protein.atomicRadius[idxProt];

                const double radToRemove = (atomicRadiusLig + atomicRadiusProt);

                const double distance = rawDist - radToRemove;

                distance_total += distance;

                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);
                } else {

                    gauss1_total += vinaGaussComponent(distance, 0.0, 0.5);
                    gauss2_total += vinaGaussComponent(distance, 3.0, 2.0);
                    repuls_total += vinaRepulsionComponent(distance, 0.0);
                    hydrophobic_total += vinaHydrophobicComponent(distance,
                                                                  atom1AtomicNumber, atom1AtomVariant,
                                                                  atom2AtomicNumber, atom2AtomVariant);
                    hydrogen_total += vinaHydrogenComponent(distance,
                                                            atom1AtomicNumber, atom1AtomVariant,
                                                            atom2AtomicNumber, atom2AtomVariant);

                    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);
                    score_raw += VinaClassic::coeff_hydrophobic * vinaHydrophobicComponent(distance,
                                                                                atom1AtomicNumber,
                                                                                atom1AtomVariant,
                                                                                atom2AtomicNumber,
                                                                                atom2AtomVariant);

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

            } // for
        } // for

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

/*
        std::cout << "\n Non-Vectorized : \n";
        std::cout << "dist    : " << distance_total << std::endl;
        std::cout << "gauss1  : " << gauss1_total << std::endl;
        std::cout << "gauss2  : " << gauss2_total << std::endl;
        std::cout << "repuls  : " << repuls_total << std::endl;
        std::cout << "hydroph : " << hydrophobic_total << std::endl;
        std::cout << "hydrog  : " << hydrogen_total << std::endl;
        std::cout << "\n\n";
*/
        return final_score;
    }

    template<bool OnlyIntermolecular, bool useNonDefaultCoefficients>
    double VinaLikeIntermolecularScoringFunction_vectorized(const iConformer_Vectorized &ligand_, iTransform &transform,
                                                            const iProtein_vectorized &protein,
                                                            std::array<double, VinaLike_numCoefficients> nonDefaultCoeffs) {

        BOOST_ASSERT(ligand_.x.entriesCount() != 0);
        BOOST_ASSERT(protein.x.entriesCount() != 0);

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

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

        double score_raw = 0;

        double distance_total = 0.0;
        double gauss1_total = 0.0;
        double gauss2_total = 0.0;
        double repuls_total = 0.0;
        double hydrophobic_total = 0.0;
        double hydrogen_total = 0.0;

        iConformer_Vectorized ligand = ligand_;
        //applyBondRotationInPlace(ligand, transform);

        const double xProtCenter = protein.center_x;
        const double yProtCenter = protein.center_y;
        const double zProtCenter = protein.center_z;

        if constexpr(!OnlyIntermolecular) // C++17
        {

            for (unsigned int idxLig1 = 0; idxLig1 < ligand.x.vectorsCount(); ++idxLig1) {
                for (unsigned int idxLig2 = idxLig1; idxLig2 < ligand.x.vectorsCount(); ++idxLig2) {

                    const Vc::Vector<double> x_diff = ligand.x.vector(idxLig1) - ligand.x.vector(idxLig2);
                    const Vc::Vector<double> y_diff = ligand.y.vector(idxLig1) - ligand.y.vector(idxLig2);
                    const Vc::Vector<double> z_diff = ligand.z.vector(idxLig1) - ligand.z.vector(idxLig2);
                    const Vc::Vector<double> squared_sum = (x_diff * x_diff) + (y_diff * y_diff) + (z_diff * z_diff);
                    const Vc::Vector<double> distances_raw = Vc::sqrt(squared_sum);

                    const Vc::Vector<double> distances = distanceFromRawDistance(distances_raw,
                                                                                ligand.atomicRadius.vector(idxLig1),
                                                                                ligand.atomicRadius.vector(idxLig2));

                    score_raw += VinaClassic::coeff_repulsion * vinaRepulsionComponent(distances, 0.0).sum();

                }

            }
        }

        int atomicNumber_VectorIdx_ligand = -1;
        int atomicNumber_VectorIdx_protein = -1;
        int atomVariant_VectorIdx_ligand = -1;
        int atomVariant_VectorIdx_protein = -1;

        Vc::Memory<Vc::Vector<unsigned char>, Vc::Vector<double>::Size> ligandAtomicNumInformation;
        Vc::Memory<Vc::Vector<unsigned int>, Vc::Vector<double>::Size> ligandAtomVariantInformation;

        Vc::Memory<Vc::Vector<unsigned char>, Vc::Vector<double>::Size> proteinAtomicNumInformation;
        Vc::Memory<Vc::Vector<unsigned int>, Vc::Vector<double>::Size> proteinAtomVariantInformation;

        for (unsigned int idxLig = 0; idxLig < ligand.x.vectorsCount(); ++idxLig) {
            const unsigned int globalIdx_Ligand = (idxLig * Vc::Vector<double>::Size);
            const unsigned char offsetUChar_Ligand = globalIdx_Ligand % Vc::Vector<unsigned char>::Size;
            const unsigned char offsetUInt_Ligand = globalIdx_Ligand % Vc::Vector<unsigned int>::Size;

            const unsigned int offset_UChar_ligand = offsetUChar_Ligand % Vc::Vector<double>::Size;
            if (offset_UChar_ligand == 0) {
                // we are at the start of a
                atomicNumber_VectorIdx_ligand++;
                ligandAtomicNumInformation = ligand.type.vector(atomicNumber_VectorIdx_ligand);
            }

            const unsigned int offset_UInt_ligand = offsetUInt_Ligand % Vc::Vector<double>::Size;
            if (offset_UInt_ligand == 0) {
                // we are at the start of a
                atomVariant_VectorIdx_ligand++;
                ligandAtomVariantInformation = ligand.variant.vector(atomVariant_VectorIdx_ligand);
            }

            Vc::Vector<double> ligandHydrophobicMaskMaker;
            for (unsigned int k = 0; k < Vc::Vector<double>::Size; ++k) {
                ligandHydrophobicMaskMaker[k] = isHydrophobic_prepareMask(
                        ligandAtomicNumInformation[offset_UChar_ligand + k],
                        ligandAtomVariantInformation[offset_UChar_ligand + k]
                );
            }

            Vc::Mask<double> ligandHydrophobicMask = (ligandHydrophobicMaskMaker == Vc::Vector<double>::One());

            for (unsigned int idxProt = 0; idxProt < protein.x.vectorsCount(); idxProt++) {
                Vc::Vector<double> xLig, yLig, zLig;
                xLig = ligand.x.vector(idxLig);
                yLig = ligand.y.vector(idxLig);
                zLig = ligand.z.vector(idxLig);

                applyRigidTransformInPlace(xLig, yLig, zLig, transform);

                const Vc::Vector<double> xsquared_disttoProtCenter = (xLig - xProtCenter) * (xLig - xProtCenter);
                const Vc::Vector<double> ysquared_disttoProtCenter = (yLig - yProtCenter) * (yLig - yProtCenter);
                const Vc::Vector<double> zsquared_disttoProtCenter = (zLig - zProtCenter) * (zLig - zProtCenter);

                const Vc::Vector<double> distancesToProtCenter = Vc::sqrt(xsquared_disttoProtCenter
                                                                          + ysquared_disttoProtCenter
                                                                          + zsquared_disttoProtCenter);

                const Vc::Vector<double> tooFarFromCenterPenalty = Vc::iif(
                        distancesToProtCenter > (protein.radius - 1),
                        Vc::exp(4 * Vc::log(distancesToProtCenter - protein.radius)) + 10,
                        Vc::Vector<double>(Vc::Zero));

                score_raw += tooFarFromCenterPenalty.sum();

                const Vc::Vector<double> xProt = protein.x.vector(idxProt);
                const Vc::Vector<double> yProt = protein.y.vector(idxProt);
                const Vc::Vector<double> zProt = protein.z.vector(idxProt);

                const Vc::Vector<double> xsquared = (xLig - xProt) * (xLig - xProt);
                const Vc::Vector<double> ysquared = (yLig - yProt) * (yLig - yProt);
                const Vc::Vector<double> zsquared = (zLig - zProt) * (zLig - zProt);

                const Vc::Vector<double> rawDistances = Vc::sqrt(xsquared + ysquared + zsquared);

                if (Vc::all_of(rawDistances >= VinaClassic::interaction_cutoff)) {
                    /*
                     * Theoretically, this helps because atoms which are close in idx are ~ close in position
                     * So all_of will be true a non negigible fraction of the time
                     * TODO : benchmark this versus just the mask
                     */
                    continue;
                }

                // If we have some higher and some lower than the cutoff, we we mask them at the end in .sum()
                auto cutOffMask = rawDistances < VinaClassic::interaction_cutoff;

                const Vc::Vector<double> distances = distanceFromRawDistance(rawDistances,
                                                                             ligand.atomicRadius.vector(idxLig),
                                                                             protein.atomicRadius.vector(idxProt));

                //std::cout << cutOffMask << " -> " << distances << std::endl;

                distance_total += distances.sum(cutOffMask);

                const unsigned int globalIdx_Prot = (idxProt * Vc::Vector<double>::Size);
                const unsigned char offsetUChar_Prot = globalIdx_Prot % Vc::Vector<unsigned char>::Size;
                const unsigned char offsetUInt_Prot = globalIdx_Prot % Vc::Vector<unsigned int>::Size;

                const unsigned int offset_UChar_protein = offsetUChar_Prot % Vc::Vector<double>::Size;
                if (offset_UChar_protein == 0) {
                    // we are at the start of a
                    atomicNumber_VectorIdx_protein++;
                    proteinAtomicNumInformation = protein.type.vector(atomicNumber_VectorIdx_protein);
                }

                const unsigned int offset_UInt_protein = offsetUInt_Prot % Vc::Vector<double>::Size;
                if (offset_UInt_protein == 0) {
                    // we are at the start of a
                    atomVariant_VectorIdx_protein++;
                    proteinAtomVariantInformation = protein.variant.vector(atomVariant_VectorIdx_ligand);
                }

                Vc::Vector<double> proteinHydrophobicMaskMaker;
                for (unsigned int k = 0; k < Vc::Vector<double>::Size; ++k) {
                    proteinHydrophobicMaskMaker[k] = isHydrophobic_prepareMask(
                            proteinAtomicNumInformation[offset_UChar_protein + k],
                            proteinAtomVariantInformation[offset_UInt_protein + k]
                    );
                }

                Vc::Vector<double> HydrogenMaskMaker;
                for (unsigned int k = 0; k < Vc::Vector<double>::Size; ++k) {
                    ligandHydrophobicMaskMaker[k] = hydrogenBondingPossible_prepareMask(
                            ligandAtomicNumInformation[offset_UChar_ligand + k],
                            ligandAtomVariantInformation[offset_UChar_ligand + k],
                            proteinAtomicNumInformation[offset_UChar_ligand + k],
                            proteinAtomVariantInformation[offset_UChar_ligand + k]
                    );
                }

                Vc::Mask<double> hydrophobicMask =
                        (proteinHydrophobicMaskMaker == Vc::Vector<double>::One()) && ligandHydrophobicMask;
                Vc::Mask<double> hydrogenMask = (HydrogenMaskMaker == Vc::Vector<double>::One());

                if constexpr(useNonDefaultCoefficients) {

                    score_raw += nonDefaultCoeffs[0] * vinaGaussComponent(distances, 0.0, 0.5).sum(cutOffMask);
                    score_raw += nonDefaultCoeffs[1] * vinaGaussComponent(distances, 3.0, 2.0).sum(cutOffMask);
                    score_raw += nonDefaultCoeffs[2] * vinaRepulsionComponent(distances, 0.0).sum(cutOffMask);
                    score_raw +=
                            nonDefaultCoeffs[3] * vinaHydrophobicComponent(distances, hydrophobicMask).sum(cutOffMask);
                    score_raw += nonDefaultCoeffs[4] * vinaHydrogenComponent(distances, hydrogenMask).sum(cutOffMask);
                } else {
                    gauss1_total += vinaGaussComponent(distances, 0.0, 0.5).sum(cutOffMask);
                    gauss2_total += vinaGaussComponent(distances, 3.0, 2.0).sum(cutOffMask);
                    repuls_total += vinaRepulsionComponent(distances, 0.0).sum(cutOffMask);
                    hydrophobic_total += vinaHydrophobicComponent(distances, hydrophobicMask).sum(cutOffMask);
                    hydrogen_total += vinaHydrogenComponent(distances, hydrogenMask).sum(cutOffMask);

                    score_raw += VinaClassic::coeff_gauss1 * vinaGaussComponent(distances, 0.0, 0.5).sum(cutOffMask);
                    score_raw += VinaClassic::coeff_gauss2 * vinaGaussComponent(distances, 3.0, 2.0).sum(cutOffMask);
                    score_raw += VinaClassic::coeff_repulsion * vinaRepulsionComponent(distances, 0.0).sum(cutOffMask);
                    score_raw += VinaClassic::coeff_hydrophobic *
                                 vinaHydrophobicComponent(distances, hydrophobicMask).sum(cutOffMask);
                    score_raw += VinaClassic::coeff_hydrogen *
                                 vinaHydrogenComponent(distances, hydrogenMask).sum(cutOffMask);
                }

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

        std::cout << "\n Vectorized : \n";
        std::cout << "dist    : " << distance_total << std::endl;
        std::cout << "gauss1  : " << gauss1_total << std::endl;
        std::cout << "gauss2  : " << gauss2_total << std::endl;
        std::cout << "repuls  : " << repuls_total << std::endl;
        std::cout << "hydroph : " << hydrophobic_total << std::endl;
        std::cout << "hydrog  : " << hydrogen_total << std::endl;
        std::cout << "\n\n";

        return final_score;
    }

    VinaLike::VinaLike(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();
        }

        if (this->useNonDefaultCoefficient) {
                this->nonDefaultCoefficients[0] = VinaClassic::coeff_gauss1;
                this->nonDefaultCoefficients[1] = VinaClassic::coeff_gauss2;
                this->nonDefaultCoefficients[2] = VinaClassic::coeff_repulsion;
                this->nonDefaultCoefficients[3] = VinaClassic::coeff_hydrophobic;
                this->nonDefaultCoefficients[4] = VinaClassic::coeff_hydrogen;
        }

    }

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

        iTransform tr = this->internalToExternalRepr(x);

        normalizeQuaternionInPlace(tr.rota);

        double score_ = this->useNonDefaultCoefficient ?
                        VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation, tr, this->prot,
                                                                           this->nonDefaultCoefficients)
                                                       : VinaLikeIntermolecularScoringFunction<false, false>(
                        this->startingConformation, tr, this->prot);

        return score_;
    }

    double VinaLike::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_ = this->useNonDefaultCoefficient ?
                        VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation, tr, this->prot,
                                                                           this->nonDefaultCoefficients)
                                                       : VinaLikeIntermolecularScoringFunction<false, false>(
                        this->startingConformation, tr, this->prot);

        // Translation
        {
            iTransform transform_dx = tr;
            transform_dx.transl.x() += this->differential_epsilon;
            const double gradScore = this->useNonDefaultCoefficient ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dx, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dy, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dz, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dqs, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dqx, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dqy, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dqz, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 ?
                                     VinaLikeIntermolecularScoringFunction<false, true>(this->startingConformation,
                                                                                transform_dbondrot, this->prot,
                                                                                this->nonDefaultCoefficients)
                                                                    : VinaLikeIntermolecularScoringFunction<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 VinaLike::getDifferentialEpsilon() const {
        return this->differential_epsilon;
    }

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

    iConformer VinaLike::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 VinaLike::getParamVectorDimension() const {
        return this->numberOfParamInState;
    }

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

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

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

        BOOST_ASSERT(!this->startingConformation.x.empty());
        BOOST_ASSERT(!this->prot.x.empty());
        BOOST_ASSERT(tr.bondRotationsAngles.size() == this->startingConformation.num_rotatable_bond);

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

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

        double intramolecular_repuls_total = 0.0;
        double intramolecular_score = 0.0;

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

        Eigen::Vector3d ProtCenterPosition = {this->prot.center_x, this->prot.center_y, this->prot.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 < this->prot.x.size(); idxProt++) {

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

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

                double distanceToProteinCenter = distToCenterVector.norm();

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

                Eigen::Vector3d distVect = ProtPosition - LigPosition;

                double rawDist = distVect.norm();

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

                double distance = distanceFromRawDistance(rawDist, ligand.atomicRadius[idxLig],
                                                          this->prot.atomicRadius[idxProt]);

                const unsigned int atom1AtomicNumber = ligand.type[idxLig];
                const unsigned int atom1AtomVariant = ligand.variant[idxLig];
                const unsigned int atom2AtomicNumber = this->prot.type[idxProt];
                const unsigned int atom2AtomVariant = this->prot.variant[idxProt];

                gauss1_total += vinaGaussComponent(distance, 0.0, 0.5);
                gauss2_total += vinaGaussComponent(distance, 3.0, 2.0);
                repulsion_total += vinaRepulsionComponent(distance, 0.0);
                hydrophobic_total += vinaHydrophobicComponent(distance,
                                                              atom1AtomicNumber, atom1AtomVariant,
                                                              atom2AtomicNumber, atom2AtomVariant);
                hydrogen_total += vinaHydrogenComponent(distance,
                                                        atom1AtomicNumber, atom1AtomVariant,
                                                        atom2AtomicNumber, atom2AtomVariant);

            } // for
        } // for

        double score_sum = 0.0;

        if (this->useNonDefaultCoefficient) {
            ret.emplace_back(std::make_tuple("NonDefaultCoeffs", 1.0));
            score_sum = this->nonDefaultCoefficients[0] * gauss1_total
                        + this->nonDefaultCoefficients[1] * gauss2_total
                        + this->nonDefaultCoefficients[2] * repulsion_total
                        + this->nonDefaultCoefficients[3] * hydrophobic_total
                        + this->nonDefaultCoefficients[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;
        }

        double final_score_fromSum = score_sum / (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", 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("Intra_Repuls", intramolecular_repuls_total));
        ret.emplace_back(std::make_tuple("Intra_Score", intramolecular_score));
        ret.emplace_back(std::make_tuple("ScoreRawSum", score_sum));
        ret.emplace_back(std::make_tuple("Score", final_score_fromSum));
        return ret;
    }

    double VinaLike::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_ = this->useNonDefaultCoefficient ?
                        VinaLikeIntermolecularScoringFunction<true, true>(this->startingConformation, tr, this->prot,
                                                                           this->nonDefaultCoefficients)
                                                       : VinaLikeIntermolecularScoringFunction<true, false>(
                        this->startingConformation, tr, this->prot);

        return score_;
    }

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

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

    std::vector<double> VinaLike::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};
    }

    bool VinaLike::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;
    }

}