#include <algorithm>
#include <fstream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include <Eigen/Dense>
#include "gmpxx.h"

#include "Expression.h"
#include "MonomialExpression.h"
#include "Scalar.h"
#include "ScalarSum.h"
#include "TensorMonomial.h"

template <typename Archive>
void Expression::serialize(Archive & ar, unsigned int const version) {
  if (version > 0) {
  } else {
  }
  ar & dimension;
  ar & summands;
}

template void Expression::serialize<boost::archive::text_oarchive>(boost::archive::text_oarchive&, unsigned int const);
template void Expression::serialize<boost::archive::text_iarchive>(boost::archive::text_iarchive&, unsigned int const);

typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> MatrixXq;

Expression::Expression() : dimension(4), summands(std::make_unique<std::vector<Summand>>()) { }

Expression::Expression(Expression const & other) : dimension(other.dimension), summands(std::make_unique<Sum>()) {
  std::for_each(other.summands->cbegin(), other.summands->cend(), [this](auto const & it) {
    summands->push_back(std::pair<std::unique_ptr<MonomialExpression>, std::unique_ptr<ScalarSum>>());
    auto monexp_new = std::make_unique<MonomialExpression>(*(it.first));
    auto scalar_new = std::make_unique<ScalarSum>(*(it.second));
    std::swap((*(--summands->end())).first, monexp_new);
    std::swap((*(--summands->end())).second, scalar_new);
  });
}

Expression & Expression::operator=(Expression const & other) {
  Expression tmp ( other );
  dimension = other.dimension;
  std::swap(summands, tmp.summands);
  return *this;
}

void Expression::set_dimension (unsigned int const dimension_set) {
  dimension = dimension_set;
}

unsigned int Expression::get_dimension (void) const {
  return dimension;
}

void Expression::AddSummand (MonomialExpression const & monomial_expression, ScalarSum const & scalar_sum) {
  summands->push_back(std::make_pair(std::make_unique<MonomialExpression>(monomial_expression), std::make_unique<ScalarSum>(scalar_sum)));
}

void Expression::AddSummand (MonomialExpression const & monomial_expression, Scalar const & scalar) {
  summands->push_back(std::make_pair(std::make_unique<MonomialExpression>(monomial_expression), std::make_unique<ScalarSum>(scalar)));
}

void Expression::AddSummand (MonomialExpression const & monomial_expression) {
  AddSummand(monomial_expression, Scalar());
}

void Expression::AddSummand (MonomialExpression const & monomial_expression, Rational const & rational) {
  AddSummand(monomial_expression, Scalar(rational));
}

void Expression::AddSummand (MonomialExpression const & monomial_expression, size_t variable) {
  AddSummand(monomial_expression, Scalar(variable));
}

void Expression::AddSummand (MonomialExpression const & monomial_expression, Rational const & rational, size_t variable) {
  AddSummand(monomial_expression, Scalar(rational, variable));
}

std::set<size_t> Expression::GetVariableSet () const {
  std::set<size_t> ret;
  std::for_each(summands->cbegin(), summands->cend(),
    [&ret] (auto const & summand) {
      ret.merge(summand.second->CoefficientSet());
    });

  return ret;
}

std::map<size_t, size_t> Expression::GetCoefficientMap () const {
  std::set<size_t> variables = GetVariableSet ();

  std::map<size_t, size_t> coefficient_map;
  std::for_each(variables.cbegin(), variables.cend(), [&coefficient_map,n=0] (auto const & a) mutable { coefficient_map[a] = n++; });

  return coefficient_map;
}

std::vector<std::vector<Rational>> Expression::GetPrefactorMatrix () const {
  std::map<size_t, size_t> coefficient_map = GetCoefficientMap ();

  std::vector<std::vector<Rational>> prefactor_matrix;
  std::for_each(summands->cbegin(), summands->cend(),
    [&prefactor_matrix, &coefficient_map] (auto const & summand) {
      prefactor_matrix.push_back(summand.second->CoefficientVector(coefficient_map));
    });

  std::sort(prefactor_matrix.begin(), prefactor_matrix.end());
  prefactor_matrix.erase(std::unique(prefactor_matrix.begin(), prefactor_matrix.end()), prefactor_matrix.end());

  return prefactor_matrix;
}

void Expression::SortMonomials() {
  auto summands_new = std::make_unique<Sum>();
  std::transform(summands->cbegin(), summands->cend(), std::back_inserter(*summands_new), [](auto const & a) { auto ret = std::make_pair(std::make_unique<MonomialExpression>(*a.first), std::make_unique<ScalarSum>(*a.second)); ret.first->Sort(); return ret; });
  std::swap(summands, summands_new);
}

void Expression::ApplyMonomialSymmetries() {
  for (auto & summand : *summands) {
    bool even = summand.first->ApplySymmetries();
    if (!even) {
      summand.second->Negate();
    }
  }
}

void Expression::ApplyMonomialSymmetriesToContractions() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat](auto & a) {
        Status ret = a.first->ApplySymmetriesToContractions();
        if (ret == SYM_EVEN) {
          repeat = true;
        } else if (ret == SYM_ODD) {
          a.second->Negate();
          repeat = true;
        } else if (ret != NO_ACTION) {
          assert(false);
        }
      });
  }
}

void Expression::CollectPrefactors() {
  if (summands->size() < 2) {
    return;
  }

  auto summands_new = std::make_unique<std::vector<std::pair<std::unique_ptr<MonomialExpression>, std::unique_ptr<ScalarSum>>>>();

  auto last = summands->begin();
  for (auto it = summands->begin() + 1; it < summands->end(); ++it) {
    if (*(last->first) == *(it->first)) {
      last->second->MergeWithOther(*(it->second));
      if (std::distance(it, summands->end()) == 1) {
        summands_new->push_back(std::pair<std::unique_ptr<MonomialExpression>, std::unique_ptr<ScalarSum>>());
        std::swap(*(--(summands_new->end())), *last);
      }
    } else {
        summands_new->push_back(std::pair<std::unique_ptr<MonomialExpression>, std::unique_ptr<ScalarSum>>());
        std::swap(*(--(summands_new->end())), *last);
      if (std::distance(it, summands->end()) == 1) {
        summands_new->push_back(std::pair<std::unique_ptr<MonomialExpression>, std::unique_ptr<ScalarSum>>());
        std::swap(*(--(summands_new->end())), *it);
      } else {
        last = it;
      }
    }
  }

  std::swap(summands, summands_new);

  CanonicalisePrefactors();
}

void Expression::CanonicalisePrefactors() {
  std::for_each(summands->begin(), summands->end(), [](auto & a) { a.second->Sort(); a.second->Collect(); });

  EliminateZeros();
}

void Expression::EliminateGamma() {
  bool repeat = true;

  while (repeat) {
    auto summands_new = std::make_unique<Sum>();
    repeat = false;
    std::transform(summands->cbegin(), summands->cend(), std::back_inserter(*summands_new),
      [&repeat](auto const & a) {
        auto summand_new = std::make_pair(std::make_unique<MonomialExpression>(*a.first), std::make_unique<ScalarSum>(*a.second));;
        Status ret = summand_new.first->EliminateGamma();
        if (ret == DELTA_OK ) {
          repeat = true;
        } else if (ret == DELTA_TO_TRACE) {
          summand_new.second->MultiplyCoefficient(Rational(3, 1));
          repeat = true;
        } else if (ret == ERROR) {
          assert(false);
        }
        return summand_new;
      });
    std::swap(summands, summands_new);
  }

}

void Expression::EliminateDelta() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat,this](auto & a) {
        Status ret = a.first->EliminateDelta();
        if (ret == DELTA_OK ) {
          repeat = true;
        } else if (ret == DELTA_TO_TRACE) {
          a.second->MultiplyCoefficient(Rational(dimension, 1));
          repeat = true;
        } else if (ret == ERROR) {
          assert(false);
        }
      });
  }

}

void Expression::EliminateEpsilon() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat,this](auto & a) {
        Status ret = a.first->EliminateEpsilon(dimension);
        if (ret == EPSILON_TO_ZERO) {
          a.second->MultiplyCoefficient(Rational(0, 1));
          repeat = true;
        } else if (ret == ERROR) {
          assert(false);
        }
      });
    EliminateZeros();
  }
}

void Expression::EliminateEpsilonI() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat](auto & a) {
        Status ret = a.first->EliminateEpsilonI();
        if (ret == EPSILONI) {
          repeat = true;
        } else if (ret != NO_ACTION) {
          assert(false);
        }
      });
    EliminateZeros();
  }
}

static bool Sort(std::vector<size_t> vec) {
  if (vec.empty()) {
    return true;
  }
  unsigned int swap_counter = 0;
  bool swapped;
  do {
    swapped = false;
    for (size_t counter = 0; counter < vec.size() - 1; ++counter) {
      if (vec[counter] > vec[counter + 1]) {
        std::swap(vec[counter], vec[counter + 1]);
        swapped = true;
        swap_counter += 1;
      }
    }
  } while (swapped);
  return (swap_counter % 2 == 0) ? true : false;
}

void Expression::EliminateEpsilonEpsilonI() {
  bool repeat = true;

  while (repeat) {
    // new vector of summands
    auto summands_new = std::make_unique<Sum>();
    // don't repeat unless there has been some action
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat,&summands_new,this](auto const & a) {
        // eliminate epsilon and epsilonI in summand
        // return indices of epsilon and epsilonI
        auto indices_pair = a.first->EliminateEpsilonEpsilonI(dimension);

        // if there was no action (returned indices are of size zero),
        // push back old summand to new vector of summands
        //
        // else, for every permutation, push back the (dimension) deltas and
        // then what remains from the original summand (if there is any tensor left)
        if ( indices_pair.first.size() == 0 && indices_pair.second.size() == 0 && !a.first->IsZero()) {
          summands_new->push_back(std::make_pair(std::make_unique<MonomialExpression>(*a.first), std::make_unique<ScalarSum>(*a.second)));
        } else if ( indices_pair.first.size() == dimension && indices_pair.second.size() == dimension ) {
          Tensor delta (2, (dimension == 3 ? "gamma" : "delta"));
          delta.SetSymmetric();

          TensorMonomial dim_deltas;

          std::vector<size_t> index_numbers (dimension);
          std::generate(index_numbers.begin(), index_numbers.end(),
            [&delta,&dim_deltas,n=0] () mutable {
              dim_deltas.AddFactorRight(delta);
              return n++;
            });

          do {
            auto index_number_copy = index_numbers;
            auto const index_number_copy_const = index_numbers;
            bool sign = Sort(index_number_copy);

            ScalarSum coeff = *a.second;
            if (dimension == 4) {
              if (sign) {
                coeff.Negate(); // Attention! Additional minus sign because epsilon epsilonI = - delta delta delta delta
              }
            } else if (dimension == 3) {
              if (!sign) {
                coeff.Negate();
              }
            } else {
              assert(false);
            }

            std::vector<Index> dim_deltas_indices (2*dimension);

            std::for_each(index_number_copy_const.cbegin(), index_number_copy_const.cend(),
              [&dim_deltas_indices,&indices_pair,&index_numbers] (auto const & a) {
                dim_deltas_indices[2*a] = indices_pair.first.at(a);
                dim_deltas_indices[2*a+1] = indices_pair.second.at(index_numbers.at(a));
              });

            MonomialExpression dim_deltas_me (dim_deltas, Indices(dim_deltas_indices));
            MonomialExpression new_summand = dim_deltas_me.MultiplyOther(*a.first);

            summands_new->push_back(std::make_pair(std::make_unique<MonomialExpression>(new_summand), std::make_unique<ScalarSum>(coeff)));
          } while (std::next_permutation(index_numbers.begin(), index_numbers.end()));

//          repeat = true;

        } else {
          assert(false);
        }
      });
    std::swap(summands, summands_new);
  }
}

void Expression::EliminateTracefree() {
  std::for_each(summands->begin(), summands->end(),
    [](auto & a) {
      if (a.first->EliminateTracefree() == TRACE_TO_ZERO) {
        a.second->MultiplyCoefficient(Rational(0, 1));
      }
    });

  EliminateZeros();
}

void Expression::EliminateGammaGamma() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat](auto & a) {
        Status ret = a.first->EliminateGammaGamma();
        if (ret == GAMMA_GAMMA_TO_TRACE) {
          a.second->MultiplyCoefficient(Rational(3, 1));
          repeat = true;
        } else if (ret == GAMMA_GAMMA_TO_DELTA) {
          repeat = true;
        } else if (ret == ERROR) {
          assert(false);
        }
      });
  }
}

void Expression::EliminateEtaEta() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat](auto & a) {
        Status ret = a.first->EliminateEtaEta();
        if (ret == ETA_ETA_TO_TRACE) {
          a.second->MultiplyCoefficient(Rational(4, 1));
          repeat = true;
        } else if (ret == ETA_ETA_TO_DELTA) {
          repeat = true;
        } else if (ret == ERROR) {
          assert(false);
        }
      });
  }
}

void Expression::EliminateEtaPartial() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat](auto & a) {
        Status ret = a.first->EliminateEtaPartial();
        if (ret == ETA_PARTIAL_CONTRACTION) {
          repeat = true;
        } else if (ret != NO_ACTION) {
          assert(false);
        }
      });
  }
}

void Expression::EliminateEtaRankOne() {
  bool repeat = true;

  while (repeat) {
    repeat = false;
    std::for_each(summands->begin(), summands->end(),
      [&repeat](auto & a) {
        Status ret = a.first->EliminateEtaRankOne();
        if (ret == ETA_CONTRACTION) {
          repeat = true;
        } else if (ret != NO_ACTION) {
          assert(false);
        }
      });
  }
}

void Expression::EliminateZeros() {
  auto summands_new = std::make_unique<std::vector<std::pair<std::unique_ptr<MonomialExpression>, std::unique_ptr<ScalarSum>>>>();

  for (auto & summand : *summands) {
    if (summand.second->IsZero()) {
      continue;
    }
    summands_new->push_back(std::pair<std::unique_ptr<MonomialExpression>, std::unique_ptr<ScalarSum>>());
    std::swap(*(--(summands_new->end())), summand);
  }

  std::swap(summands, summands_new);
}

void Expression::SubstituteIndices(Indices const & indices_old, Indices const & indices_new) {
  std::for_each(summands->begin(), summands->end(),
    [&indices_old,&indices_new] (auto & a) {
      a.first->ExchangeIndices(indices_old,indices_new);
    });
}

void Expression::SubstituteFreeIndices(Indices const & indices_old, Indices const & indices_new) {
  std::for_each(summands->begin(), summands->end(),
    [&indices_old,&indices_new] (auto & a) {
      a.first->ExchangeFreeIndices(indices_old,indices_new);
    });
}

void Expression::SubstituteVariable(size_t const variable_old, ScalarSum const & scalar_sum_new) {
  std::for_each(summands->begin(), summands->end(),
    [variable_old,scalar_sum_new](auto & a) {
      a.second->SubstituteVariable(variable_old, scalar_sum_new);
    });
}

void Expression::SubstituteVariables(std::map<size_t, ScalarSum> substitution_map) {
  std::for_each(summands->begin(), summands->end(),
    [substitution_map](auto & a) {
      a.second->SubstituteVariables(substitution_map);
    });
}

void Expression::SubstituteVariables(std::map<size_t, size_t> substitution_map) {
  std::for_each(summands->begin(), summands->end(),
    [substitution_map](auto & a) {
      a.second->SubstituteVariables(substitution_map);
    });
}

void Expression::EliminateVariable(size_t const variable) {
  std::for_each(summands->begin(), summands->end(), [variable](auto & a) { a.second->EliminateVariable(variable); });
}

void Expression::SortSummands() {
  std::sort(summands->begin(), summands->end());
}

bool CompareSummandsReversed(Summand const & a, Summand const & b);

void Expression::SortSummandsByLastFactors() {
  std::sort(summands->begin(), summands->end(), CompareSummandsReversed);
}

void Expression::ExchangeSymmetrise(Indices const & indices1, Indices const & indices2, bool anti) {
  auto summands_new = std::make_unique<Sum>();

  for (auto & summand : *summands) {
    summand.second->DivideByTwo();
    Summand summand_copy;
    summand_copy.first = std::make_unique<MonomialExpression>(*summand.first);
    summand_copy.second = std::make_unique<ScalarSum>(*summand.second);
    summands_new->push_back(std::make_pair(std::make_unique<MonomialExpression>(), std::make_unique<ScalarSum>()));
    std::swap(*(--(summands_new->end())), summand);
    if (anti) {
      summand_copy.second->Negate();
    }
    summand_copy.first->ExchangeFreeIndices(indices1, indices2);
    summands_new->push_back(std::make_pair(std::make_unique<MonomialExpression>(), std::make_unique<ScalarSum>()));
    std::swap(*(--(summands_new->end())), summand_copy);
  }

  std::swap(summands, summands_new);
}

void Expression::SortSummandsByPrefactors() {
  std::sort(summands->begin(), summands->end(), [](auto & a, auto & b) {
    if (*a.second < *b.second) {
      return true;
    } else if (*b.second < *a.second) {
      return false;
    } else if (*a.first < *b.first) {
      return true;
    } else {
      return false;
    }
  });
}

void Expression::ThreePlusOne(std::vector<Index> indices_to_zero) {
  auto summands_new = std::make_unique<Sum>();

  std::for_each(summands->begin(), summands->end(),
    [&indices_to_zero,&summands_new] (auto & a) {
      auto status = a.first->ThreePlusOne(indices_to_zero);
      if (status == ZERO_ZERO) {
        return;
      } else if (status == ZERO_POSITIVE) {
        summands_new->emplace_back(std::make_unique<MonomialExpression>(*a.first), std::make_unique<ScalarSum>(*a.second));
      } else if (status == ZERO_NEGATIVE) {
        summands_new->emplace_back(std::make_unique<MonomialExpression>(*a.first), std::make_unique<ScalarSum>(*a.second));
        summands_new->back().second->Negate();
      } else {
        assert(false);
      }
    });

  dimension = 3;
  std::swap(summands_new, summands);
}

bool Expression::ContainsMonomial (MonomialExpression const & monexpr) const {
  bool ret = false;
  for (auto & summand : *summands) {
    if (*(summand.first) == monexpr) {
      ret = true;
      break;
    }
  }

  return ret;
}

void Expression::RedefineScalars() {
  if (summands->size() == 0) {
    return;
  }

  std::map<size_t, size_t> coefficient_map = GetCoefficientMap ();

  std::vector<std::vector<Rational>> prefactor_matrix = GetPrefactorMatrix ();

  MatrixXq mq(prefactor_matrix.size(), prefactor_matrix.front().size());

  for (size_t row_counter = 0; row_counter < prefactor_matrix.size(); ++row_counter) {
    for (size_t column_counter = 0; column_counter < prefactor_matrix.front().size(); ++column_counter) {
      Fraction frac = prefactor_matrix[row_counter][column_counter].get_fraction();
      mq(row_counter, column_counter) = mpq_class(frac.first, frac.second);
    }
  }

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  MatrixXq kq = lu_decompq.kernel();

  std::set<size_t> coeff_removed;

  for (int column_counter = 0; column_counter < kq.cols(); ++column_counter) {
    for (int row_counter = kq.rows() - 1; row_counter >= 0; --row_counter) {
      if (kq(row_counter, column_counter) == 0 ) {
        continue;
      } else if (std::find(coeff_removed.begin(), coeff_removed.end(), row_counter) != coeff_removed.end()) {
        continue;
      } else {
        coeff_removed.insert(row_counter);
        break;
      }
    }
  }

  std::map<size_t, size_t> coefficient_rmap;
  std::for_each(coefficient_map.begin(), coefficient_map.end(), [&coefficient_rmap](auto a) { coefficient_rmap.insert(std::make_pair(a.second, a.first)); });

  Expression ret (*this);
  std::for_each(coeff_removed.cbegin(), coeff_removed.cend(), [&ret, &coefficient_rmap, this] (auto a) { EliminateVariable(coefficient_rmap.at(a)); });

  CanonicalisePrefactors();

  std::map<size_t, size_t> substitution_map;

  std::for_each(coefficient_map.cbegin(), coefficient_map.cend(),
    [&substitution_map,&coeff_removed,n=1] (auto const & a) mutable {
      if (coeff_removed.find(a.second) == coeff_removed.end()) {
        substitution_map[a.first] = n++;
      }
    });

  SubstituteVariables(substitution_map);

/*
  auto summands_new = std::make_unique<Sum>();
  summands_new->push_back(std::make_pair(std::make_unique<MonomialExpression>(), std::make_unique<ScalarSum>(Scalar(1))));
  std::swap(summands->front().first, summands_new->back().first);

  size_t variable_counter = 0;

  auto it_last = summands->begin();

  for (auto it = summands->begin() + 1; it != summands->end(); ++it) {
    Rational coefficient(1,1);
    if (*(it->second) == *((it - 1)->second)) {
      coefficient = coefficient.DivideOther((it_last)->second->Ratio(*(it->second)));
    } else {
      ++variable_counter;
      it_last = it;
    }
    summands_new->push_back(std::make_pair(std::make_unique<MonomialExpression>(), std::make_unique<ScalarSum>(Scalar(coefficient, variable_counter + 1))));
    std::swap(it->first, summands_new->back().first);
  }

  std::swap(summands, summands_new);
*/
}

void Expression::RenameDummies() {
  std::for_each(summands->begin(), summands->end(),
    [](auto & a) {
      a.first->RenameDummies();
    });
}

void Expression::MultiplyCoefficient (Rational const & q) {
  std::for_each(summands->begin(), summands->end(),
    [q] (auto & a) {
      a.second->MultiplyCoefficient(q);
    });
}

ScalarSum Expression::EvaluateIndices(Indices const & indices, std::vector<size_t> const & numbers) const {
  std::map<Index, size_t> evaluation_map;

  for (size_t counter = 0; counter < indices.size(); ++counter) {
    evaluation_map[indices.at(counter)] = numbers.at(counter);
  }
  ScalarSum ret;
  std::for_each(summands->begin(), summands->end(),
    [&ret, &evaluation_map](auto & a) {
      ScalarSum new_summand(*(a.second));
      new_summand.MultiplyCoefficient(a.first->EvaluateIndices(evaluation_map));
      ret.MergeWithOther(new_summand);
    });

  ret.Sort();
  ret.Collect();

  return ret;
}

void Expression::AddOther(Expression const & other) {
  auto summands_new = std::make_unique<Sum>();

  summands_new->reserve(this->summands->size() + other.summands->size());
  std::for_each(summands->cbegin(), summands->cend(),
    [&summands_new] (auto const & a) {
      summands_new->emplace_back(std::make_pair(std::make_unique<MonomialExpression>(*a.first), std::make_unique<ScalarSum>(*a.second)));
    });
  std::for_each(other.summands->cbegin(), other.summands->cend(),
    [&summands_new] (auto const & a) {
      summands_new->emplace_back(std::make_pair(std::make_unique<MonomialExpression>(*a.first), std::make_unique<ScalarSum>(*a.second)));
    });

  assert(summands_new->size() == this->summands->size() + other.summands->size());

  std::swap(summands, summands_new);
}

void Expression::MultiplyOther(Expression const & other) {
  auto summands_new = std::make_unique<Sum>();

  if (!(IsZero() || other.IsZero())) {

  std::for_each(summands->cbegin(), summands->cend(),
    [&summands_new, &other] (auto const & a) {
      std::for_each(other.summands->cbegin(), other.summands->cend(),
        [&summands_new, &a] (auto const & b) {
          summands_new->push_back(std::make_pair(std::make_unique<MonomialExpression>(a.first->MultiplyOther(*(b.first))), std::make_unique<ScalarSum>(a.second->MultiplyOther(*(b.second)))));
        });
      });

  }

  assert (summands_new->size() == this->summands->size() * other.summands->size());

  std::swap(summands, summands_new);
}

bool Expression::IsZero() const { return summands->empty(); }

void Expression::SaveToFile(std::string const & filename) const {
  std::ofstream file;
  file.open(filename);
  boost::archive::text_oarchive oa {file};
  oa << *this;
}

void Expression::LoadFromFile(std::string const & filename) {
  std::ifstream file;
  file.open(filename);
  boost::archive::text_iarchive ia {file};
  ia >> *this;
}

std::string const Expression::GetLatexString(std::string base_name, bool upper) const {
  std::stringstream ss;

  bool first = true;

  if (summands->empty()) {
    return "0";
  }

  for (auto const & summand : *summands) {
    if (!first) {
      ss << std::endl;
    }
    ss << summand.second->ToString(base_name, !first) << " ";
    first = false;

    ss << summand.first->GetLatexString(upper);
  }

  return ss.str();
}

bool operator< (Summand const & lop, Summand const & rop) {
  if (*lop.first < *rop.first) {
    return true;
  } else if (*rop.first < *lop.first) {
    return false;
  } else if (*lop.second < *rop.second) {
    return true;
  } else {
    return false;
  }
}

bool CompareSummandsReversed (Summand const & lop, Summand const & rop) {
  if (MonomialExpression::CompareReversed(*lop.first, *rop.first)) {
    return true;
  } else if (MonomialExpression::CompareReversed(*rop.first, *lop.first)) {
    return false;
  } else if (*lop.second < *rop.second) {
    return true;
  } else {
    return false;
  }
}


/// \file   tatonnement.hpp
///
/// \brief  Implements the tâtonnement process (hill climbing), implemented as a
///         numerical optimisation using quasi-Newton methods.
///
/// \remark This code uses the spelling `tatonnement`, as the accent on `â` can
///         not be rendered in some filesystem character sets.
///
/// \authors    Maarten P. Scholl
/// \date       2018-02-02
/// \copyright  Copyright 2017-2019 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             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.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#ifndef PROJECT_TATONNEMENT_HPP
#define PROJECT_TATONNEMENT_HPP

#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <vector>
#include <map>

#include <adept.h>

#include <esl/law/property_collection.hpp>
#include <esl/economics/markets/quote.hpp>
#include <esl/economics/markets/differentiable_demand_supply_function.hpp>
#include <esl/economics/markets/walras/differentiable_order_message.hpp>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

///
/// The following functions defined callbacks for various optimisers used to
/// solve the tatonnement problem. These functions reside in the
/// global namespace, because external "C" linkage doesn't deal with namespaces.
///

extern "C" double c_minimizer_function_value(const gsl_vector *variables, void *params);
extern "C" void c_minimizer_function_gradient(const gsl_vector *x, void *params, gsl_vector *gradJ);
extern "C" void c_minimizer_function_value_and_gradient(const gsl_vector *x, void *params, double *J, gsl_vector *gradJ);

extern "C" int multiroot_function_value_cb(const gsl_vector *x, void *params, gsl_vector *f);
extern "C" int multiroot_function_jacobian_cb(const gsl_vector * x, void * params, gsl_matrix * df);
extern "C" int multiroot_function_value_and_gradient_cb(const gsl_vector * x, void * params, gsl_vector * f, gsl_matrix *df);

extern "C" double uniroot_function_value (double x, void *params);
extern "C" double uniroot_function_value_and_gradient (double x, void *params);
extern "C" void   uniroot_function_jacobian_cb (double x, void *parameters, double *y, double *dy);

namespace esl::economics::markets::tatonnement {
    ///
    /// \brief
    ///
    class excess_demand_model
    {
    public:

        std::vector<std::shared_ptr<walras::differentiable_order_message>> excess_demand_functions_;

        ///
        /// \brief  Constructs the excess demand model given initial quotes.
        ///
        /// \param initial_quotes   Initial quote values that the solver
        ///                         uses to start the tatonnement process
        explicit excess_demand_model(law::property_map<quote> initial_quotes);

        virtual ~excess_demand_model();

        ///
        /// \brief  Types of solver that are offered
        ///
        enum solver
        { minimization  // minimize q: (demand(q) - supply(q))^2
        , root          // find a vector q: (demand(q) - supply(q))_i = 0 for all i
        , derivative_free_minimization
        , derivative_free_root
        };

        ///
        /// \brief  Limits maximum price decrease/increase
        ///
        std::pair<double, double> circuit_breaker = {1./11, 11.};

        ///
        /// \brief  The default approach is to try the root-finding approach
        ///         first, followed by the minimization approach if the
        ///         root finding solver does not make progress towards a
        ///         solution.
        ///
        std::vector<solver> methods =
            { root
            , minimization
            , derivative_free_root
            , derivative_free_minimization
            };

        ///
        /// \brief  The quotes to start the solving process from
        ///
        law::property_map<quote> quotes;

    protected:
        ///
        /// \brief  Adept data structure to track expressions
        ///
        adept::Stack stack_;

        ///
        /// \brief  Currently active differentiable variables. Stored in one place on the model, so that we can use
        ///         external solvers that operate on a pointer or reference to these variables
        ///
        std::vector<adept::adouble> active_;

        ///
        /// \brief
        ///
        /// \param multipliers
        /// \return
        adept::adouble demand_supply_mismatch(const adept::adouble *multipliers);

        ///
        /// \brief
        ///
        /// \param multipliers
        /// \return
        std::vector<adept::adouble> excess_demand(const adept::adouble *multipliers);

        double excess_demand_function_value(const double *multipliers);

        double minimizer_function_value_and_gradient(const double *multipliers, double *derivatives) ;

        friend void ::c_minimizer_function_gradient(const gsl_vector *multipliers, void *model, gsl_vector *derivatives);
        friend void ::c_minimizer_function_value_and_gradient(const gsl_vector *x, void *params, double *J, gsl_vector *gradJ);
        friend double ::c_minimizer_function_value(const gsl_vector *variables, void *params);

        friend int ::multiroot_function_value_cb(const gsl_vector *x, void *params, gsl_vector *f);
        friend int ::multiroot_function_value_and_gradient_cb(const gsl_vector *multipliers, void * params, gsl_vector * f, gsl_matrix *derivatives);
        friend int ::multiroot_function_jacobian_cb(const gsl_vector *multipliers, void * params, gsl_matrix *derivatives);
        std::vector<double> multiroot_function_value_and_gradient(const double *multipliers, double *derivatives) ;
        std::vector<double> multiroot_function_value(const double *multipliers);

        friend double ::uniroot_function_value(double x, void *params);
        friend double ::uniroot_function_value_and_gradient (double x, void *params);
        friend void   ::uniroot_function_jacobian_cb (double x, void *parameters, double *y, double *dy);

    public:
        ///
        /// \param max_iterations   The maximum number of iterations to perform
        ///                         while solving
        /// \return
        std::optional<std::map<identity<law::property>, double>>
        compute_clearing_quotes(size_t max_iterations = 16);

    };
} // namespace esl::economics::markets::tatonnement

#endif  // PROJECT_TATONNEMENT_HPP


#include <boost/math/tools/roots.hpp>


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/**
 * @file binaryArith.cpp
 * @brief Implementing integer addition, multiplication in binary representation
 */
#include <numeric>
#include <climits>
#include <map>
#include <algorithm>
#include <stdexcept>
#include <cassert>
#include <atomic>
#include <mutex>          // std::mutex, std::unique_lock

#include <NTL/BasicThreadPool.h>
#include "binaryArith.h"

#define BPL_ESTIMATE (30)
// FIXME: this should really be dynamic

NTL_CLIENT

#ifdef DEBUG_PRINTOUT
#include "debugging.h"

void decryptAndSum(ostream& s, const CtPtrMat& numbers, bool negative=false);
#endif

typedef std::pair<long,long> NodeIdx; // nodes are indexed by a pair (i,j)

/**
 * @class DAGnode
 * @brief A node in an addition-DAG structure.
 *
 **/
class DAGnode {
public:
  NodeIdx idx; // the indexes of this node
  bool isQ;    // if not then isP
  long level;   // The level at the time of computation

  std::atomic_long childrenLeft; // how many children were not computed yet
  DAGnode *parent1, *parent2;

  std::mutex ct_mtx; // controls access to ctxt pointer (and the ctxt itself)
  Ctxt* ct;          // points to the actual ciphertext (or NULL)

  DAGnode(NodeIdx ii, bool qq, long lvl, long chl=0,
           DAGnode* pt1=nullptr, DAGnode* pt2=nullptr):
    idx(ii), isQ(qq), level(lvl),
    childrenLeft(chl), parent1(pt1), parent2(pt2), ct(nullptr) {}

  DAGnode(DAGnode &&other): // move constructor
    idx(other.idx), isQ(other.isQ), level(other.level),
    childrenLeft(long(other.childrenLeft)),// copy value of atomic_long
    parent1(other.parent1), parent2(other.parent2), ct(other.ct) {}

  std::string nodeName() const
  { return (std::string(isQ? "Q(":"P(")
            +std::to_string(idx.first)+','+ std::to_string(idx.second) +')');}
};

//! A class to help manage the allocation of temporary Ctxt objects
class ScratchCell {
public:
  std::atomic_bool used;
  std::unique_ptr<Ctxt> ct; // scratch space owns this pointer
  ScratchCell(const Ctxt& c): used(true), ct(new Ctxt(ZeroCtxtLike,c)) {}
  ScratchCell(ScratchCell&& other): // move constructor
    used(bool(other.used)), ct(std::move(other.ct)) {}
};

/**
 * @class AddDAG
 * @brief A class representing the logic of the order of bit products when
 *        adding two integers.
 *
 * Given two input arrays a[], b[], we build a DAG with each node representing
 * a term either of the form p_{i,j} = \prod_{t=j}^i (a[t]+b[t]), or of the
 * form q_{i,j} = (a[j]*b[j]) * \prod_{t=j+1}^i (a[t]+b[t]). The source nodes
 * are of the forms (a[i]*b[i]) and (a[i]+b[i]), and each non-source node has
 * exactly two parents, whose product yeilds that node.
 *
 * When building the DAG, we keep the level of each node as high as possible.
 * For example we can set q_{i,j}=p_{i,k}*q_{k-1,j} or q_{i,j}=p_{i,k+1}*q_{k,j}
 * (among other options), and we choose the option that results in the highest
 * level. In addition, we try to minimize the number of nodes in the DAG that
 * actually need to be computed while adding the two numbers (subject to still
 * consuming as few levels as possible).
 **/
class AddDAG {
  std::mutex scratch_mtx;  // controls access to scratch vector
  std::vector<ScratchCell> scratch; // scratch space for ciphertexts
  std::map<NodeIdx,DAGnode> p; // p[i,j]= prod_{t=j}^i (a[t]+b[t])
  std::map<NodeIdx,DAGnode> q; // q[i,j]= a[j]b[j]*prod_{t=j+1}^i (a[t]+b[t])
  long aSize, bSize;

  Ctxt* allocateCtxtLike(const Ctxt& c); // Allocate a new ciphertext if needed
  void markAsAvailable(DAGnode* node);  // Mark temporary Ctxt object as unused
  const Ctxt& getCtxt(DAGnode* node,    // Compute a new Ctxt if neeed
                      const CtPtrs& a, const CtPtrs& b);

  // Add to c the Ctxt from the given node
  void addCtxtFromNode(Ctxt& c, DAGnode* node,
                       const CtPtrs& a, const CtPtrs& b) {
    std::unique_lock<std::mutex> lck(node->ct_mtx);
    c += getCtxt(node, a, b);
    if (--(node->childrenLeft) == 0) markAsAvailable(node);
  }

public:
  //! Build a plan to add a and b
  void init(const CtPtrs& a, const CtPtrs& b);

  // Build the addition DAG
  AddDAG(const CtPtrs& a, const CtPtrs& b) { init(a,b); }

  //! Perform the actual addition
  void apply(CtPtrs& sum, const CtPtrs& a, const CtPtrs& b, long sizeLimit=0);

  //! Returns the lowest level in this DAG
  long lowLvl() const
  {
    if (aSize<1) return 0;
    return findQ(bSize-1,0)->level;
  }
  //! Returns a pointer to the a 'p' node of index (i,j)
  DAGnode* findP(long i, long j) const { // returns NULL if not exists
    auto it = p.find(NodeIdx(i,j));
    if (it == p.end()) {
      cerr << "  findP("<<i<<','<<j<<") not found\n";
      return nullptr;  // not found
    }
    return (DAGnode*)&(it->second);
  }
  //! Returns a pointer to the a 'q' node of index (i,j)
  DAGnode* findQ(long i, long j) const { // returns NULL if not exists
    auto it = q.find(NodeIdx(i,j));
    if (it == q.end()) {
      cerr << "  findQ("<<i<<','<<j<<") not found\n";
      return nullptr;  // not found
    }
    return (DAGnode*)&(it->second);
  }
#ifdef DEBUG_PRINTOUT
  void printAddDAG(bool printCT=false);
#endif
};

// When searching for a good middle point, we use a "good default", so that
// in the cases covered by that default solution we get the smallest number
// of nodes with childrenLeft>0.
// When initializing p[i,j] the default should be p[i,i+1-2^e]*p[i-2^e,j]
// where e is the largest exponent with 2^e <= i-j.
inline long defaultPmiddle(long delta)
{
  return 1 << (NTL::NumBits(delta)-1);
}
// When initializing q[i,j] the default should be p[i,i+1-2^e]*q[i-2^e,j]
// where e is the largest exponent with 2^e+2^{e-1} <= i-j.
inline long defaultQmiddle(long delta)
{
  delta = (2*delta+2)/3; // ceil(2 delta / 3)
  return 1 << (NTL::NumBits(delta)-1);
}

//! Build a plan to add a and b
void AddDAG::init(const CtPtrs& aa, const CtPtrs& bb)
{
  // make sure that lsize(b) >= lsize(a)
  const CtPtrs& a = (lsize(bb)>=lsize(aa))? aa : bb;
  const CtPtrs& b = (lsize(bb)>=lsize(aa))? bb : aa;

  aSize = lsize(a);
  bSize = lsize(b);
  assert (aSize>=1);

  // Initialize the p[i,i]'s and q[i,i]'s
  p.clear();
  q.clear();
  for (long i=0; i<bSize; i++) {
    NodeIdx idx(i,i);
    long lvl = (b.isSet(i) && !(b[i]->isEmpty()))? // The level of b[i]
      b[i]->bitCapacity() : LONG_MAX;
    if (i<aSize) {
      long aLvl = (a.isSet(i) && !(a[i]->isEmpty()))? // The level of a[i]
        a[i]->bitCapacity() : LONG_MAX;
      lvl = std::min(lvl, aLvl);
      if (lvl==LONG_MAX || aLvl==LONG_MAX) // is either a[i] or b[i] is empty
           q.emplace(idx,DAGnode(idx, true, LONG_MAX, 1));
      else q.emplace(idx,DAGnode(idx, true, lvl-1, 1));
    }
    p.emplace(idx,DAGnode(idx, false, lvl, 1));
  }

  // Initialize p[i,j] for bSize>=i>j>0
  for (long delta=1; delta<bSize; delta++) for (long i=bSize-1; i>=delta; --i) {
      long j = i-delta;
      long mid= i-defaultPmiddle(delta); // initialize to a "good default"
      DAGnode* prnt2 = findP(i,mid+1);
      DAGnode* prnt1 = findP(mid,j);
      long maxLvl = std::min(prnt1->level, prnt2->level) - BPL_ESTIMATE;
      if (prnt1->level==LONG_MAX || prnt2->level==LONG_MAX) // parent is empty
        maxLvl = LONG_MAX;
      long maxN =std::min(long(prnt1->childrenLeft), long(prnt2->childrenLeft));
      for (long m=j; m<i; m++) { // find middle point maximizing lvl(p[i,j])
        if (m==mid) continue;
        DAGnode* p2 = findP(i,m+1);
        DAGnode* p1 = findP(m,j);
        long lvl = std::min(p1->level, p2->level) - BPL_ESTIMATE;
        if (p1->level==LONG_MAX || p2->level==LONG_MAX) // parent is empty
          lvl = LONG_MAX;
        long n = std::min(long(p1->childrenLeft), long(p2->childrenLeft));
        if (lvl > maxLvl || (lvl == maxLvl && n > maxN)) {
          maxLvl = lvl;
          maxN = n;
          prnt1 = p1;
          prnt2 = p2;
        }
      }
      NodeIdx idx(i,j);
      p.emplace(idx, DAGnode(idx,false,maxLvl,0,prnt1,prnt2));
      prnt1->childrenLeft++;
      prnt2->childrenLeft++;
  }

  // Initialize q[i,j] for bSize>=i>j>=0
  for (long delta=1; delta<bSize; delta++) for (long i=bSize-1; i>=delta; --i) {
      long j = i-delta;
      if (j>=aSize) continue;
      long maxLvl=0, maxN=0;
      long mid= i-defaultQmiddle(delta); // initialize to a "good default"
      DAGnode* prnt2 = findP(i,mid+1);
      DAGnode* prnt1 = findQ(mid,j);
      if (prnt1!=nullptr) {
        maxLvl = std::min(prnt1->level, prnt2->level) - BPL_ESTIMATE;
        if (prnt1->level==LONG_MAX || prnt2->level==LONG_MAX)// parent is empty
          maxLvl = LONG_MAX;
        maxN = long(prnt2->childrenLeft);
      }
      for (long m=j; m<i; m++) { // find middle point maximizing lvl(p[i,j])
        if (m==mid) continue;
        DAGnode* p2 = findP(i,m+1);
        DAGnode* p1 = findQ(m,j);
        if (p1==nullptr) continue;
        long lvl = std::min(p1->level, p2->level) - BPL_ESTIMATE;
        if (p1->level==LONG_MAX || p2->level==LONG_MAX) // parent is empty
          lvl = LONG_MAX;
        long n = long(p2->childrenLeft);
        if (lvl > maxLvl || (lvl == maxLvl && n > maxN)) {
          maxLvl = lvl;
          maxN = n;
          prnt1 = p1;
          prnt2 = p2;
        }
      }
      if  (prnt1==nullptr) continue; // cannot create node
      NodeIdx idx(i,j);
      q.emplace(idx, DAGnode(idx,true,maxLvl,1,prnt1,prnt2));
      prnt1->childrenLeft++;
      prnt2->childrenLeft++;
    }
}

//! Apply the DAG to actually compute the sum
void AddDAG::apply(CtPtrs& sum,
                   const CtPtrs& aa, const CtPtrs& bb, long sizeLimit)
{
  // make sure that lsize(b) >= lsize(a)
  const CtPtrs& a = (lsize(bb)>=lsize(aa))? aa : bb;
  const CtPtrs& b = (lsize(bb)>=lsize(aa))? bb : aa;
  if (aSize != lsize(a) || bSize != lsize(b))
    throw std::logic_error("DAG applied to wrong vectors");

  if (sizeLimit==0)
    sizeLimit = bSize+1;
  if (lsize(sum)!=sizeLimit)
    sum.resize(sizeLimit, &b); // allocate space for the output
  for (long i=0; i<lsize(sum); i++)
    sum[i]->clear();

  // Allow multi-threading in this loop
  NTL_EXEC_RANGE(sizeLimit, first, last)
  for (long i=first; i<last; i++) { //  for (long i=0; i<sizeLimit; i++) {
    if (i<bSize)
      addCtxtFromNode(*(sum[i]), this->findP(i,i), a, b);
    for (long j=std::min(i-1, aSize-1); j>=0; --j) {
      DAGnode *node = this->findQ(i-1,j);
      if (node!=nullptr) addCtxtFromNode(*(sum[i]), node, a, b);
    }
  }
  NTL_EXEC_RANGE_END
}

//! Get the ciphertext for a node, compiuting it as needed
const Ctxt& AddDAG::getCtxt(DAGnode* node,
                            const CtPtrs& a, const CtPtrs& b)
{
  // NOTE: node->ct_mtx should be locked before calling this function

  if (node->ct == nullptr) { // ciphertext not computed yet, do it now
    if (node->parent1!=nullptr && node->parent2!=nullptr) { // internal node
      // Obtain locks and ciphertexts for both parents. Also reduce the
      // number of children of that parents that still need to be computed
      std::unique_lock<std::mutex> pt1_lck(node->parent1->ct_mtx);
      const Ctxt& c1 = getCtxt(node->parent1, a, b);
      long n1 = --(node->parent1->childrenLeft);

      std::unique_lock<std::mutex> pt2_lck(node->parent2->ct_mtx);
      const Ctxt& c2 = getCtxt(node->parent2, a, b);
      long n2 = --(node->parent2->childrenLeft);

      if (n1==0) {              // reuse space from parent1
        node->parent1->ct = nullptr;
        node->ct = (Ctxt*) &c1;
        if (c1.isEmpty() || c2.isEmpty())
             node->ct->clear(); // ct is zero if any of the parents is
	else node->ct->multiplyBy(c2);
        if (n2==0)
          markAsAvailable(node->parent2);
      }
      else if (n2==0) {         // reuse space from parent2
        node->parent2->ct = nullptr;
        node->ct = (Ctxt*) &c2;
        if (c1.isEmpty() || c2.isEmpty())
             node->ct->clear(); // ct is zero if any of the parents is
	else node->ct->multiplyBy(c1);
      }
      else {                    // allocate new space
        node->ct = allocateCtxtLike(c2);
        if (c1.isEmpty() || c2.isEmpty())
          node->ct->clear();    // ct is zero if any of the parents is
        else {
          *(node->ct) = c2;
          node->ct->multiplyBy(c1);
        }
      }
    }
    else { // no parents, either a[i]+b[i] or a[i]*b[i]
      long i = node->idx.first;
      long j = node->idx.second; // we expect i==j
      const Ctxt* ct_ptr = b.ptr2nonNull();
      assert(ct_ptr != nullptr);
      node->ct = allocateCtxtLike(*ct_ptr);

      if (node->isQ) { // This is b[i]*a[j]
        if (b.isSet(i) && !(b[i]->isEmpty())
            && a.isSet(j) && !(a[j]->isEmpty())) {
          *(node->ct) = *(b[i]);
          node->ct->multiplyBy(*(a[j]));
        } // if a[j] or b[i] is empty then node->ct is a zero ciphertext
	else node->ct->clear();
      }
      else {           // This is b[i]+a[i]
        if (!b.isSet(i) || b[i]->isEmpty())
	     node->ct->clear();
        else *(node->ct) = *(b[i]);
        if (a.isSet(j) && !(a[j]->isEmpty()))
          *(node->ct) += *(a[j]);
      }
    } // end of no-parents case
  }
  return *(node->ct);
}

//! Adds another cell to scratch space, or use an existing one that's free
Ctxt* AddDAG::allocateCtxtLike(const Ctxt& c)
{
  // look for an unused cell in the scratch array
  for (long i=0; i<lsize(scratch); i++)
    if (scratch[i].used == false) { // found a free one, try to use it
      bool used = scratch[i].used.exchange(true); // mark it as used
      if (used==false)     // make sure no other thread got there first
        return scratch[i].ct.get();
    }

  // If not found, allocate a new cell
  ScratchCell sc(c);      // cell points to new ctxt, with used=true
  Ctxt* pt = sc.ct.get(); // remember the raw pointer
  std::unique_lock<std::mutex> lck(scratch_mtx);   // protect scratch vector
  scratch.emplace_back(std::move(sc));  // scratch now owns the pointer
  return pt;              // return the raw pointer
}

// Mark a scratch ciphertext as unused. We assume that no two nodes
// ever share a ciphertext object, so if this node is done with the
// object then the object is unused.
void AddDAG::markAsAvailable(DAGnode* node)
{
  // NOTE: node->ct_mtx should be locked before calling this function
  // NOTE: somewhat inefficient, use linear search for the raw pointer
  for (long i=0; i<(long)scratch.size(); i++)
    if (scratch[i].ct.get()==node->ct)
      scratch[i].used = false;
  node->ct = nullptr;
}

/********************************************************************/
/********************************************************************/

// Use packed bootstrapping, so we can bootstrap all in just one go.
void packedRecrypt(const CtPtrs& a, const CtPtrs& b,
                   std::vector<zzX>* unpackSlotEncoding)
{
  const Ctxt* ct = b.ptr2nonNull(); // find some non-null Ctxt
  if (ct==nullptr) ct = a.ptr2nonNull();
  if (ct==nullptr) return;    // nothing to do

  assert(unpackSlotEncoding!=nullptr);
  assert(ct->getPubKey().isBootstrappable());

  struct CtPtrs_pair : CtPtrs {
    const CtPtrs& a;
    const CtPtrs& b;
    CtPtrs_pair(const CtPtrs &_a, const CtPtrs &_b): a(_a), b(_b) {}
    Ctxt* operator[](long i) const override
    { return (i<lsize(a))? a[i] : b[i-lsize(a)]; }
    long size() const override { return lsize(a) + lsize(b); }
  };
  const CtPtrs_pair ab(a,b);

  packedRecrypt(ab, *unpackSlotEncoding, *(ct->getContext().ea));
}

//! Add two integers in binary representation
void addTwoNumbers(CtPtrs& sum, const CtPtrs& a, const CtPtrs& b,
                   long sizeLimit, std::vector<zzX>* unpackSlotEncoding)
{
  FHE_TIMER_START;
  if (lsize(a)<1)      { vecCopy(sum,b,sizeLimit); return; }
  else if (lsize(b)<1) { vecCopy(sum,a,sizeLimit); return; }

  // Work out the order of multiplications to compute all the carry bits
  AddDAG addPlan(a,b);

#ifdef DEBUG_PRINTOUT // print plan
  addPlan.printAddDAG();
#endif

  // Ensure that we have enough levels to compute everything,
  // bootstrap otherwise
  if (addPlan.lowLvl()< BPL_ESTIMATE) {
    packedRecrypt(a,b,unpackSlotEncoding);
    addPlan.init(a,b); // Re-compute the DAG
    if (addPlan.lowLvl()<BPL_ESTIMATE) { // still not enough levels
      throw std::logic_error("not enough levels for addition DAG");
    }
  }
  addPlan.apply(sum, a, b, sizeLimit);    // perform the actual addition
}

// Return pointers to the three inputs, ordered by size
static std::tuple<const CtPtrs*,const CtPtrs*,const CtPtrs*>
orderBySize(const CtPtrs& a, const CtPtrs& b, const CtPtrs& c)
{
  if (lsize(a) <= lsize(b)) {
    if (lsize(b) <= lsize(c))
      return std::make_tuple(&a,&b,&c); // a <= b <= c
    else if (lsize(a) <= lsize(c))
      return std::make_tuple(&a,&c,&b); // a <= c < b
    else
      return std::make_tuple(&c,&a,&b); // c < a <= b
  } else { // lsize(b) < lsize(a)
    if (lsize(a) <= lsize(c))
      return std::make_tuple(&b,&a,&c); // b < a <= c
    else if (lsize(b) <= lsize(c))
      return std::make_tuple(&b,&c,&a); // b <= c < a
    else
      return std::make_tuple(&c,&b,&a); // c < b < a
  }
}

// Implementing the basic 3-for-2 trick: u,v,w encrypt bits, return two bits
// x,y such that x+2y = u+v+w over the integers. Outputs can alias the inputs.
static void three4Two(Ctxt& lsb, Ctxt& msb,
                      const Ctxt& u, const Ctxt& v, const Ctxt& w)
{
  Ctxt tmp_v = v;
  Ctxt tmp_w = w;
  lsb = u;
  msb = u;

  lsb += tmp_v;          // u+v
  msb.multiplyBy(tmp_v); // u*v

  tmp_v = lsb;             // u+v

  tmp_v.multiplyBy(tmp_w); // (u+v)*w

  lsb += tmp_w;            // u+v+w
  msb += tmp_v;   // u*v + (u+v)*w = u*v + u*w + v*w
}

// Same as three4Two above, but some of the inputs could be null.
// Returns the number of output bits that are not identically zero.
static long three4Two(Ctxt* lsb, Ctxt* msb, Ctxt* u, Ctxt* v, Ctxt* w)
{
  if (u!=nullptr && !u->isEmpty()
      && v!=nullptr && !v->isEmpty()
      && w!=nullptr && !w->isEmpty()) { // if none are empty
    three4Two(*lsb,*msb, *u, *v, *w);   // call the function above
    return 2;
  }
  if ((u==nullptr||u->isEmpty())
      && (v==nullptr||v->isEmpty())
      && (w==nullptr||w->isEmpty())) {  // if all are empty
    lsb->clear();                       // result is emptry too
    msb->clear();
    return 0;
  }

  // Some are empty, others are not, arrange so that emptys are at the end
  if (u==nullptr||u->isEmpty()) {
    if (v==nullptr||v->isEmpty()) u=w;    // only w was non-empty
    else { u=v; v=w; }                    // v,w were non-empty
  }
  else if (v==nullptr||v->isEmpty()) v=w; // u is non-empty, v was empty
  w = nullptr;                            // we don't use w anymore

  if (v==nullptr||v->isEmpty()) {         // only u is non-empty
    *lsb = *u;
    msb->clear();
    return 1;
  }

  // both u,v are non-empty
  Ctxt tmp = *v;
  *lsb = *u;
  *msb = *u;
  *lsb += tmp;
  msb->multiplyBy(tmp);
  return 2;
}

// Apply the 3-for-2 routine to integers (i.e., an array of bits). The
// inputs need not be of the same size, and size of the output x is
// equal to the largest of them, and the size of the output y is one
// larger. This is safe even when the outputs alias some of the inputs
static void three4Two(CtPtrs& lsb, CtPtrs& msb,
                      const CtPtrs& u, const CtPtrs& v, const CtPtrs& w,
                      long sizeLimit)
{
  FHE_TIMER_START;
  // Arrange u,v,w by size from smallest to largest
  const CtPtrs *p1, *p2, *p3;
  std::tie(p1,p2,p3) = orderBySize(u,v,w); // size(p3)>=size(p2)>=size(p1)

  if (p3->size() <= 0) { // empty input
    setLengthZero(lsb);
    setLengthZero(msb);
    return;
  }
  if (p1->size()<=0) { // two or less inputs
    std::vector<Ctxt> tmp;
    vecCopy(tmp,*p2,sizeLimit); // just in case p2, msb share pointers
    vecCopy(msb,*p3,sizeLimit);
    vecCopy(lsb,tmp);
    return;
  }
  if (sizeLimit==0) sizeLimit = p3->size()+1;

  // Allocate space in the output vectors

  const Ctxt* ctptr = p3->ptr2nonNull();
  std::vector<Ctxt> tmpMsb, tmpLsb;

  long lsbSize = std::min(sizeLimit, lsize(*p3));
  long msbSize = lsbSize;
  if (lsize(*p2)==lsize(*p3) && lsbSize<sizeLimit)
    msbSize++;                  // possible carry out of last position

  resize(tmpLsb, lsbSize, Ctxt(ZeroCtxtLike,*ctptr));
  resize(tmpMsb, msbSize, Ctxt(ZeroCtxtLike,*ctptr));

  NTL_EXEC_RANGE(msbSize-1, first, last)
  for (long i=first; i<last; i++) {
    if (i<lsize(*p1))
      three4Two(&tmpLsb[i], &tmpMsb[i+1], (*p1)[i], (*p2)[i], (*p3)[i]);
    else if (i<lsize(*p2)) {
      three4Two(&tmpLsb[i], &tmpMsb[i+1], (*p2)[i], (*p3)[i], nullptr);
    }
    else if (p3->isSet(i)) tmpLsb[i] = *((*p3)[i]);
  }
  NTL_EXEC_RANGE_END

  if (msbSize==lsbSize) { // we only computed upto lsbSize-1, do the last LSB
    if (p1->isSet(lsbSize-1)) tmpLsb[lsbSize-1] =  *((*p1)[lsbSize-1]);
    if (p2->isSet(lsbSize-1)) tmpLsb[lsbSize-1] += *((*p2)[lsbSize-1]);
    if (p3->isSet(lsbSize-1)) tmpLsb[lsbSize-1] += *((*p3)[lsbSize-1]);
  }
  vecCopy(lsb, tmpLsb);
  vecCopy(msb, tmpMsb);
}

//! @brief An implementation of PtrMatrix using vector< PtrVector<T>* >
template<typename T>
struct PtrMatrix_PtPtrVector : PtrMatrix<T> {
  std::vector< PtrVector<T>* >& rows;
  PtrMatrix_PtPtrVector(std::vector< PtrVector<T>* >& mat): rows(mat) {}
  PtrVector<T>& operator[](long i) override             // returns a row
  { return *rows[i]; }
  const PtrVector<T>& operator[](long i) const override // returns a row
  { return *rows[i]; }
  long size() const override { return lsize(rows); }    // How many rows
};

// Calculates the sum of many numbers using the 3-for-2 method
void addManyNumbers(CtPtrs& sum, CtPtrMat& numbers, long sizeLimit,
                    std::vector<zzX>* unpackSlotEncoding)
{
#ifdef DEBUG_PRINTOUT
  cout << " addManyNumbers: "<<numbers.size()
       << " numbers with size-limit="<<sizeLimit<<endl;
#endif
  FHE_TIMER_START;
  const Ctxt* ct_ptr = numbers.ptr2nonNull();
  if (lsize(numbers)<1 || ct_ptr==nullptr) { // nothign to add
    setLengthZero(sum);
    return;
  }
  if (lsize(numbers)==1) { vecCopy(sum, numbers[0]); return; }

  bool bootstrappable = ct_ptr->getPubKey().isBootstrappable();
  const EncryptedArray& ea = *(ct_ptr->getContext().ea);

  long leftInQ = lsize(numbers);
  std::vector<CtPtrs*> numPtrs(leftInQ);
  for (long i=0; i<leftInQ; i++) numPtrs[i] = &(numbers[i]);

  // use 3-for-2 repeatedly until only two numbers are leff to add
  while (leftInQ>2) {
    // If any number is too low level, then bootstrap everything
    PtrMatrix_PtPtrVector<Ctxt> wrapper(numPtrs);
    if (findMinBitCapacity(wrapper)<3*ct_ptr->getContext().BPL()) {
      assert(bootstrappable && unpackSlotEncoding!=nullptr);
      packedRecrypt(wrapper, *unpackSlotEncoding, ea, /*belowLvl=*/10);
    }
    // Prepare a vector for pointers to the output of this iteration
    long nTriples = leftInQ/3;
    long leftOver = leftInQ - (3*nTriples);
    std::vector<CtPtrs*> numPtrs2(2*nTriples +leftOver);

    if (leftOver>0) { // copy the leftover pointers
      numPtrs2[0] = numPtrs[3*nTriples];
      if (leftOver>1) numPtrs2[1] = numPtrs[3*nTriples +1];
    }
    // Allow multi-threading in this loop
    //    NTL_EXEC_RANGE(nTriples, first, last)
    //    for (long i=first; i<last; i++) {   // call the three-for-two procedure
    for (long i=0; i<nTriples; i++) {   // call the three-for-two procedure
      three4Two(*numPtrs[3*i], *numPtrs[3*i+1], // three4Two works in-place
                *numPtrs[3*i], *numPtrs[3*i+1], *numPtrs[3*i+2], sizeLimit);

      numPtrs2[leftOver +2*i]    = numPtrs[3*i]; // copy the output pointers
      numPtrs2[leftOver +2*i +1] = numPtrs[3*i +1];
    }
    //    NTL_EXEC_RANGE_END
    numPtrs.swap(numPtrs2);   // swap input/output vectors
    leftInQ = lsize(numPtrs); // update the size
  }
  // final addition
  addTwoNumbers(sum, *numPtrs[0], *numPtrs[1], sizeLimit, unpackSlotEncoding);
}

// Multiply a positive a by a potentially negative b, we need to sign-extend b
static void multByNegative(CtPtrs& product, const CtPtrs& a, const CtPtrs& b,
                           long sizeLimit,std::vector<zzX>* unpackSlotEncoding)
{
  FHE_TIMER_START;
  long resSize = lsize(a)+lsize(b);
  if (sizeLimit>0 && sizeLimit<resSize) resSize=sizeLimit;

  NTL::Vec< NTL::Vec<Ctxt> > numbers(INIT_SIZE, std::min(lsize(a),resSize));
  long nNums = lsize(numbers);
  for (long i=0; i<nNums; i++)
    numbers[i].SetLength(resSize, Ctxt(ZeroCtxtLike,*(a[0])));

  std::vector<std::pair<long,long> > pairs;
  for (long i=0; i<nNums; i++) for (long j=i; j<resSize; j++)
    if (j<i+lsize(b) && a.isSet(i)  && !a[i]->isEmpty()
                     && b.isSet(j-i)&& !b[j-i]->isEmpty()) {
      pairs.push_back(std::pair<long,long>(i,j));
    }
  long nPairs = lsize(pairs);

  NTL_EXEC_RANGE(nPairs, first, last)
  for (long idx=first; idx<last; idx++) {
    long i,j; std::tie(i,j) = pairs[idx];
    numbers[i][j] = *(b[j-i]);
    numbers[i][j].multiplyBy(*(a[i]));   // multiply by the bit of a
  }
  NTL_EXEC_RANGE_END

  // sign extension
  for (long i=0; i<nNums; i++) for (long j=i+lsize(b); j<resSize; j++) {
      numbers[i][j] = numbers[i][i+lsize(b)-1]; // sign extension
    }

  CtPtrMat_VecCt nums(numbers); // Wrapper around numbers
#ifdef DEBUG_PRINTOUT
  long pa, pb;
  vector<long> slots;
  decryptBinaryNums(slots, a, *dbgKey, *dbgEa, false); pa=slots[0];
  decryptBinaryNums(slots, b, *dbgKey, *dbgEa, true);  pb=slots[0];
  decryptAndSum((cout<<" multByNegative: "<<pa<<'*'<<pb<<" = "),
                nums, true);
#endif
  addManyNumbers(product, nums, resSize, unpackSlotEncoding);
}

// Multiply two integers (i.e. an array of bits) a, b.
// Computes the pairwise products x_{i,j} = a_i * b_j
// then sums the prodcuts using the 3-for-2 method.
void multTwoNumbers(CtPtrs& product, const CtPtrs& a, const CtPtrs& b,
                    bool bNegative, long sizeLimit,
                    std::vector<zzX>* unpackSlotEncoding)
{
  FHE_TIMER_START;
  long aSize = lsize(a);
  long bSize = lsize(b);
  long resSize = aSize+bSize;
  if (sizeLimit>0 && sizeLimit<resSize) resSize=sizeLimit;

  if (a.numNonNull()<1 || b.numNonNull()<1) {
    setLengthZero(product);
    return; // return 0
  }

#ifdef DEBUG_PRINTOUT
  cout << " before multiplication, capacity="<<findMinBitCapacity({&a, &b})
       << endl;
#endif
  // Edge case, if a or b is 1 bit
  if (aSize==1) {
    if (a[0]->isEmpty()) {
      setLengthZero(product);
      return;
    }
    vecCopy(product,b,resSize);
    for (long i=0; i<resSize; i++)
      product[i]->multiplyBy(*(a[0]));
    return;
  }
  if (bNegative) { // somewhat different implementation for 2s complement
    multByNegative(product, a, b, sizeLimit, unpackSlotEncoding);
    return;
  }
  if (bSize==1) {
    if (b[0]->isEmpty()) {
      setLengthZero(product);
      return;
    }
    vecCopy(product,a,resSize);
    for (long i=0; i<resSize; i++)
      a[i]->multiplyBy(*(b[0]));
    return;
  }

  // We make sure aa is the larger of the two integers
  // to keep the number of additions to a minimum
  const CtPtrs& aa = (aSize>=bSize)? a : b;
  const CtPtrs& bb = (aSize>=bSize)? b : a;
  aSize = lsize(aa);
  bSize = lsize(bb);

  NTL::Vec< NTL::Vec<Ctxt> > numbers(INIT_SIZE, std::min(lsize(b),resSize));
  const Ctxt* ct_ptr = a.ptr2nonNull();
  long nNums = lsize(numbers);
  for (long i=0; i<nNums; i++)
    numbers[i].SetLength(std::min((i+aSize),resSize),
                         Ctxt( ZeroCtxtLike,*ct_ptr ) );
  std::vector<std::pair<long,long> > pairs;
  for (long i=0; i<nNums; i++) for (long j=i; j<lsize(numbers[i]); j++) {
    if (a.isSet(j-i)&& !(a[j-i]->isEmpty())&& b.isSet(i)&& !(b[i]->isEmpty()))
      pairs.push_back(std::pair<long,long>(i,j));
  }
  long nPairs = lsize(pairs);
  NTL_EXEC_RANGE(nPairs, first, last)
  for (long idx=first; idx<last; idx++) {
    long i,j; std::tie(i,j) = pairs[idx];
    numbers[i][j] = *(a[j-i]);
    numbers[i][j].multiplyBy(*(b[i])); // multiply by the bit of b
  }
  NTL_EXEC_RANGE_END

  CtPtrMat_VecCt nums(numbers); // A wrapper aroune numbers
#ifdef DEBUG_PRINTOUT
  long pa, pb;
  vector<long> slots;
  decryptBinaryNums(slots, a, *dbgKey, *dbgEa, false); pa=slots[0];
  decryptBinaryNums(slots, b, *dbgKey, *dbgEa, false);  pb=slots[0];
  decryptAndSum((cout<<" multTwoNumbers: "<<pa<<'*'<<pb<<" = "),
                nums, false);
#endif
  addManyNumbers(product, nums, resSize, unpackSlotEncoding);
}

/* seven4Three: adding seven input bits, getting a 3-bit counter
 *
 * input: in[6..0]
 * ----------------
 *         in[6]
 *       b2 b1 = sum of in[2..0] (b2=msb, b1=lsb)
 *       b4 b3 = sum of in[5..3]
 * ------------
 *       c2 c1 = sum of in[6],b1,b3
 *    c4 c3    = sum of b2,b4
 * ------------
 *    d2 d1 c1 = out[2..0]
 */
// The output Ctxts[0..2] must be initialized, can alias the inputs
static void seven4Three(const CtPtrs& out, const CtPtrs& in, long sizeLimit)
{
  // we need 4 scratch ciphertexts
  std::vector<Ctxt> tmp(4, *out[0]);

  // Aliasas, referring to the scheme above. Aliases for temporary
  // vars chosen so that inputs, outputs of three4two are distinct

  Ctxt& c1=*out[0]; Ctxt& d1=*out[1]; Ctxt& d2=*out[2];

  Ctxt& b1=tmp[0];  Ctxt& b2=tmp[1];  Ctxt& b3=tmp[2];  Ctxt& b4=tmp[3];
  Ctxt& c2=d1;  Ctxt& c3=b1;  Ctxt& c4=b3;

  three4Two(&b1,&b2,in[0],in[1],in[2]); // b2 b1 = 3for2(in[0..2])
  three4Two(&b3,&b4,in[3],in[4],in[5]); // b4 b3 = 3for2(in[3..5])

  three4Two(&c1,&c2, in[6], &b1, &b3);  // c2 c1 = 3for2(in[6],b1,b3)
  if (sizeLimit < 2) return;
  c3 = b2;

  c3 += b4;                           // c3 = b2 ^ b4
  c4 = b2;

  c4.multiplyBy(b4);                  // c4 = b2 * b4
  d2 = c2;

  d1 += c3;                           // d1 = c2 ^ c3 (d1 alias c2)
  if (sizeLimit < 3) return;
  d2.multiplyBy(c3);

  d2 += c4;                           // d2 = c4 ^ (c2*c3)
}

/* fifteen4Four: adding fifteen input bits, getting a 4-bit counter
 *
 * input: in[14..0]
 * ----------------
 *       b2 b1 = sum of in[2..0] (b2=msb, b1=lsb)
 *       b4 b3 = sum of in[5..3]
 *       b6 b5 = sum of in[8..6]
 *       b8 b7 = sum of in[11..9]
 *      b10 b9 = sum of in[14..12]
 * ------------
 *       b8 b7
 *      b10 b9
 *       c2 c1 = sum of b1,b3,b5
 *    c4 c3    = sum of b2,b4,b6
 * ------------
 *    c4 c3
 *       d2 d1 = sum of b7,b9,c1
 *    d4 d3    = sum of b8,b10,c2
 * ------------
 *          d1
 *    e2 e1    = sum of c3,d2,d3
 * e4 e3       = sum of c4,d4
 * ------------
 * f2 f1 e1 d1 = out[3..0]
 */
// The output Ctxts[0..3] must be initialized, can alias the inputs
static void fifteen4Four(const CtPtrs& out, const CtPtrs& in, long sizeLimit)
{
  // we need 6 scratch ciphertexts
  std::vector<Ctxt> tmp(8, *out[0]);

  // Aliasas, referring to the scheme above.

  Ctxt& d1=*out[0]; Ctxt& e1=*out[1]; Ctxt& f1=*out[2]; Ctxt& f2=*out[3];

  Ctxt& b1=tmp[0];  Ctxt& b2=tmp[1];  Ctxt& b3=tmp[2];  Ctxt& b4=tmp[3];
  Ctxt& b5=tmp[4];  Ctxt& b6=tmp[5];  Ctxt& c1=tmp[6];  Ctxt& c2=tmp[7];
  Ctxt& c3=b1;  Ctxt& c4=b3;
  Ctxt& b7=b5;  Ctxt& b8=b2;  Ctxt& b9=b4;  Ctxt& b10=b6;
  Ctxt& d2=b7;  Ctxt& d3=b9;  Ctxt& d4=f2;
  Ctxt& e2=c1;  Ctxt& e3=c2;  Ctxt& e4=f2;

  long nThreads = std::min(NTL::AvailableThreads(), 3L);
  NTL_EXEC_INDEX(nThreads, index)     // run these three lines in parallel
  switch (index) {
  case 0: three4Two(&b1,&b2,in[0],in[1],in[2]); // b2 b1 = 3for2(in[0..2])
    if (nThreads>1) break;
  case 1: three4Two(&b3,&b4,in[3],in[4],in[5]); // b4 b3 = 3for2(in[3..5])
    if (nThreads>2) break;
  default: three4Two(&b5,&b6,in[6],in[7],in[8]);// b6 b5 = 3for2(in[6..8])
  }
  NTL_EXEC_INDEX_END

  three4Two(c1,c2, b1, b3, b5);         // c2 c1 = 3for2(b1,b3,b5)

  three4Two(c3,c4, b2, b4, b6);         // c4 c3 = 3for2(b2,b4,b6)

  nThreads = std::min(NTL::AvailableThreads(), 2L);
  NTL_EXEC_INDEX(nThreads, index)       // run these two lines in parallel
  switch (index) {
  case 0: three4Two(&b7,&b8,in[9],in[10],in[11]);   // b8 b7 = 3for2(in[9..11])
    if (nThreads>1) break;
  default: three4Two(&b9,&b10,in[12],in[13],in[14]);// b10 b9 = 3for2(in[12..14])
  }
  NTL_EXEC_INDEX_END

  NTL_EXEC_INDEX(nThreads, index)       // run these two lines in parallel
  switch (index) {
  case 0: three4Two(d1,d2, b7, b9, c1); // d2 d1 = 3for2(b7,b9,c1)
    if (nThreads>1) break;
  default: if (sizeLimit >= 2)
      three4Two(d3,d4, b8, b10, c2);    // d4 d3 = 3for2(b8,b10,c2)
  }
  NTL_EXEC_INDEX_END
  if (sizeLimit < 2) return;

  NTL_EXEC_INDEX(nThreads, index)       // run these two blocks in parallel
  switch (index) {
  case 0: three4Two(e1,e2, c3, d2, d3); // e2 e1 = 3for2(c3,d2,d3)
    if (nThreads>1) break;
  default: if (sizeLimit >= 3) {
      e3 = c4;
      e3 += d4;                         // e3 = c4 ^ d4
      e4.multiplyBy(c4);                // e4 = c4 * d4 (e4 alias d4)
    }
  }
  NTL_EXEC_INDEX_END
  if (sizeLimit < 3) return;

  f1 = e2;
  f1 += e3;                             // f1 = e2 ^ e3
  if (sizeLimit < 4) return;
  e2.multiplyBy(e3);
  f2 += e2;                             // f2 = e4^(e2*e3)  (f2 alias e4)
}
// Same as above, but some of the pointers may be null.
// Returns number of output bits that are not identically zero.
long fifteenOrLess4Four(const CtPtrs& out, const CtPtrs& in, long sizeLimit)
{
  FHE_TIMER_START;
  long numNonNull = in.numNonNull();
  if (numNonNull>7) {
    fifteen4Four(out, in, sizeLimit);
    return 4;
  }

  // At most 7 non-null pointers, collect them in the first entires of a vector
  long lastNonNull = -1;
  std::vector<Ctxt*> inPtrs(7,nullptr);
  for (long i=0; i<15; i++)
    if (in.isSet(i)) inPtrs[++lastNonNull] = in[i];

  if (numNonNull>3) {
    seven4Three(out, CtPtrs_vectorPt(inPtrs), sizeLimit);
    out[3]->clear(); // msb is zero
    return 3;
  }
  numNonNull = three4Two(out[0],out[1], inPtrs[0],inPtrs[1],inPtrs[2]);
  out[3]->clear(); // msb is zero
  out[2]->clear(); // 2nd msb is zero
  return numNonNull;
}

/********************************************************************/
/***************** test/debugging functions *************************/

// Decrypt the binary numbers that are encrypted in eNums. The bits
// are encrypted in a bit-sliced manner. Namely, encNums[0] contains
// the LSB of all the numbers, encNums[1] the next bits from all, etc.
// If allSlots==false then we only return the subcube with index i=0
// in the last dimension within each ciphertext. Namely, the bit for
// the j'th counter is found in slot of index j*sizeOf(lastDim).
void decryptBinaryNums(vector<long>& pNums, const CtPtrs& eNums,
                       const FHESecKey& sKey, const EncryptedArray& ea,
                       bool negative, bool allSlots)
{
  int offset=1, size=ea.size();
  if (!allSlots) { // only slots of index i=0 in the last dimension
    offset = ea.sizeOfDimension(ea.dimension()-1);
    size /= offset;
  }
  pNums.assign(size, 0); // initialize to zero

  for (int i=0; i<lsize(eNums); i++) if (eNums.isSet(i)) {
    vector<long> slots;
    ea.decrypt(*eNums[i], sKey, slots);
    for (int j=0; j<lsize(pNums); j++)
      if (negative && i==lsize(eNums)-1)
        pNums[j] -= (slots[j*offset] << i);
      else
        pNums[j] += (slots[j*offset] << i);
  }
}

/********************************************************************/
#ifdef DEBUG_PRINTOUT
#include <cstdio>

void AddDAG::printAddDAG(bool printCT)
{
  cout << "aSize="<<aSize<<", bSize="<<bSize<<endl;
  cout << "The p[i,j]'s\n============\n";
  for (long delta=0; delta<bSize; delta++) {
    cout << "delta="<<delta<<endl;
    for (long j=0; j<bSize-delta; j++) {
      long i = j+delta;
      DAGnode* node = findP(i,j);
      if (node==nullptr) continue;
      cout << node->nodeName()<<":{ lvl=";
      if (node->level==LONG_MAX) cout << "XX";
      else                       cout << node->level;
      cout <<", chLeft="<<int(node->childrenLeft)
           <<", ct="<<node->ct;
      if (node->parent2)
        cout << ", prnt2="<< node->parent2->nodeName();
      if (node->parent1)
        cout << ", prnt1="<< node->parent1->nodeName();
      cout << " }\n";
      if (printCT && node->ct!=nullptr)
        decryptAndPrint(cout, *(node->ct), *dbgKey, *dbgEa, FLAG_PRINT_VEC);
    }
  }
  cout << "\nThe q[i,j]'s\n============\n";
  for (long delta=0; delta<bSize; delta++) {
    cout << "delta="<<delta<<endl;
    for (long j=0; j<std::min(aSize, bSize-delta); j++) {
      long i = j+delta;
      DAGnode* node = findQ(i,j);
      if (node==nullptr) continue;
      cout << node->nodeName() <<":{ lvl=";
      if (node->level==LONG_MAX) cout << "XX";
      else                       cout << node->level;
      cout <<", chLeft="<<long(node->childrenLeft)
           <<", ct="<<node->ct;
      if (node->parent2)
        cout << ", prnt2="<< node->parent2->nodeName();
      if (node->parent1)
        cout << ", prnt1="<< node->parent1->nodeName();
      cout << " }\n";
      if (printCT && node->ct!=nullptr)
        decryptAndPrint(cout, *(node->ct), *dbgKey, *dbgEa, FLAG_PRINT_VEC);
    }
  }
  cout << endl;
}

void decryptAndSum(ostream& s, const CtPtrMat& numbers, bool negative)
{
  s << "sum(";
  long sum=0;
  for (long i=0; i<numbers.size(); i++) {
    vector<long> slots;
    const CtPtrs& num = numbers[i];
    decryptBinaryNums(slots, num, *dbgKey, *dbgEa, negative);
    s << slots[0] << ' ';
    sum += slots[0];
  }
  s << ")="<<sum<<endl;
}

#endif // ifdef DEBUG_PRINTOUT


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/bool.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/iterable/mcd.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/list/mcd.hpp>
using namespace boost::hana;

struct LazyList;

template <typename X, typename Xs>
struct lazy_cons_type {
    X x;
    Xs xs;
    using hana_datatype = LazyList;
};

auto lazy_cons = [](auto x, auto xs) {
    return lazy_cons_type<decltype(x), decltype(xs)>{x, xs};
};

struct lazy_nil_type { using hana_datatype = LazyList; };

constexpr lazy_nil_type lazy_nil{};

auto repeat = fix([](auto repeat, auto x) {
    return lazy_cons(x, lazy(repeat)(x));
});

namespace boost { namespace hana {
    template <>
    struct Iterable::instance<LazyList> : Iterable::mcd {
        template <typename Xs>
        static constexpr auto head_impl(Xs lcons)
        { return lcons.x; }

        template <typename Xs>
        static constexpr auto tail_impl(Xs lcons)
        { return eval(lcons.xs); }

        template <typename Xs>
        static constexpr auto is_empty_impl(Xs lcons)
        { return false_; }

        static constexpr auto is_empty_impl(lazy_nil_type)
        { return true_; }
    };

    template <>
    struct List::instance<LazyList> : List::mcd<LazyList> {
        static constexpr auto nil_impl()
        { return lazy_nil; }

        template <typename X, typename Xs>
        static constexpr auto cons_impl(X x, Xs xs)
        { return lazy_cons(x, lazy(xs)); }
    };
}}

int main() {
    BOOST_HANA_CONSTANT_ASSERT(!is_empty(repeat(1)));
    BOOST_HANA_CONSTEXPR_ASSERT(head(repeat(1)) == 1);
    BOOST_HANA_CONSTEXPR_ASSERT(at(int_<10>, repeat(1)) == 1);

    BOOST_HANA_CONSTEXPR_ASSERT(take(int_<2>, repeat('x')) == list('x', 'x'));
}


#include <catch2/catch.hpp>
#include <glm/glm.hpp>
#include <Eigen/Dense>
#include <spdlog/spdlog.h>

#include "maths/rbf.h"

SCENARIO("Radial basis functions should allow to create custom 3D functions which goes through set points", "[rbf]") {
    GIVEN("A list of control points with their associated weights") {
        std::vector<glm::ivec3> controlPointsXYZ = {
            glm::ivec3(1, 1, 3),
            glm::ivec3(0, 0, 8),
            glm::ivec3(2, 2, 2)
        };
        Eigen::VectorXd controlPointWeights(controlPointsXYZ.size());
        controlPointWeights << 1.0f, 1.0f, 1.0f;
        const float eps = 1.0f;

        WHEN("We ask for z coordinates for a list of points on the horizontal plane") {
            std::vector<glm::ivec3> coordWhereToFindZ = {
                glm::ivec3(1, 9, 0),
                glm::ivec3(3, 2, 0),
                glm::ivec3(5, 8, 0)
            };
            std::vector<glm::ivec3> backup = coordWhereToFindZ;

            voxmt::rbfInterpolate(coordWhereToFindZ, controlPointsXYZ, controlPointWeights, voxmt::RBFType::LINEAR, eps, voxmt::RBFTransformAxis::Z);

            THEN("It should keep the same x and y values") {
                for (size_t i = 0; i < coordWhereToFindZ.size(); i++) {
                    REQUIRE(coordWhereToFindZ.at(i).x == backup.at(i).x);
                    REQUIRE(coordWhereToFindZ.at(i).y == backup.at(i).y);
                }
            }
        }
    }
}


#ifndef PYTHONIC_SCIPY_SPECIAL_KVP_HPP
#define PYTHONIC_SCIPY_SPECIAL_KVP_HPP

#include "pythonic/include/scipy/special/kvp.hpp"

#include "pythonic/types/ndarray.hpp"
#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_traits.hpp"

#define BOOST_MATH_THREAD_LOCAL thread_local
#include <boost/math/special_functions/bessel_prime.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
    namespace details
    {
      template <class T0, class T1>
      double kvp(T0 x, T1 y)
      {
        using namespace boost::math::policies;
        return boost::math::cyl_bessel_k_prime(
            x, y, make_policy(promote_double<true>()));
      }
    }

#define NUMPY_NARY_FUNC_NAME kvp
#define NUMPY_NARY_FUNC_SYM details::kvp
#include "pythonic/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


#include <Columns/ColumnArray.h>
#include <Columns/IColumn.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypesNumber.h>
#include <DataTypes/IDataType.h>
#include <DataTypes/getLeastSupertype.h>
#include <Functions/FunctionFactory.h>
#include <Functions/FunctionHelpers.h>

#include <Eigen/Core>

namespace DB
{
namespace ErrorCodes
{
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
    extern const int LOGICAL_ERROR;
    extern const int SIZES_OF_ARRAYS_DOESNT_MATCH;
}

template <const int N>
struct LpDistance
{
    static inline String name = "L" + std::to_string(N);
    template <typename T>
    static void compute(const Eigen::MatrixX<T> & left, const Eigen::MatrixX<T> & right, PaddedPODArray<T> & array)
    {
        auto norms = (left - right).colwise().template lpNorm<N>();
        array.reserve(norms.size());
        // array.insert() failed to work with Eigen iterators
        for (auto n : norms)
            array.push_back(n);
    }
};

struct LinfDistance : LpDistance<Eigen::Infinity>
{
    static inline String name = "Linf";
};

struct CosineDistance
{
    static inline String name = "Cosine";
    template <typename T>
    static void compute(const Eigen::MatrixX<T> & left, const Eigen::MatrixX<T> & right, PaddedPODArray<T> & array)
    {
        auto prod = left.cwiseProduct(right).colwise().sum();
        auto nx = left.colwise().norm();
        auto ny = right.colwise().norm();
        auto nm = nx.cwiseProduct(ny).cwiseInverse();
        auto dist = 1.0 - prod.cwiseProduct(nm).array();
        array.reserve(dist.size());
        for (auto d : dist)
            array.push_back(d);
    }
};

template <class Kernel>
class FunctionArrayDistance : public IFunction
{
public:
    static inline auto name = "array" + Kernel::name + "Distance";
    String getName() const override { return name; }
    static FunctionPtr create(ContextPtr) { return std::make_shared<FunctionArrayDistance<Kernel>>(); }
    size_t getNumberOfArguments() const override { return 2; }
    bool isSuitableForShortCircuitArgumentsExecution(const DataTypesWithConstInfo & /*arguments*/) const override { return true; }
    bool useDefaultImplementationForConstants() const override { return true; }

    DataTypePtr getReturnTypeImpl(const ColumnsWithTypeAndName & arguments) const override
    {
        DataTypes types;
        for (const auto & argument : arguments)
        {
            const auto * array_type = checkAndGetDataType<DataTypeArray>(argument.type.get());
            if (!array_type)
                throw Exception(ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT, "Argument of function {} must be array.", getName());

            types.push_back(array_type->getNestedType());
        }
        const auto & common_type = getLeastSupertype(types);
        switch (common_type->getTypeId())
        {
            case TypeIndex::UInt8:
            case TypeIndex::UInt16:
            case TypeIndex::UInt32:
            case TypeIndex::Int8:
            case TypeIndex::Int16:
            case TypeIndex::Int32:
            case TypeIndex::Float32:
                return std::make_shared<DataTypeFloat32>();
            case TypeIndex::UInt64:
            case TypeIndex::Int64:
            case TypeIndex::Float64:
                return std::make_shared<DataTypeFloat64>();
            default:
                throw Exception(
                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                    "Arguments of function {} has nested type {}. "
                    "Support: UInt8, UInt16, UInt32, UInt64, Int8, Int16, Int32, Int64, Float32, Float64.",
                    getName(), common_type->getName());
        }
    }

    ColumnPtr
    executeImpl(const ColumnsWithTypeAndName & arguments, const DataTypePtr & result_type, size_t /*input_rows_count*/) const override
    {
        DataTypePtr type_x = typeid_cast<const DataTypeArray *>(arguments[0].type.get())->getNestedType();
        DataTypePtr type_y = typeid_cast<const DataTypeArray *>(arguments[1].type.get())->getNestedType();

        ColumnPtr col_x = arguments[0].column->convertToFullColumnIfConst();
        ColumnPtr col_y = arguments[1].column->convertToFullColumnIfConst();

        const auto * arr_x = assert_cast<const ColumnArray *>(col_x.get());
        const auto * arr_y = assert_cast<const ColumnArray *>(col_y.get());

        auto result = result_type->createColumn();
        switch (result_type->getTypeId())
        {
            case TypeIndex::Float32:
                executeWithType<Float32>(*arr_x, *arr_y, type_x, type_y, result);
                break;
            case TypeIndex::Float64:
                executeWithType<Float64>(*arr_x, *arr_y, type_x, type_y, result);
                break;
            default:
                throw Exception(ErrorCodes::LOGICAL_ERROR, "Unexpected result type.");
        }
        return result;
    }

private:
    template <typename MatrixType>
    void executeWithType(
        const ColumnArray & array_x,
        const ColumnArray & array_y,
        const DataTypePtr & type_x,
        const DataTypePtr & type_y,
        MutableColumnPtr & column) const
    {
        Eigen::MatrixX<MatrixType> mx, my;
        columnToMatrix(array_x, type_x, mx);
        columnToMatrix(array_y, type_y, my);

        if (mx.rows() && my.rows() && mx.rows() != my.rows())
        {
            throw Exception(
                ErrorCodes::SIZES_OF_ARRAYS_DOESNT_MATCH,
                "Arguments of function {} have different array sizes: {} and {}",
                getName(), mx.rows(), my.rows());
        }
        auto & data = assert_cast<ColumnVector<MatrixType> &>(*column).getData();
        Kernel::compute(mx, my, data);
    }

    template <typename MatrixType>
    void columnToMatrix(const ColumnArray & array, const DataTypePtr & nested_type, Eigen::MatrixX<MatrixType> & mat) const
    {
        const auto & offsets = array.getOffsets();
        size_t cols = offsets.size();
        size_t rows = cols > 0 ? offsets.front() : 0;

        ColumnArray::Offset prev = 0;
        for (ColumnArray::Offset off : offsets)
        {
            if (off - prev != rows)
                throw Exception(
                    ErrorCodes::SIZES_OF_ARRAYS_DOESNT_MATCH,
                    "Arrays in a column passed to function {} have different sizes: {} and {}",
                    getName(), rows, off - prev);
            prev = off;
        }

        switch (nested_type->getTypeId())
        {
            case TypeIndex::UInt8:
                fillMatrix<MatrixType, UInt8>(mat, array, rows, cols);
                break;
            case TypeIndex::UInt16:
                fillMatrix<MatrixType, UInt16>(mat, array, rows, cols);
                break;
            case TypeIndex::UInt32:
                fillMatrix<MatrixType, UInt32>(mat, array, rows, cols);
                break;
            case TypeIndex::UInt64:
                fillMatrix<MatrixType, UInt64>(mat, array, rows, cols);
                break;
            case TypeIndex::Int8:
                fillMatrix<MatrixType, Int8>(mat, array, rows, cols);
                break;
            case TypeIndex::Int16:
                fillMatrix<MatrixType, Int16>(mat, array, rows, cols);
                break;
            case TypeIndex::Int32:
                fillMatrix<MatrixType, Int32>(mat, array, rows, cols);
                break;
            case TypeIndex::Int64:
                fillMatrix<MatrixType, Int64>(mat, array, rows, cols);
                break;
            case TypeIndex::Float32:
                fillMatrix<MatrixType, Float32>(mat, array, rows, cols);
                break;
            case TypeIndex::Float64:
                fillMatrix<MatrixType, Float64>(mat, array, rows, cols);
                break;
            default:
                throw Exception(
                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                    "Arguments of function {} has nested type {}. "
                    "Support: UInt8, UInt16, UInt32, UInt64, Int8, Int16, Int32, Int64, Float32, Float64.",
                    getName(), nested_type->getName());
        }
    }

    // optimize for float/ double
    template <typename MatrixType, typename DataType>
    requires std::is_same_v<MatrixType, DataType>
    void fillMatrix(Eigen::MatrixX<MatrixType> & mat, const ColumnArray & array, size_t rows, size_t cols) const
    {
        const auto & data = typeid_cast<const ColumnVector<DataType> &>(array.getData()).getData();
        mat = Eigen::Map<const Eigen::MatrixX<MatrixType>>(data.data(), rows, cols);
    }

    template <typename MatrixType, typename DataType>
    void fillMatrix(Eigen::MatrixX<MatrixType> & mat, const ColumnArray & array, size_t rows, size_t cols) const
    {
        const auto & data = typeid_cast<const ColumnVector<DataType> &>(array.getData()).getData();
        mat.resize(rows, cols);
        for (size_t col = 0; col < cols; ++col)
        {
            for (size_t row = 0; row < rows; ++row)
            {
                size_t off = col * rows;
                mat(row, col) = static_cast<MatrixType>(data[off + row]);
            }
        }
    }
};

void registerFunctionArrayDistance(FunctionFactory & factory)
{
    factory.registerFunction<FunctionArrayDistance<LpDistance<1>>>();
    factory.registerFunction<FunctionArrayDistance<LpDistance<2>>>();
    factory.registerFunction<FunctionArrayDistance<LinfDistance>>();
    factory.registerFunction<FunctionArrayDistance<CosineDistance>>();
}

}


#include <math.h>
#include <stdio.h>
#include <cmath>
#include "MBTB_DATA.hpp"
#include <boost/math/quaternion.hpp>
#include "op3x3.h"
#define EQUIPAGE 4
#define PLATINE 3

//#define VERBOSE_DEBUG_PLUGIN

/*we assume forces are computed before the mometums, thanks siconos.*/

//static double ManetteForce[3];
extern "C" void manetteForcesFromSpring(double t, double* q, double *v,double *f, unsigned int size_z,double *z){
 /*spring K is N.mm/rad*/
  double K=0.382*10;//*10;
  double A0=1.99;
  double angle0=2*acos(z[3]);
  double angle =2*acos(q[3]);
  // point of contact of the spring.
  double P0contactSpringx=0;
  double P0contactSpringy=-5.65;
  double P0contactSpringz=-0.9;
  //double d=5.65;
  double GP=3.19;
  if (q[6]<0)
    angle=-angle;
  if (angle-angle0<-1e7)
    printf("ERROR in externalManetteForces, angle negatif=%lf\n",angle-angle0);
  double C_p=K*(A0-(angle-angle0));
#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalManetteForces angle = %e, angle0=%e \n",angle, angle0);
#endif
  double F_p=C_p/fabs(P0contactSpringy);
  //Mf_p e_z= F /\

  double GP0x=0-z[0];
  double GP0y=-5.65-z[1];
  double GP0z=-0.9-z[2];
  double Dir0x=1.0;
  double Dir0y=0;//5.65;
  double Dir0z=0;
  ::boost::math::quaternion<double>    quattrf(q[3],q[4],q[5],q[6]);
  ::boost::math::quaternion<double>    quatDir0(0,Dir0x,Dir0y,Dir0z);
  ::boost::math::quaternion<double>    cquattrf(q[3],-q[4],-q[5],-q[6]);
  ::boost::math::quaternion<double>    quatbuf;
  quatbuf=quattrf*quatDir0*cquattrf;
  f[0]=F_p*quatbuf.R_component_2();
  f[1]=F_p*quatbuf.R_component_3();
  f[2]=F_p*quatbuf.R_component_4();
  //ManetteForce[0]=f[0];
  //ManetteForce[1]=f[1];
  //ManetteForce[2]=f[2];
}

extern "C" void externalManetteForces(double t, double* q,double *v,double *f, unsigned int size_z,double *z){
  f[0]=0;
  f[1]=0;
  f[2]=0;
  manetteForcesFromSpring( t,  q,  v, f, size_z, z);
  //printf("%e %e %e\n", f[0], f[1], f[2]);
#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalManetteForces : The spring forces intensity is %e in the direction %e %e %e \n",F_p,quatbuf.R_component_2(),quatbuf.R_component_3(),quatbuf.R_component_4());
#endif
  f[0]=-f[0];
  f[1]=-f[1];
  f[2]=-f[2];
}

/*////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////*/
/*////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////*/

extern "C" void externalManetteMomentum(double t, double* q,double *v,double *m, unsigned int size_z,double *z){
  m[0]=0;
  m[1]=0;
  m[2]=0;
  double GP_manette[3];
  double GP0x=0-z[0];
  double GP0y=-5.65-z[1];
  double GP0z=-0.9-z[2];
#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalManetteMomentum : GP0= %e, %e, %e\n",GP0x,GP0y,GP0z);
#endif
  ::boost::math::quaternion<double>    quattrf(q[3],q[4],q[5],q[6]);
  ::boost::math::quaternion<double>    quatGP0(0,GP0x,GP0y,GP0z);
  ::boost::math::quaternion<double>    cquattrf(q[3],-q[4],-q[5],-q[6]);
  ::boost::math::quaternion<double>    quatbuf;
  quatbuf=quattrf*quatGP0*cquattrf;
  GP_manette[0]=quatbuf.R_component_2();
  GP_manette[1]=quatbuf.R_component_3();
  GP_manette[2]=quatbuf.R_component_4();
  double f[3];
  manetteForcesFromSpring( t,  q,  v, f, size_z, z);

  cross3(GP_manette,f,m);
  //printf("externalManetteMometum: m[0] = %lf, m[1] = %lf, m[2]=%lf.\n",m[0],m[1],m[2]);
  m[0]=-m[0];
  m[1]=-m[1];
  m[2]=-m[2];

}
/*////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////*/

extern "C" void MZPlatineMomemtumDueSpringFct(double *qEquipage, double *qPlatine, double *MZPlatineMomemtumDueSpring, double *  Theta_equipage)
{
  /*PlatineEquipage is K is N.mm/rad*/
  double K2=244.46;

  // double norm2_q =0.0;
  // for (int i =0; i <7; i++){norm2_q +=(qEquipage[i]-sDS[EQUIPAGE]->q()->getValue(i))*(qEquipage[i]-sDS[EQUIPAGE]->q()->getValue(i));
  // }
  // if (sqrt(norm2_q) < 1e-12)
  // {
  //   std::cout << "qEquipage belongs to DS EQUIPAGE. accuracy "<< norm2_q <<std::endl;
  // }
  // norm2_q =0.0;
  // for (int i =0; i <7; i++){norm2_q +=(qPlatine[i]-sDS[PLATINE]->q()->getValue(i))*(qPlatine[i]-sDS[PLATINE]->q()->getValue(i));
  // }
  // if (sqrt(norm2_q) < 1e-12)
  // {
  //   std::cout << "qPlatine belongs to DS PLATINE. accuracy"<< sqrt(norm2_q) <<std::endl;
  // }

  *Theta_equipage=2*acos(qEquipage[3]);
  if (qEquipage[6]<0)
    *Theta_equipage=- *Theta_equipage;
/*   printf("Theta_equipage:%e\n",Theta_equipage);*/
  double Theta_platine=2*acos(qPlatine[3]);
  if (qPlatine[6]<0)
    Theta_platine=-Theta_platine;
/*   printf("Theta_platine:%e\n",Theta_platine);*/
  /*diffAngle is zero in the closed position.*/
  double diffAngle = *Theta_equipage-Theta_platine-(-54.54-22.0)*M_PI/180.0;/*-(-54.54-22.0)(-69+2).....FvsD -78.5+2 */
 // printf("diffangle1:%e\n",diffAngle);
 // printf("Equipage:%e\n",*Theta_equipage);
  //printf("Plate:%e\n",Theta_platine);
  /*The Z momentum:*/
  /*if (Theta_equipage < -69.0*M_PI/180.0)
    {
    MZPlatineMomemtumDueSpring=-K2*(0.5236);
    }
    else
    {
    MZPlatineMomemtumDueSpring-K2*(diffAngle + 0.5236);
    }*/
  /*The force intensity*/
  *MZPlatineMomemtumDueSpring =K2*(diffAngle + 0.49);/*0.6547, 0.9599*/

}

extern "C" void FPlatineA2(double t, double* q,double *v,double *f, unsigned int size_z,double *z){

  ::boost::math::quaternion<double>    quattrf(q[3],q[4],q[5],q[6]);
  ::boost::math::quaternion<double>    cquattrf(q[3],-q[4],-q[5],-q[6]);
  ::boost::math::quaternion<double>    quatbuf;

  f[0]=0;
  f[1]=0;
  f[2]=0;
  double g=0.0;
  /*spring A2*/
  double K=1.16;
  double L0=18.44;
  /*coordonite of the application on the batie*/
  double PbX=11.8;
  double PbY=9;
  double PbZ=1.95;
  /*P0 coordinate of the application point of the spring on platine.*/
  ::boost::math::quaternion<double>   G0P0(0,
					   0.19-3.319,
					   9-4.365,
					   -7.6+4.7);

  quatbuf=quattrf*G0P0*cquattrf;
  double Px=quatbuf.R_component_2()+q[0];
  double Py=quatbuf.R_component_3()+q[1];
  double Pz=quatbuf.R_component_4()+q[2];
  double lengthSpring = sqrt((PbX-Px)*(PbX-Px)+
			     (PbY-Py)*(PbY-Py)+
			     (PbZ-Pz)*(PbZ-Pz));

/*  printf("plugin, externalPlatineForces, spring A2 lenght:%e\n",lengthSpring);*/
#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("plugin, externalPlatineForces, spring A2 lenght:%e\n",lengthSpring);
#endif
  double nFp=K*(L0-lengthSpring);
  /*direction*/
  ::boost::math::quaternion<double>    quatDir0(0,-1,0,0);
  quatbuf=quattrf*quatDir0*cquattrf;
  f[0]=nFp*quatbuf.R_component_2();
  f[1]=nFp*quatbuf.R_component_3();
  f[2]=nFp*quatbuf.R_component_4();

}

extern "C" void externalPlatineForces(double t, double* q,double *v,double *f, unsigned int size_z,double *z){

  FPlatineA2( t, q, v, f, size_z, z);

#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("plugin, externalPlatineForces force due to A2 %e, %e, %e\n",f[0],f[1],f[2]);
#endif

  double MZPlatineMomemtumDueSpring ;
  double Theta_equipage;
  MZPlatineMomemtumDueSpringFct( &(*(sDS[EQUIPAGE]->q()))(0), q, &MZPlatineMomemtumDueSpring, &Theta_equipage);

  double F=MZPlatineMomemtumDueSpring/5.48; //updated 26 May 2015 (old value 5.0)// new value 5.48//
/*  printf("externalPlatineForces, F=%e, diffAngle contact spring =%lf\n",F,diffAngle);*/
#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalPlatineForces, F=%e, diffAngle contact spring =%lf\n",F,diffAngle);
#endif
  if (MZPlatineMomemtumDueSpring<0)
    printf("externalPlatineForces, M negatif:%lf\n",MZPlatineMomemtumDueSpring);

  ::boost::math::quaternion<double>    quattrf(q[3],q[4],q[5],q[6]);
  ::boost::math::quaternion<double>    cquattrf(q[3],-q[4],-q[5],-q[6]);
  ::boost::math::quaternion<double>    quatbuf;

  ::boost::math::quaternion<double>    quatDir0_2(0,-1.1,0.9,0);

  quatbuf=quattrf*quatDir0_2*cquattrf;
  double norm2FP=sqrt(quatbuf.R_component_2()*quatbuf.R_component_2()+quatbuf.R_component_3()*quatbuf.R_component_3()+quatbuf.R_component_4()*quatbuf.R_component_4());
  double FPlatineSpringP[3];
  FPlatineSpringP[0]=quatbuf.R_component_2()* F/norm2FP;
  FPlatineSpringP[1]=quatbuf.R_component_3()* F/norm2FP;
  FPlatineSpringP[2]=quatbuf.R_component_4()* F/norm2FP;

  f[0]+=FPlatineSpringP[0];
  f[1]+=FPlatineSpringP[1];
  f[2]+=FPlatineSpringP[2];
/*  printf("externalPlatineForces : The springP force is :%e,%e,%e\n",FPlatineSpringP[0],FPlatineSpringP[1],FPlatineSpringP[2]);

  printf("externalPlatineForces : The ext forces is :%e %e %e\n",f[0],f[1],f[2]);*/

#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalPlatineForces : The springP force is :%e,%e,%e\n",FPlatineSpringP[0],FPlatineSpringP[1],FPlatineSpringP[2]);

  printf("externalPlatineForces : The ext forces is :%e %e %e\n",f[0],f[1],f[2]);
#endif
  f[0]=-f[0];
  f[1]=-f[1];
  f[2]=-f[2];

}

/*/////////////////////////////////////////////////////////////////////////////////////////////////////////*/

extern "C" void externalPlatineMomentum(double t, double* q,double *v,double *m, unsigned int size_z,double *z){
  m[0]=0;
  m[1]=0;
  m[2]=0;

  ::boost::math::quaternion<double>    quattrf(q[3],q[4],q[5],q[6]);
  ::boost::math::quaternion<double>    cquattrf(q[3],-q[4],-q[5],-q[6]);
  ::boost::math::quaternion<double>    quatbuf;

  /*location of the force from A2.*/
  double G0[3];
  G0[0]=3.319088;
  G0[1]=4.365461;
  G0[2]=-4.706395;
  double P0A2[3];
  P0A2[0]=0.19;
  P0A2[1]=9;
  P0A2[2]=-7.6;
  double GP0A2[3] ;
  GP0A2[0]=P0A2[0]-G0[0];
  GP0A2[1]=P0A2[1]-G0[1];
  GP0A2[2]=P0A2[2]-G0[2];
  ::boost::math::quaternion<double>    quatGP0A2(0,GP0A2[0],GP0A2[1],GP0A2[2]);
  quatbuf=quattrf*quatGP0A2*cquattrf;
  double GPA2[3];
  double MA2[3];
  GPA2[0]=quatbuf.R_component_2();
  GPA2[1]=quatbuf.R_component_3();
  GPA2[2]=quatbuf.R_component_4();
  double f[3];
  FPlatineA2( t, q, v, f, size_z, z);

  cross3(GPA2,f,MA2);
/*  printf("externalPlatineMomentum : momemtum due to A2:%e,%e,%e\n",MA2[0],MA2[1],MA2[2]);*/

#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalPlatineMomentum : momemtum due to A2:%e,%e,%e\n",MA2[0],MA2[1],MA2[2]);
#endif

  m[0]=MA2[0];
  m[1]=MA2[1];
  double MZPlatineMomemtumDueSpring ;
  double Theta_equipage;
  double lva1r=0.2513;
  MZPlatineMomemtumDueSpringFct(&(*(sDS[EQUIPAGE]->q()))(0), q, &MZPlatineMomemtumDueSpring, &Theta_equipage);

  m[2]=MA2[2]+(lva1r*MZPlatineMomemtumDueSpring);
/*  printf("externalPlatineMomentum :%e,%e,%e\n",m[0],m[1],m[2]);*/
#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalPlatineMomentum :%e,%e,%e\n",m[0],m[1],m[2]);
#endif
  m[0]=-m[0];
  m[1]=-m[1];
  m[2]=-m[2];
}

/*////////////////////////////////////////////////////////////////////////////////////////////////////////////*/
double FEquipageSpringP[3];
extern "C" void externalEquipageForces(double t, double* q,double *v,double *f, unsigned int size_z,double *z){
   f[0]=0;
   f[1]=0;
   f[2]=0;

   ::boost::math::quaternion<double>    quattrf1(q[3],q[4],q[5],q[6]);
   ::boost::math::quaternion<double>    cquattrf1(q[3],-q[4],-q[5],-q[6]);
   ::boost::math::quaternion<double>    quatbuf1;

  double M ;
  double Theta_equipage;
  MZPlatineMomemtumDueSpringFct(q,  &(*(sDS[PLATINE]->q()))(0)   ,  &M, &Theta_equipage);

  double d_platine=5.8;
  double d_equipage=6.6;
  double d_gplatine = 5.48;//5(old plugin value validated);/*5.48 new value*/
  double d_gequipage = 6.06;//4.84(old plugin value validated);/*7.99 new value*/
  //double Theta0_g_equi= M_PI+1.35; /* 1.22(old plugin value validated)*//*1.35 new value*/
  //double Theta_g_equip = Theta0_g_equi+Theta_equipage;
  //  double Theta0_g_platine = angleOrthoPG_X;
  //f[0]+=(M/d_gequipage)*cos(Theta_g_equip);
  //f[1]+=(M/d_gequipage)*sin(Theta_g_equip);
  /*printf("externalEquipageForces : The equivalent, spring A3 force is fg= %lf, theta_g_equip = %lf.\n",(M/d_gequipage),Theta_g_equip);*/
/*printf("externalEquipageForces : The equivalent, spring A3 force is fg= %lf, f_0 = %lf,f_1 = %lf,theta_g_equip = %lf.\n",(M/d_gequipage), f[0],f[1],Theta_g_equip);*/
#ifdef  VERBOSE_DEBUG_PLUGIN
  printf("externalEquipageForces : The equivalent, spring A3 force is fg= %lf, theta = %lf.\n",(M/d_gequipage),Theta_g_equip);
#endif

  ::boost::math::quaternion<double>    quatDir0_3(0,0.6,-1.1,-0.01);

  quatbuf1=quattrf1*quatDir0_3*cquattrf1;

  double norm2FE1=sqrt(quatbuf1.R_component_2()*quatbuf1.R_component_2()+quatbuf1.R_component_3()*quatbuf1.R_component_3()+quatbuf1.R_component_4()*quatbuf1.R_component_4());

   FEquipageSpringP[0]=quatbuf1.R_component_2()* (M/d_gequipage)/norm2FE1;
   FEquipageSpringP[1]=quatbuf1.R_component_3()* (M/d_gequipage)/norm2FE1;
   FEquipageSpringP[2]=quatbuf1.R_component_4()* (M/d_gequipage)/norm2FE1;

   f[0]+=FEquipageSpringP[0];
   f[1]+=FEquipageSpringP[1];
   f[2]+=FEquipageSpringP[2];

   f[0]=-f[0];
   f[1]=-f[1];
   f[2]=-f[2];
}

/*////////////////////////////////////////////////////////////////////////////////////////////////////////////*/

extern "C" void externalBarreForces(double t, double* q,double *v,double *f, unsigned int size_z,double *z){
 f[0]=0;
 f[1]=0;
 f[2]=0;
}
extern "C" void externalBarreMomentum(double t, double* q,double *v,double *m, unsigned int size_z,double *z){
  if (t <1.0)
  {
    m[0]=0;
    m[1]=0;
    m[2]=15.0;
  }

  else
  {
    m[0]=0;
    m[1]=0;
    m[2]=0.0;
  }

//-3.0;/*FvD bigger clearance-5;23May2015 */
  //printf("externalBarreMomentums: q[6]=%lf \n",q[6]);
  return; //changed on date 08/05/2015
//   if (q[6]<0.349911) //  if (q[6]<0.349911) ...changed on date 08/05/2015
//     f[2]=100;
//   else
//     f[2]=-10;

// /*artificial momentum*/
//   /*spring white*/
//   double Theta_equipage=2*acos(sDS[EQUIPAGE]->q()->getValue(3));
//   if (sDS[EQUIPAGE]->q()->getValue(6)<0)
//     Theta_equipage=-Theta_equipage;

//   double Theta0_equipage=2*acos(sDS[EQUIPAGE]->q0()->getValue(3));
//   if (sDS[EQUIPAGE]->q0()->getValue(6)<0)
//     Theta0_equipage=-Theta0_equipage;
//   if (Theta_equipage>Theta0_equipage + 4*M_PI/180.0){
//     f[2]-=5;
//    }
//   else{
//     f[2]+=5;
//    }
// //printf("externalBarreMomentums: spring white available :f[2]=%lf. %lf \n",f[2],q[6]);
// #ifdef  VERBOSE_DEBUG_PLUGIN
//   printf("externalBarreMomentums: spring white available :f[2]=%lf. %lf \n",f[2],q[6]);
// #endif
}

extern "C" void externalMomentumx(double t, double* q,double *v,double *m, unsigned int size_z,double *z){

double angle =2*acos(q[3]);
//printf("angle:%e\n",angle);
 if (angle > 0.459)
  {
  	m[0]=0;
  	m[1]=0;
  	m[2]=-0;
  }
 else
  {
    m[0]=0.0;
    m[1]=0;
    m[2]=0;
  }

 m[0]=-m[0];
 m[1]=-m[1];
 m[2]=-m[2];
 return;
}

extern "C" void prescribedvelocityB1(double time, unsigned int sizeofprescribedvelocity, double *pv)
{
  /* the plugin implements v(t) = C + A cos(omega *t) */
   if (time <0.7)
    {
      double C = 0.0;
      pv[0] =  C;
    }
   else
    {
      double C = 0.2;
      pv[0] =  C;
    }
    //printf("prescribed velocity = %e\n", pv[0]);

}