// Copyright (c) 2020, The Evolution Network
// Copyright (c) 2018-2019, The Arqma Network
// Copyright (c) 2017-2018, The Monero Project
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other
//    materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors may be
//    used to endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
// THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Adapted from Java code by Sarang Noether

#include <stdlib.h>
#include <openssl/ssl.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/lock_guard.hpp>
#include "misc_log_ex.h"
#include "common/perf_timer.h"
extern "C"
{
#include "crypto/crypto-ops.h"
}
#include "rctOps.h"
#include "multiexp.h"
#include "bulletproofs.h"

#undef EVOLUTION_DEFAULT_LOG_CATEGORY
#define EVOLUTION_DEFAULT_LOG_CATEGORY "bulletproofs_legacy"

//#define DEBUG_BP

#define PERF_TIMER_START_BP(x) PERF_TIMER_START_UNIT(x, 1000000)

namespace rct
{

static rct::key vector_exponent(const rct::keyV &a, const rct::keyV &b);
static rct::keyV vector_powers(const rct::key &x, size_t n);
static rct::keyV vector_dup(const rct::key &x, size_t n);
static rct::key inner_product(const rct::keyV &a, const rct::keyV &b);

static constexpr size_t maxN = 64;
static constexpr size_t maxM = 16;
static rct::key Hi[maxN*maxM], Gi[maxN*maxM];
static ge_p3 Hi_p3[maxN*maxM], Gi_p3[maxN*maxM];
static ge_dsmp Gprecomp[maxN*maxM], Hprecomp[maxN*maxM];
static std::shared_ptr<straus_cached_data> HiGi_cache;
static const rct::key TWO = { {0x02, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00  } };
static const rct::keyV oneN = vector_dup(rct::identity(), maxN);
static const rct::keyV twoN = vector_powers(TWO, maxN);
static const rct::key ip12 = inner_product(oneN, twoN);
static boost::mutex init_mutex;

static inline rct::key multiexp(const std::vector<MultiexpData> &data, bool HiGi)
{
  static const size_t STEP = getenv("STRAUS_STEP") ? atoi(getenv("STRAUS_STEP")) : 0;
  if (HiGi || data.size() < 1000)
    return straus(data, HiGi ? HiGi_cache: NULL, STEP);
  else
    return bos_coster_heap_conv_robust(data);
}

//addKeys3acc_p3
//aAbB += a*A + b*B where a, b are scalars, A, B are curve points
//A and B must be input after applying "precomp"
static void addKeys3acc_p3(ge_p3 *aAbB, const key &a, const ge_dsmp A, const key &b, const ge_dsmp B)
{
    ge_p3 rv;
    ge_p1p1 p1;
    ge_p2 p2;
    ge_double_scalarmult_precomp_vartime2_p3(&rv, a.bytes, A, b.bytes, B);
    ge_cached cached;
    ge_p3_to_cached(&cached, aAbB);
    ge_add(&p1, &rv, &cached);
    ge_p1p1_to_p3(aAbB, &p1);
}

static void addKeys_acc_p3(ge_p3 *acc_p3, const rct::key &a, const rct::key &point)
{
    ge_p3 p3;
    CHECK_AND_ASSERT_THROW_MES(ge_frombytes_vartime(&p3, point.bytes) == 0, "ge_frombytes_vartime failed");
    ge_scalarmult_p3(&p3, a.bytes, &p3);
    ge_cached cached;
    ge_p3_to_cached(&cached, acc_p3);
    ge_p1p1 p1;
    ge_add(&p1, &p3, &cached);
    ge_p1p1_to_p3(acc_p3, &p1);
}

static rct::key get_exponent(const rct::key &base, size_t idx)
{
  static const std::string salt("bulletproof");
  std::string hashed = std::string((const char*)base.bytes, sizeof(base)) + salt + tools::get_varint_data(idx);
  return rct::hashToPoint(rct::hash2rct(crypto::cn_fast_hash(hashed.data(), hashed.size())));
}

static void init_exponents()
{
  boost::lock_guard<boost::mutex> lock(init_mutex);

  static bool init_done = false;
  if (init_done)
    return;
  std::vector<MultiexpData> data;
  for (size_t i = 0; i < maxN*maxM; ++i)
  {
    Hi[i] = get_exponent(rct::H, i * 2);
    rct::precomp(Hprecomp[i], Hi[i]);
    CHECK_AND_ASSERT_THROW_MES(ge_frombytes_vartime(&Hi_p3[i], Hi[i].bytes) == 0, "ge_frombytes_vartime failed");
    Gi[i] = get_exponent(rct::H, i * 2 + 1);
    rct::precomp(Gprecomp[i], Gi[i]);
    CHECK_AND_ASSERT_THROW_MES(ge_frombytes_vartime(&Gi_p3[i], Gi[i].bytes) == 0, "ge_frombytes_vartime failed");

    data.push_back({rct::zero(), Gi[i]});
    data.push_back({rct::zero(), Hi[i]});
  }
  HiGi_cache = straus_init_cache(data);
  size_t cache_size = (sizeof(Hi)+sizeof(Hprecomp)+sizeof(Hi_p3))*2 + straus_get_cache_size(HiGi_cache);
  MINFO("cache size: " << cache_size/1024 << " kB");
  init_done = true;
}

/* Given two scalar arrays, construct a vector commitment */
static rct::key vector_exponent(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  CHECK_AND_ASSERT_THROW_MES(a.size() <= maxN*maxM, "Incompatible sizes of a and maxN");

  std::vector<MultiexpData> multiexp_data;
  multiexp_data.reserve(a.size()*2);
  for (size_t i = 0; i < a.size(); ++i)
  {
    multiexp_data.emplace_back(a[i], Gi_p3[i]);
    multiexp_data.emplace_back(b[i], Hi_p3[i]);
  }
  return multiexp(multiexp_data, true);
}

/* Compute a custom vector-scalar commitment */
static rct::key vector_exponent_custom(const rct::keyV &A, const rct::keyV &B, const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(A.size() == B.size(), "Incompatible sizes of A and B");
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  CHECK_AND_ASSERT_THROW_MES(a.size() == A.size(), "Incompatible sizes of a and A");
  CHECK_AND_ASSERT_THROW_MES(a.size() <= maxN*maxM, "Incompatible sizes of a and maxN");

  std::vector<MultiexpData> multiexp_data;
  multiexp_data.reserve(a.size()*2);
  for (size_t i = 0; i < a.size(); ++i)
  {
    multiexp_data.resize(multiexp_data.size() + 1);
    multiexp_data.back().scalar = a[i];
    CHECK_AND_ASSERT_THROW_MES(ge_frombytes_vartime(&multiexp_data.back().point, A[i].bytes) == 0, "ge_frombytes_vartime failed");
    multiexp_data.resize(multiexp_data.size() + 1);
    multiexp_data.back().scalar = b[i];
    CHECK_AND_ASSERT_THROW_MES(ge_frombytes_vartime(&multiexp_data.back().point, B[i].bytes) == 0, "ge_frombytes_vartime failed");
  }
  return multiexp(multiexp_data, false);
}

/* Given a scalar, construct a vector of powers */
static rct::keyV vector_powers(const rct::key &x, size_t n)
{
  rct::keyV res(n);
  if (n == 0)
    return res;
  res[0] = rct::identity();
  if (n == 1)
    return res;
  res[1] = x;
  for (size_t i = 2; i < n; ++i)
  {
    sc_mul(res[i].bytes, res[i-1].bytes, x.bytes);
  }
  return res;
}

/* Given a scalar, return the sum of its powers from 0 to n-1 */
static rct::key vector_power_sum(const rct::key &x, size_t n)
{
  if (n == 0)
    return rct::zero();
  rct::key res = rct::identity();
  if (n == 1)
    return res;
  rct::key prev = x;
  for (size_t i = 1; i < n; ++i)
  {
    if (i > 1)
      sc_mul(prev.bytes, prev.bytes, x.bytes);
    sc_add(res.bytes, res.bytes, prev.bytes);
  }
  return res;
}

/* Given two scalar arrays, construct the inner product */
static rct::key inner_product(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::key res = rct::zero();
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_muladd(res.bytes, a[i].bytes, b[i].bytes, res.bytes);
  }
  return res;
}

/* Given two scalar arrays, construct the Hadamard product */
static rct::keyV hadamard(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_mul(res[i].bytes, a[i].bytes, b[i].bytes);
  }
  return res;
}

/* Given two curvepoint arrays, construct the Hadamard product */
static rct::keyV hadamard2(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::addKeys(res[i], a[i], b[i]);
  }
  return res;
}

/* Add two vectors */
static rct::keyV vector_add(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_add(res[i].bytes, a[i].bytes, b[i].bytes);
  }
  return res;
}

/* Subtract two vectors */
static rct::keyV vector_subtract(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_sub(res[i].bytes, a[i].bytes, b[i].bytes);
  }
  return res;
}

/* Multiply a scalar and a vector */
static rct::keyV vector_scalar(const rct::keyV &a, const rct::key &x)
{
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_mul(res[i].bytes, a[i].bytes, x.bytes);
  }
  return res;
}

/* Create a vector from copies of a single value */
static rct::keyV vector_dup(const rct::key &x, size_t N)
{
  return rct::keyV(N, x);
}

/* Exponentiate a curve vector by a scalar */
static rct::keyV vector_scalar2(const rct::keyV &a, const rct::key &x)
{
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::scalarmultKey(res[i], a[i], x);
  }
  return res;
}

/* Get the sum of a vector's elements */
static rct::key vector_sum(const rct::keyV &a)
{
  rct::key res = rct::zero();
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_add(res.bytes, res.bytes, a[i].bytes);
  }
  return res;
}

static rct::key switch_endianness(rct::key k)
{
  std::reverse(k.bytes, k.bytes + sizeof(k));
  return k;
}

/* Compute the inverse of a scalar, the stupid way */
static rct::key invert(const rct::key &x)
{
  rct::key inv;

  BN_CTX *ctx = BN_CTX_new();
  BIGNUM *X = BN_new();
  BIGNUM *L = BN_new();
  BIGNUM *I = BN_new();

  BN_bin2bn(switch_endianness(x).bytes, sizeof(rct::key), X);
  BN_bin2bn(switch_endianness(rct::curveOrder()).bytes, sizeof(rct::key), L);

  CHECK_AND_ASSERT_THROW_MES(BN_mod_inverse(I, X, L, ctx), "Failed to invert");

  const int len = BN_num_bytes(I);
  CHECK_AND_ASSERT_THROW_MES((size_t)len <= sizeof(rct::key), "Invalid number length");
  inv = rct::zero();
  BN_bn2bin(I, inv.bytes);
  std::reverse(inv.bytes, inv.bytes + len);

  BN_free(I);
  BN_free(L);
  BN_free(X);
  BN_CTX_free(ctx);

#ifdef DEBUG_BP
  rct::key tmp;
  sc_mul(tmp.bytes, inv.bytes, x.bytes);
  CHECK_AND_ASSERT_THROW_MES(tmp == rct::identity(), "invert failed");
#endif
  return inv;
}

/* Compute the slice of a vector */
static rct::keyV slice(const rct::keyV &a, size_t start, size_t stop)
{
  CHECK_AND_ASSERT_THROW_MES(start < a.size(), "Invalid start index");
  CHECK_AND_ASSERT_THROW_MES(stop <= a.size(), "Invalid stop index");
  CHECK_AND_ASSERT_THROW_MES(start < stop, "Invalid start/stop indices");
  rct::keyV res(stop - start);
  for (size_t i = start; i < stop; ++i)
  {
    res[i - start] = a[i];
  }
  return res;
}

static rct::key hash_cache_mash(rct::key &hash_cache, const rct::key &mash0, const rct::key &mash1)
{
  rct::keyV data;
  data.reserve(3);
  data.push_back(hash_cache);
  data.push_back(mash0);
  data.push_back(mash1);
  return hash_cache = rct::hash_to_scalar(data);
}

static rct::key hash_cache_mash(rct::key &hash_cache, const rct::key &mash0, const rct::key &mash1, const rct::key &mash2)
{
  rct::keyV data;
  data.reserve(4);
  data.push_back(hash_cache);
  data.push_back(mash0);
  data.push_back(mash1);
  data.push_back(mash2);
  return hash_cache = rct::hash_to_scalar(data);
}

static rct::key hash_cache_mash(rct::key &hash_cache, const rct::key &mash0, const rct::key &mash1, const rct::key &mash2, const rct::key &mash3)
{
  rct::keyV data;
  data.reserve(5);
  data.push_back(hash_cache);
  data.push_back(mash0);
  data.push_back(mash1);
  data.push_back(mash2);
  data.push_back(mash3);
  return hash_cache = rct::hash_to_scalar(data);
}

/* Given a value v (0..2^N-1) and a mask gamma, construct a range proof */
Bulletproof bulletproof_PROVE_old(const rct::key &sv, const rct::key &gamma)
{
  init_exponents();

  PERF_TIMER_UNIT(PROVE, 1000000);

  constexpr size_t logN = 6; // log2(64)
  constexpr size_t N = 1<<logN;

  rct::key V;
  rct::keyV aL(N), aR(N);

  PERF_TIMER_START_BP(PROVE_v);
  rct::addKeys2(V, gamma, sv, rct::H);
  PERF_TIMER_STOP(PROVE_v);

  PERF_TIMER_START_BP(PROVE_aLaR);
  for (size_t i = N; i-- > 0; )
  {
    if (sv[i/8] & (((uint64_t)1)<<(i%8)))
    {
      aL[i] = rct::identity();
    }
    else
    {
      aL[i] = rct::zero();
    }
    sc_sub(aR[i].bytes, aL[i].bytes, rct::identity().bytes);
  }
  PERF_TIMER_STOP(PROVE_aLaR);

  rct::key hash_cache = rct::hash_to_scalar(V);

  // DEBUG: Test to ensure this recovers the value
#ifdef DEBUG_BP
  uint64_t test_aL = 0, test_aR = 0;
  for (size_t i = 0; i < N; ++i)
  {
    if (aL[i] == rct::identity())
      test_aL += ((uint64_t)1)<<i;
    if (aR[i] == rct::zero())
      test_aR += ((uint64_t)1)<<i;
  }
  uint64_t v_test = 0;
  for (int n = 0; n < 8; ++n) v_test |= (((uint64_t)sv[n]) << (8*n));
  CHECK_AND_ASSERT_THROW_MES(test_aL == v_test, "test_aL failed");
  CHECK_AND_ASSERT_THROW_MES(test_aR == v_test, "test_aR failed");
#endif

  PERF_TIMER_START_BP(PROVE_step1);
  // PAPER LINES 38-39
  rct::key alpha = rct::skGen();
  rct::key ve = vector_exponent(aL, aR);
  rct::key A;
  rct::addKeys(A, ve, rct::scalarmultBase(alpha));

  // PAPER LINES 40-42
  rct::keyV sL = rct::skvGen(N), sR = rct::skvGen(N);
  rct::key rho = rct::skGen();
  ve = vector_exponent(sL, sR);
  rct::key S;
  rct::addKeys(S, ve, rct::scalarmultBase(rho));

  // PAPER LINES 43-45
  rct::key y = hash_cache_mash(hash_cache, A, S);
  rct::key z = hash_cache = rct::hash_to_scalar(y);

  // Polynomial construction before PAPER LINE 46
  rct::key t0 = rct::zero();
  rct::key t1 = rct::zero();
  rct::key t2 = rct::zero();

  const auto yN = vector_powers(y, N);

  rct::key ip1y = vector_sum(yN);
  rct::key tmp;
  sc_muladd(t0.bytes, z.bytes, ip1y.bytes, t0.bytes);

  rct::key zsq;
  sc_mul(zsq.bytes, z.bytes, z.bytes);
  sc_muladd(t0.bytes, zsq.bytes, sv.bytes, t0.bytes);

  rct::key k = rct::zero();
  sc_mulsub(k.bytes, zsq.bytes, ip1y.bytes, k.bytes);

  rct::key zcu;
  sc_mul(zcu.bytes, zsq.bytes, z.bytes);
  sc_mulsub(k.bytes, zcu.bytes, ip12.bytes, k.bytes);
  sc_add(t0.bytes, t0.bytes, k.bytes);

  // DEBUG: Test the value of t0 has the correct form
#ifdef DEBUG_BP
  rct::key test_t0 = rct::zero();
  rct::key iph = inner_product(aL, hadamard(aR, yN));
  sc_add(test_t0.bytes, test_t0.bytes, iph.bytes);
  rct::key ips = inner_product(vector_subtract(aL, aR), yN);
  sc_muladd(test_t0.bytes, z.bytes, ips.bytes, test_t0.bytes);
  rct::key ipt = inner_product(twoN, aL);
  sc_muladd(test_t0.bytes, zsq.bytes, ipt.bytes, test_t0.bytes);
  sc_add(test_t0.bytes, test_t0.bytes, k.bytes);
  CHECK_AND_ASSERT_THROW_MES(t0 == test_t0, "t0 check failed");
#endif
  PERF_TIMER_STOP(PROVE_step1);

  PERF_TIMER_START_BP(PROVE_step2);
  const auto HyNsR = hadamard(yN, sR);
  const auto vpIz = vector_dup(z, N);
  const auto vp2zsq = vector_scalar(twoN, zsq);
  const auto aL_vpIz = vector_subtract(aL, vpIz);
  const auto aR_vpIz = vector_add(aR, vpIz);

  rct::key ip1 = inner_product(aL_vpIz, HyNsR);
  sc_add(t1.bytes, t1.bytes, ip1.bytes);

  rct::key ip2 = inner_product(sL, vector_add(hadamard(yN, aR_vpIz), vp2zsq));
  sc_add(t1.bytes, t1.bytes, ip2.bytes);

  rct::key ip3 = inner_product(sL, HyNsR);
  sc_add(t2.bytes, t2.bytes, ip3.bytes);

  // PAPER LINES 47-48
  rct::key tau1 = rct::skGen(), tau2 = rct::skGen();

  rct::key T1 = rct::addKeys(rct::scalarmultKey(rct::H, t1), rct::scalarmultBase(tau1));
  rct::key T2 = rct::addKeys(rct::scalarmultKey(rct::H, t2), rct::scalarmultBase(tau2));

  // PAPER LINES 49-51
  rct::key x = hash_cache_mash(hash_cache, z, T1, T2);

  // PAPER LINES 52-53
  rct::key taux = rct::zero();
  sc_mul(taux.bytes, tau1.bytes, x.bytes);
  rct::key xsq;
  sc_mul(xsq.bytes, x.bytes, x.bytes);
  sc_muladd(taux.bytes, tau2.bytes, xsq.bytes, taux.bytes);
  sc_muladd(taux.bytes, gamma.bytes, zsq.bytes, taux.bytes);
  rct::key mu;
  sc_muladd(mu.bytes, x.bytes, rho.bytes, alpha.bytes);

  // PAPER LINES 54-57
  rct::keyV l = vector_add(aL_vpIz, vector_scalar(sL, x));
  rct::keyV r = vector_add(hadamard(yN, vector_add(aR_vpIz, vector_scalar(sR, x))), vp2zsq);
  PERF_TIMER_STOP(PROVE_step2);

  PERF_TIMER_START_BP(PROVE_step3);
  rct::key t = inner_product(l, r);

  // DEBUG: Test if the l and r vectors match the polynomial forms
#ifdef DEBUG_BP
  rct::key test_t;
  sc_muladd(test_t.bytes, t1.bytes, x.bytes, t0.bytes);
  sc_muladd(test_t.bytes, t2.bytes, xsq.bytes, test_t.bytes);
  CHECK_AND_ASSERT_THROW_MES(test_t == t, "test_t check failed");
#endif

  // PAPER LINES 32-33
  rct::key x_ip = hash_cache_mash(hash_cache, x, taux, mu, t);

  // These are used in the inner product rounds
  size_t nprime = N;
  rct::keyV Gprime(N);
  rct::keyV Hprime(N);
  rct::keyV aprime(N);
  rct::keyV bprime(N);
  const rct::key yinv = invert(y);
  rct::key yinvpow = rct::identity();
  for (size_t i = 0; i < N; ++i)
  {
    Gprime[i] = Gi[i];
    Hprime[i] = scalarmultKey(Hi[i], yinvpow);
    sc_mul(yinvpow.bytes, yinvpow.bytes, yinv.bytes);
    aprime[i] = l[i];
    bprime[i] = r[i];
  }
  rct::keyV L(logN);
  rct::keyV R(logN);
  int round = 0;
  rct::keyV w(logN); // this is the challenge x in the inner product protocol
  PERF_TIMER_STOP(PROVE_step3);

  PERF_TIMER_START_BP(PROVE_step4);
  // PAPER LINE 13
  while (nprime > 1)
  {
    // PAPER LINE 15
    nprime /= 2;

    // PAPER LINES 16-17
    rct::key cL = inner_product(slice(aprime, 0, nprime), slice(bprime, nprime, bprime.size()));
    rct::key cR = inner_product(slice(aprime, nprime, aprime.size()), slice(bprime, 0, nprime));

    // PAPER LINES 18-19
    L[round] = vector_exponent_custom(slice(Gprime, nprime, Gprime.size()), slice(Hprime, 0, nprime), slice(aprime, 0, nprime), slice(bprime, nprime, bprime.size()));
    sc_mul(tmp.bytes, cL.bytes, x_ip.bytes);
    rct::addKeys(L[round], L[round], rct::scalarmultKey(rct::H, tmp));
    R[round] = vector_exponent_custom(slice(Gprime, 0, nprime), slice(Hprime, nprime, Hprime.size()), slice(aprime, nprime, aprime.size()), slice(bprime, 0, nprime));
    sc_mul(tmp.bytes, cR.bytes, x_ip.bytes);
    rct::addKeys(R[round], R[round], rct::scalarmultKey(rct::H, tmp));

    // PAPER LINES 21-22
    w[round] = hash_cache_mash(hash_cache, L[round], R[round]);

    // PAPER LINES 24-25
    const rct::key winv = invert(w[round]);
    Gprime = hadamard2(vector_scalar2(slice(Gprime, 0, nprime), winv), vector_scalar2(slice(Gprime, nprime, Gprime.size()), w[round]));
    Hprime = hadamard2(vector_scalar2(slice(Hprime, 0, nprime), w[round]), vector_scalar2(slice(Hprime, nprime, Hprime.size()), winv));

    // PAPER LINES 28-29
    aprime = vector_add(vector_scalar(slice(aprime, 0, nprime), w[round]), vector_scalar(slice(aprime, nprime, aprime.size()), winv));
    bprime = vector_add(vector_scalar(slice(bprime, 0, nprime), winv), vector_scalar(slice(bprime, nprime, bprime.size()), w[round]));

    ++round;
  }
  PERF_TIMER_STOP(PROVE_step4);

  // PAPER LINE 58 (with inclusions from PAPER LINE 8 and PAPER LINE 20)
  return Bulletproof(V, A, S, T1, T2, taux, mu, L, R, aprime[0], bprime[0], t);
}

Bulletproof bulletproof_PROVE_old(uint64_t v, const rct::key &gamma)
{
  // vG + gammaH
  PERF_TIMER_START_BP(PROVE_v);
  rct::key sv = rct::zero();
  sv.bytes[0] = v & 255;
  sv.bytes[1] = (v >> 8) & 255;
  sv.bytes[2] = (v >> 16) & 255;
  sv.bytes[3] = (v >> 24) & 255;
  sv.bytes[4] = (v >> 32) & 255;
  sv.bytes[5] = (v >> 40) & 255;
  sv.bytes[6] = (v >> 48) & 255;
  sv.bytes[7] = (v >> 56) & 255;
  PERF_TIMER_STOP(PROVE_v);
  return bulletproof_PROVE_old(sv, gamma);
}

/* Given a set of values v (0..2^N-1) and masks gamma, construct a range proof */
Bulletproof bulletproof_PROVE_old(const rct::keyV &sv, const rct::keyV &gamma)
{
  CHECK_AND_ASSERT_THROW_MES(sv.size() == gamma.size(), "Incompatible sizes of sv and gamma");
  CHECK_AND_ASSERT_THROW_MES(!sv.empty(), "sv is empty");

  init_exponents();

  PERF_TIMER_UNIT(PROVE, 1000000);

  constexpr size_t logN = 6; // log2(64)
  constexpr size_t N = 1<<logN;
  size_t M, logM;
  for (logM = 0; (M = 1<<logM) <= maxM && M < sv.size(); ++logM);
  CHECK_AND_ASSERT_THROW_MES(M <= maxM, "sv/gamma are too large");
  const size_t logMN = logM + logN;
  const size_t MN = M * N;

  rct::keyV V(sv.size());
  rct::keyV aL(MN), aR(MN);
  rct::key tmp;

  PERF_TIMER_START_BP(PROVE_v);
  for (size_t i = 0; i < sv.size(); ++i)
    rct::addKeys2(V[i], gamma[i], sv[i], rct::H);
  PERF_TIMER_STOP(PROVE_v);

  PERF_TIMER_START_BP(PROVE_aLaR);
  for (size_t j = 0; j < M; ++j)
  {
    for (size_t i = N; i-- > 0; )
    {
      if (j >= sv.size())
      {
        aL[j*N+i] = rct::zero();
      }
      else if (sv[j][i/8] & (((uint64_t)1)<<(i%8)))
      {
        aL[j*N+i] = rct::identity();
      }
      else
      {
        aL[j*N+i] = rct::zero();
      }
      sc_sub(aR[j*N+i].bytes, aL[j*N+i].bytes, rct::identity().bytes);
    }
  }
  PERF_TIMER_STOP(PROVE_aLaR);

  rct::key hash_cache = rct::hash_to_scalar(V);

  // DEBUG: Test to ensure this recovers the value
#ifdef DEBUG_BP
  for (size_t j = 0; j < M; ++j)
  {
    uint64_t test_aL = 0, test_aR = 0;
    for (size_t i = 0; i < N; ++i)
    {
      if (aL[j*N+i] == rct::identity())
        test_aL += ((uint64_t)1)<<i;
      if (aR[j*N+i] == rct::zero())
        test_aR += ((uint64_t)1)<<i;
    }
    uint64_t v_test = 0;
    if (j < sv.size())
      for (int n = 0; n < 8; ++n) v_test |= (((uint64_t)sv[j][n]) << (8*n));
    CHECK_AND_ASSERT_THROW_MES(test_aL == v_test, "test_aL failed");
    CHECK_AND_ASSERT_THROW_MES(test_aR == v_test, "test_aR failed");
  }
#endif

  PERF_TIMER_START_BP(PROVE_step1);
  // PAPER LINES 38-39
  rct::key alpha = rct::skGen();
  rct::key ve = vector_exponent(aL, aR);
  rct::key A;
  rct::addKeys(A, ve, rct::scalarmultBase(alpha));

  // PAPER LINES 40-42
  rct::keyV sL = rct::skvGen(MN), sR = rct::skvGen(MN);
  rct::key rho = rct::skGen();
  ve = vector_exponent(sL, sR);
  rct::key S;
  rct::addKeys(S, ve, rct::scalarmultBase(rho));

  // PAPER LINES 43-45
  rct::key y = hash_cache_mash(hash_cache, A, S);
  rct::key z = hash_cache = rct::hash_to_scalar(y);

  // Polynomial construction by coefficients
  const auto zMN = vector_dup(z, MN);
  rct::keyV l0 = vector_subtract(aL, zMN);
  const rct::keyV &l1 = sL;

  // This computes the ugly sum/concatenation from PAPER LINE 65
  rct::keyV zero_twos(MN);
  const rct::keyV zpow = vector_powers(z, M+2);
  for (size_t i = 0; i < MN; ++i)
  {
    zero_twos[i] = rct::zero();
    for (size_t j = 1; j <= M; ++j)
    {
      if (i >= (j-1)*N && i < j*N)
      {
        CHECK_AND_ASSERT_THROW_MES(1+j < zpow.size(), "invalid zpow index");
        CHECK_AND_ASSERT_THROW_MES(i-(j-1)*N < twoN.size(), "invalid twoN index");
        sc_muladd(zero_twos[i].bytes, zpow[1+j].bytes, twoN[i-(j-1)*N].bytes, zero_twos[i].bytes);
      }
    }
  }

  rct::keyV r0 = vector_add(aR, zMN);
  const auto yMN = vector_powers(y, MN);
  r0 = hadamard(r0, yMN);
  r0 = vector_add(r0, zero_twos);
  rct::keyV r1 = hadamard(yMN, sR);

  // Polynomial construction before PAPER LINE 46
  rct::key t1_1 = inner_product(l0, r1);
  rct::key t1_2 = inner_product(l1, r0);
  rct::key t1;
  sc_add(t1.bytes, t1_1.bytes, t1_2.bytes);
  rct::key t2 = inner_product(l1, r1);

  PERF_TIMER_STOP(PROVE_step1);

  PERF_TIMER_START_BP(PROVE_step2);
  // PAPER LINES 47-48
  rct::key tau1 = rct::skGen(), tau2 = rct::skGen();

  rct::key T1 = rct::addKeys(rct::scalarmultKey(rct::H, t1), rct::scalarmultBase(tau1));
  rct::key T2 = rct::addKeys(rct::scalarmultKey(rct::H, t2), rct::scalarmultBase(tau2));

  // PAPER LINES 49-51
  rct::key x = hash_cache_mash(hash_cache, z, T1, T2);

  // PAPER LINES 52-53
  rct::key taux;
  sc_mul(taux.bytes, tau1.bytes, x.bytes);
  rct::key xsq;
  sc_mul(xsq.bytes, x.bytes, x.bytes);
  sc_muladd(taux.bytes, tau2.bytes, xsq.bytes, taux.bytes);
  for (size_t j = 1; j <= sv.size(); ++j)
  {
    CHECK_AND_ASSERT_THROW_MES(j+1 < zpow.size(), "invalid zpow index");
    sc_muladd(taux.bytes, zpow[j+1].bytes, gamma[j-1].bytes, taux.bytes);
  }
  rct::key mu;
  sc_muladd(mu.bytes, x.bytes, rho.bytes, alpha.bytes);

  // PAPER LINES 54-57
  rct::keyV l = l0;
  l = vector_add(l, vector_scalar(l1, x));
  rct::keyV r = r0;
  r = vector_add(r, vector_scalar(r1, x));
  PERF_TIMER_STOP(PROVE_step2);

  PERF_TIMER_START_BP(PROVE_step3);
  rct::key t = inner_product(l, r);

  // DEBUG: Test if the l and r vectors match the polynomial forms
#ifdef DEBUG_BP
  rct::key test_t;
  const rct::key t0 = inner_product(l0, r0);
  sc_muladd(test_t.bytes, t1.bytes, x.bytes, t0.bytes);
  sc_muladd(test_t.bytes, t2.bytes, xsq.bytes, test_t.bytes);
  CHECK_AND_ASSERT_THROW_MES(test_t == t, "test_t check failed");
#endif

  // PAPER LINES 32-33
  rct::key x_ip = hash_cache_mash(hash_cache, x, taux, mu, t);

  // These are used in the inner product rounds
  size_t nprime = MN;
  rct::keyV Gprime(MN);
  rct::keyV Hprime(MN);
  rct::keyV aprime(MN);
  rct::keyV bprime(MN);
  const rct::key yinv = invert(y);
  rct::key yinvpow = rct::identity();
  for (size_t i = 0; i < MN; ++i)
  {
    Gprime[i] = Gi[i];
    Hprime[i] = scalarmultKey(Hi[i], yinvpow);
    sc_mul(yinvpow.bytes, yinvpow.bytes, yinv.bytes);
    aprime[i] = l[i];
    bprime[i] = r[i];
  }
  rct::keyV L(logMN);
  rct::keyV R(logMN);
  int round = 0;
  rct::keyV w(logMN); // this is the challenge x in the inner product protocol
  PERF_TIMER_STOP(PROVE_step3);

  PERF_TIMER_START_BP(PROVE_step4);
  // PAPER LINE 13
  while (nprime > 1)
  {
    // PAPER LINE 15
    nprime /= 2;

    // PAPER LINES 16-17
    rct::key cL = inner_product(slice(aprime, 0, nprime), slice(bprime, nprime, bprime.size()));
    rct::key cR = inner_product(slice(aprime, nprime, aprime.size()), slice(bprime, 0, nprime));

    // PAPER LINES 18-19
    L[round] = vector_exponent_custom(slice(Gprime, nprime, Gprime.size()), slice(Hprime, 0, nprime), slice(aprime, 0, nprime), slice(bprime, nprime, bprime.size()));
    sc_mul(tmp.bytes, cL.bytes, x_ip.bytes);
    rct::addKeys(L[round], L[round], rct::scalarmultKey(rct::H, tmp));
    R[round] = vector_exponent_custom(slice(Gprime, 0, nprime), slice(Hprime, nprime, Hprime.size()), slice(aprime, nprime, aprime.size()), slice(bprime, 0, nprime));
    sc_mul(tmp.bytes, cR.bytes, x_ip.bytes);
    rct::addKeys(R[round], R[round], rct::scalarmultKey(rct::H, tmp));

    // PAPER LINES 21-22
    w[round] = hash_cache_mash(hash_cache, L[round], R[round]);

    // PAPER LINES 24-25
    const rct::key winv = invert(w[round]);
    Gprime = hadamard2(vector_scalar2(slice(Gprime, 0, nprime), winv), vector_scalar2(slice(Gprime, nprime, Gprime.size()), w[round]));
    Hprime = hadamard2(vector_scalar2(slice(Hprime, 0, nprime), w[round]), vector_scalar2(slice(Hprime, nprime, Hprime.size()), winv));

    // PAPER LINES 28-29
    aprime = vector_add(vector_scalar(slice(aprime, 0, nprime), w[round]), vector_scalar(slice(aprime, nprime, aprime.size()), winv));
    bprime = vector_add(vector_scalar(slice(bprime, 0, nprime), winv), vector_scalar(slice(bprime, nprime, bprime.size()), w[round]));

    ++round;
  }
  PERF_TIMER_STOP(PROVE_step4);

  // PAPER LINE 58 (with inclusions from PAPER LINE 8 and PAPER LINE 20)
  return Bulletproof(V, A, S, T1, T2, taux, mu, L, R, aprime[0], bprime[0], t);
}

Bulletproof bulletproof_PROVE_old(const std::vector<uint64_t> &v, const rct::keyV &gamma)
{
  CHECK_AND_ASSERT_THROW_MES(v.size() == gamma.size(), "Incompatible sizes of v and gamma");

  // vG + gammaH
  PERF_TIMER_START_BP(PROVE_v);
  rct::keyV sv(v.size());
  for (size_t i = 0; i < v.size(); ++i)
  {
    sv[i] = rct::zero();
    sv[i].bytes[0] = v[i] & 255;
    sv[i].bytes[1] = (v[i] >> 8) & 255;
    sv[i].bytes[2] = (v[i] >> 16) & 255;
    sv[i].bytes[3] = (v[i] >> 24) & 255;
    sv[i].bytes[4] = (v[i] >> 32) & 255;
    sv[i].bytes[5] = (v[i] >> 40) & 255;
    sv[i].bytes[6] = (v[i] >> 48) & 255;
    sv[i].bytes[7] = (v[i] >> 56) & 255;
  }
  PERF_TIMER_STOP(PROVE_v);
  return bulletproof_PROVE_old(sv, gamma);
}

/* Given a range proof, determine if it is valid */
bool bulletproof_VERIFY_old(const std::vector<const Bulletproof*> &proofs)
{
  init_exponents();

  PERF_TIMER_START_BP(VERIFY);

  // sanity and figure out which proof is longest
  size_t max_length = 0;
  for (const Bulletproof *p: proofs)
  {
    const Bulletproof &proof = *p;
    CHECK_AND_ASSERT_MES(proof.V.size() >= 1, false, "V does not have at least one element");
    CHECK_AND_ASSERT_MES(proof.L.size() == proof.R.size(), false, "Mismatched L and R sizes");
    CHECK_AND_ASSERT_MES(proof.L.size() > 0, false, "Empty proof");

    max_length = std::max(max_length, proof.L.size());
  }
  CHECK_AND_ASSERT_MES(max_length < 32, false, "At least one proof is too large");
  size_t maxMN = 1u << max_length;

  const size_t logN = 6;
  const size_t N = 1 << logN;
  rct::key tmp;

  // setup weighted aggregates
  rct::key Z0 = rct::identity();
  rct::key z1 = rct::zero();
  rct::key Z2 = rct::identity();
  rct::key z3 = rct::zero();
  rct::keyV z4(maxMN, rct::zero()), z5(maxMN, rct::zero());
  for (const Bulletproof *p: proofs)
  {
    const Bulletproof &proof = *p;

    size_t M, logM;
    for (logM = 0; (M = 1<<logM) <= maxM && M < proof.V.size(); ++logM);
    CHECK_AND_ASSERT_MES(proof.L.size() == 6+logM, false, "Proof is not the expected size");
    const size_t MN = M*N;
    rct::key weight = rct::skGen();

    // Reconstruct the challenges
    PERF_TIMER_START_BP(VERIFY_start);
    rct::key hash_cache = rct::hash_to_scalar(proof.V);
    rct::key y = hash_cache_mash(hash_cache, proof.A, proof.S);
    rct::key z = hash_cache = rct::hash_to_scalar(y);
    rct::key x = hash_cache_mash(hash_cache, z, proof.T1, proof.T2);
    rct::key x_ip = hash_cache_mash(hash_cache, x, proof.taux, proof.mu, proof.t);
    PERF_TIMER_STOP(VERIFY_start);

    PERF_TIMER_START_BP(VERIFY_line_61);
    // PAPER LINE 61
    rct::key L61Left, L61Right;
    rct::addKeys2(L61Left, proof.taux, proof.t, rct::H);

    const rct::keyV zpow = vector_powers(z, M+3);

    rct::key k;
    const rct::key ip1y = vector_power_sum(y, MN);
    sc_mulsub(k.bytes, zpow[2].bytes, ip1y.bytes, rct::zero().bytes);
    for (size_t j = 1; j <= M; ++j)
    {
      CHECK_AND_ASSERT_MES(j+2 < zpow.size(), false, "invalid zpow index");
      sc_mulsub(k.bytes, zpow[j+2].bytes, ip12.bytes, k.bytes);
    }
    PERF_TIMER_STOP(VERIFY_line_61);

    // bos coster is slower for small numbers of calcs, straus seems not
    if (1)
    {
      PERF_TIMER_START_BP(VERIFY_line_61rl_new);
      sc_muladd(tmp.bytes, z.bytes, ip1y.bytes, k.bytes);
      std::vector<MultiexpData> multiexp_data;
      multiexp_data.reserve(3+proof.V.size());
      multiexp_data.emplace_back(tmp, rct::H);
      for (size_t j = 0; j < proof.V.size(); j++)
      {
        multiexp_data.emplace_back(zpow[j+2], proof.V[j]);
      }
      multiexp_data.emplace_back(x, proof.T1);
      rct::key xsq;
      sc_mul(xsq.bytes, x.bytes, x.bytes);
      multiexp_data.emplace_back(xsq, proof.T2);
      L61Right = multiexp(multiexp_data, false);
      PERF_TIMER_STOP(VERIFY_line_61rl_new);
    }
    else
    {
      PERF_TIMER_START_BP(VERIFY_line_61rl_old);
      sc_muladd(tmp.bytes, z.bytes, ip1y.bytes, k.bytes);
      L61Right = rct::scalarmultKey(rct::H, tmp);
      ge_p3 L61Right_p3;
      CHECK_AND_ASSERT_THROW_MES(ge_frombytes_vartime(&L61Right_p3, L61Right.bytes) == 0, "ge_frombytes_vartime failed");
      for (size_t j = 0; j+1 < proof.V.size(); j += 2)
      {
        CHECK_AND_ASSERT_MES(j+2+1 < zpow.size(), false, "invalid zpow index");
        ge_dsmp precomp0, precomp1;
        rct::precomp(precomp0, j < proof.V.size() ? proof.V[j] : rct::identity());
        rct::precomp(precomp1, j+1 < proof.V.size() ? proof.V[j+1] : rct::identity());
        rct::addKeys3acc_p3(&L61Right_p3, zpow[j+2], precomp0, zpow[j+2+1], precomp1);
      }
      for (size_t j = proof.V.size() & 0xfffffffe; j < M; j++)
      {
        CHECK_AND_ASSERT_MES(j+2 < zpow.size(), false, "invalid zpow index");
        // faster equivalent to:
        // tmp = rct::scalarmultKey(j < proof.V.size() ? proof.V[j] : rct::identity(), zpow[j+2]);
        // rct::addKeys(L61Right, L61Right, tmp);
        if (j < proof.V.size())
          addKeys_acc_p3(&L61Right_p3, zpow[j+2], proof.V[j]);
      }

      addKeys_acc_p3(&L61Right_p3, x, proof.T1);

      rct::key xsq;
      sc_mul(xsq.bytes, x.bytes, x.bytes);
      addKeys_acc_p3(&L61Right_p3, xsq, proof.T2);
      ge_p3_tobytes(L61Right.bytes, &L61Right_p3);
      PERF_TIMER_STOP(VERIFY_line_61rl_old);
    }

    if (!(L61Right == L61Left))
    {
      MERROR("Verification failure at step 1");
      return false;
    }

    PERF_TIMER_START_BP(VERIFY_line_62);
    // PAPER LINE 62
    rct::addKeys(Z0, Z0, rct::scalarmultKey(rct::addKeys(proof.A, rct::scalarmultKey(proof.S, x)), weight));
    PERF_TIMER_STOP(VERIFY_line_62);

    // Compute the number of rounds for the inner product
    const size_t rounds = logM+logN;
    CHECK_AND_ASSERT_MES(rounds > 0, false, "Zero rounds");

    PERF_TIMER_START_BP(VERIFY_line_21_22);
    // PAPER LINES 21-22
    // The inner product challenges are computed per round
    rct::keyV w(rounds);
    for (size_t i = 0; i < rounds; ++i)
    {
      w[i] = hash_cache_mash(hash_cache, proof.L[i], proof.R[i]);
    }
    PERF_TIMER_STOP(VERIFY_line_21_22);

    PERF_TIMER_START_BP(VERIFY_line_24_25);
    // Basically PAPER LINES 24-25
    // Compute the curvepoints from G[i] and H[i]
    rct::key yinvpow = rct::identity();
    rct::key ypow = rct::identity();

    PERF_TIMER_START_BP(VERIFY_line_24_25_invert);
    const rct::key yinv = invert(y);
    rct::keyV winv(rounds);
    for (size_t i = 0; i < rounds; ++i)
      winv[i] = invert(w[i]);
    PERF_TIMER_STOP(VERIFY_line_24_25_invert);

    for (size_t i = 0; i < MN; ++i)
    {
      // Convert the index to binary IN REVERSE and construct the scalar exponent
      rct::key g_scalar = proof.a;
      rct::key h_scalar;
      sc_mul(h_scalar.bytes, proof.b.bytes, yinvpow.bytes);

      for (size_t j = rounds; j-- > 0; )
      {
        size_t J = w.size() - j - 1;

        if ((i & (((size_t)1)<<j)) == 0)
        {
          sc_mul(g_scalar.bytes, g_scalar.bytes, winv[J].bytes);
          sc_mul(h_scalar.bytes, h_scalar.bytes, w[J].bytes);
        }
        else
        {
          sc_mul(g_scalar.bytes, g_scalar.bytes, w[J].bytes);
          sc_mul(h_scalar.bytes, h_scalar.bytes, winv[J].bytes);
        }
      }

      // Adjust the scalars using the exponents from PAPER LINE 62
      sc_add(g_scalar.bytes, g_scalar.bytes, z.bytes);
      CHECK_AND_ASSERT_MES(2+i/N < zpow.size(), false, "invalid zpow index");
      CHECK_AND_ASSERT_MES(i%N < twoN.size(), false, "invalid twoN index");
      sc_mul(tmp.bytes, zpow[2+i/N].bytes, twoN[i%N].bytes);
      sc_muladd(tmp.bytes, z.bytes, ypow.bytes, tmp.bytes);
      sc_mulsub(h_scalar.bytes, tmp.bytes, yinvpow.bytes, h_scalar.bytes);

      sc_muladd(z4[i].bytes, g_scalar.bytes, weight.bytes, z4[i].bytes);
      sc_muladd(z5[i].bytes, h_scalar.bytes, weight.bytes, z5[i].bytes);

      if (i != MN-1)
      {
        sc_mul(yinvpow.bytes, yinvpow.bytes, yinv.bytes);
        sc_mul(ypow.bytes, ypow.bytes, y.bytes);
      }
    }

    PERF_TIMER_STOP(VERIFY_line_24_25);

    // PAPER LINE 26
    PERF_TIMER_START_BP(VERIFY_line_26_new);
    std::vector<MultiexpData> multiexp_data;
    multiexp_data.reserve(2*rounds);

    sc_muladd(z1.bytes, proof.mu.bytes, weight.bytes, z1.bytes);
    for (size_t i = 0; i < rounds; ++i)
    {
      sc_mul(tmp.bytes, w[i].bytes, w[i].bytes);
      multiexp_data.emplace_back(tmp, proof.L[i]);
      sc_mul(tmp.bytes, winv[i].bytes, winv[i].bytes);
      multiexp_data.emplace_back(tmp, proof.R[i]);
    }
    rct::key acc = multiexp(multiexp_data, false);
    rct::addKeys(Z2, Z2, rct::scalarmultKey(acc, weight));
    sc_mulsub(tmp.bytes, proof.a.bytes, proof.b.bytes, proof.t.bytes);
    sc_mul(tmp.bytes, tmp.bytes, x_ip.bytes);
    sc_muladd(z3.bytes, tmp.bytes, weight.bytes, z3.bytes);
    PERF_TIMER_STOP(VERIFY_line_26_new);
  }

  // now check all proofs at once
  PERF_TIMER_START_BP(VERIFY_step2_check);
  rct::key Y = Z0;
  sc_sub(tmp.bytes, rct::zero().bytes, z1.bytes);
  rct::addKeys(Y, Y, rct::scalarmultBase(tmp));
  rct::addKeys(Y, Y, Z2);
  rct::addKeys(Y, Y, rct::scalarmultKey(rct::H, z3));

  std::vector<MultiexpData> multiexp_data;
  multiexp_data.reserve(2 * maxMN);
  for (size_t i = 0; i < maxMN; ++i)
  {
    sc_sub(tmp.bytes, rct::zero().bytes, z4[i].bytes);
    multiexp_data.emplace_back(tmp, Gi_p3[i]);
    sc_sub(tmp.bytes, rct::zero().bytes, z5[i].bytes);
    multiexp_data.emplace_back(tmp, Hi_p3[i]);
  }
  rct::addKeys(Y, Y, multiexp(multiexp_data, true));
  PERF_TIMER_STOP(VERIFY_step2_check);

  if (!(Y == rct::identity()))
  {
    MERROR("Verification failure at step 2");
    return false;
  }

  PERF_TIMER_STOP(VERIFY);
  return true;
}

bool bulletproof_VERIFY_old(const std::vector<Bulletproof> &proofs)
{
  std::vector<const Bulletproof*> proof_pointers;
  for (const Bulletproof &proof: proofs)
    proof_pointers.push_back(&proof);
  return bulletproof_VERIFY_old(proof_pointers);
}

bool bulletproof_VERIFY_old(const Bulletproof &proof)
{
  std::vector<const Bulletproof*> proofs;
  proofs.push_back(&proof);
  return bulletproof_VERIFY_old(proofs);
}

}


/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef THERMAL_PHYSICS_HH
#define THERMAL_PHYSICS_HH

#include <Geometry.hh>
#include <HeatSource.hh>
#include <ImplicitOperator.hh>
#include <Physics.hh>
#include <ThermalOperatorBase.hh>

#include <deal.II/base/time_stepping.h>
#include <deal.II/base/time_stepping.templates.h>
#include <deal.II/hp/fe_collection.h>

#include <boost/property_tree/ptree.hpp>

namespace adamantine
{
/**
 * This class takes care of building the linear operator and the
 * right-hand-side. Also used to evolve the system in time.
 */
template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
class ThermalPhysics : public Physics<dim, MemorySpaceType>
{
public:
  /**
   * Constructor.
   * \param[in] database requires the following entries:
   *   - materials: property tree
   *   - sources: property tree
   *   - sources.n_beams: unsigned int in \f$[0,\infty)\f$
   *   - sources.beam_X: property tree with X the number associated to
   *   the heat source
   *   - time_stepping: property tree
   *   - time_stepping.method: string
   *   - time_stepping.coarsening_parameter: double in \f$[0,\infty)\f$
   *   [optional, default value of 1.2]
   *   - time_stepping.refining_parameter: double in \f$(0,1)\f$
   *   [optional, default value of 0.8]
   *   - time_stepping.min_time_step: double in \f$[0,\infty)\f$
   *   [optional, default value of 1e-14]
   *   - time_stepping.max_time_step: double in \f$(0,\infty)\f$
   *   [optional, default value of 1e100]
   *   - time_stepping.refining_tolerance: double in \f$(0,\infty)\f$
   *   [optional, default value of 1e-8]
   *   - time_stepping.coarsening_tolerance: double in \f$(0, \infty)\f$
   *   [optional, default value of 1e-12]
   *   - time_stepping.max_iteration: unsigned int \f$[0,\infty)\f$
   *   [optional, default value of 1000]
   *   - time_stepping.right_preconditioning: boolean [optional, default
   *   value is false]
   *   - time_stepping.n_tmp_vectors: unsigned int in \f$[0,\infty)\f$
   *   [optional, default of 30]
   *   - time_stepping.newton_max_iteration: unsigned int in
   *   \f$[0,\infty)\f$ [optional, default valie of 100]
   *   - time_stepping.newton_tolerance: double in \f$(0,\infty)\f$
   *   [optional, default of 1e-6]
   *   - time_stepping.jfnk: boolean [optional, default value of false]
   */
  ThermalPhysics(MPI_Comm const &communicator,
                 boost::property_tree::ptree const &database,
                 Geometry<dim> &geometry);

  void setup_dofs() override;

  void compute_inverse_mass_matrix() override;

  void add_material(
      std::vector<typename dealii::DoFHandler<dim>::active_cell_iterator> const
          &elements_to_activate,
      double initial_temperature,
      dealii::LA::distributed::Vector<double, MemorySpaceType> &solution)
      override;

  double evolve_one_time_step(
      double t, double delta_t,
      dealii::LA::distributed::Vector<double, MemorySpaceType> &solution,
      std::vector<Timer> &timers) override;

  double get_delta_t_guess() const override;

  void initialize_dof_vector(
      dealii::LA::distributed::Vector<double, MemorySpaceType> &vector)
      const override;

  /**
   * Initialize the given vector. The value is assumed to be a temperature.
   */
  void
  initialize_dof_vector(double const value,
                        dealii::LA::distributed::Vector<double, MemorySpaceType>
                            &vector) const override;

  void get_state_from_material_properties() override;

  void set_state_to_material_properties() override;

  dealii::DoFHandler<dim> &get_dof_handler() override;

  dealii::AffineConstraints<double> &get_affine_constraints() override;

  std::shared_ptr<MaterialProperty<dim>> get_material_property() override;

  /**
   * Return the heat sources.
   */
  std::vector<std::shared_ptr<HeatSource<dim>>> &get_heat_sources();

  /**
   * Return the current height of the heat source.
   */
  double get_current_source_height() const;

private:
  using LA_Vector =
      typename dealii::LA::distributed::Vector<double, MemorySpaceType>;

  /**
   * Compute the right-hand side and apply the TermalOperator.
   */
  LA_Vector evaluate_thermal_physics(double const t, LA_Vector const &y,
                                     std::vector<Timer> &timers) const;

  /**
   * Compute the inverse of the ImplicitOperator.
   */
  LA_Vector id_minus_tau_J_inverse(double const t, double const tau,
                                   LA_Vector const &y,
                                   std::vector<Timer> &timers) const;

  /**
   * This flag is true if the time stepping method is embedded.
   */
  bool _embedded_method = false;
  /**
   * This flag is true if the time stepping method is implicit.
   */
  bool _implicit_method = false;
  /**
   * This flag is true if right preconditioning is used to invert the
   * ImplicitOperator.
   */
  bool _right_preconditioning;
  /**
   * Maximum number of iterations to invert the ImplicitOperator.
   */
  unsigned int _max_iter;
  /**
   * Maximum number of temporary vectors when inverting the ImplicitOperator.
   */
  unsigned int _max_n_tmp_vectors;
  /**
   * Guess of the next time step.
   */
  double _delta_t_guess;
  /**
   * Tolerance to inverte the ImplicitOperator.
   */
  double _tolerance;
  /**
   * Current height of the object.
   */
  double _current_source_height = 0.;
  /**
   * Type of boundary.
   */
  BoundaryType _boundary_type;
  /**
   * Associated geometry.
   */
  Geometry<dim> &_geometry;
  /**
   * Associated Lagrange finite elements.
   */
  dealii::hp::FECollection<dim> _fe_collection;
  /**
   * Associated DoFHandler.
   */
  dealii::DoFHandler<dim> _dof_handler;
  /**
   * Associated AffineConstraints<double>.
   */
  dealii::AffineConstraints<double> _affine_constraints;
  /**
   * Associated quadature, either Gauss or Gauss-Lobatto.
   */
  dealii::hp::QCollection<1> _q_collection;
  /**
   * Shared pointer to the material properties associated to the domain.
   */
  std::shared_ptr<MaterialProperty<dim>> _material_properties;
  /**
   * Vector of heat sources.
   */
  std::vector<std::shared_ptr<HeatSource<dim>>> _heat_sources;
  /**
   * Shared pointer to the underlying ThermalOperator.
   */
  std::shared_ptr<ThermalOperatorBase<dim, MemorySpaceType>> _thermal_operator;
  /**
   * Unique pointer to the underlying ImplicitOperator.
   */
  std::unique_ptr<ImplicitOperator<MemorySpaceType>> _implicit_operator;
  /**
   * Shared pointer to the underlying time stepping scheme.
   */
  std::unique_ptr<dealii::TimeStepping::RungeKutta<LA_Vector>> _time_stepping;
};

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline double ThermalPhysics<dim, fe_degree, MemorySpaceType,
                             QuadratureType>::get_delta_t_guess() const
{
  return _delta_t_guess;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline dealii::DoFHandler<dim> &
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_dof_handler()
{
  return _dof_handler;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline dealii::AffineConstraints<double> &
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_affine_constraints()
{
  return _affine_constraints;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline std::shared_ptr<MaterialProperty<dim>>
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_material_property()
{
  return _material_properties;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline std::vector<std::shared_ptr<HeatSource<dim>>> &
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_heat_sources()
{
  return _heat_sources;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline double ThermalPhysics<dim, fe_degree, MemorySpaceType,
                             QuadratureType>::get_current_source_height() const
{
  return _current_source_height;
}
} // namespace adamantine

#endif


// Compile with cmake (CMakeLists.txt is provided) or with the following lines in bash:
// g++ -c -fPIC libautosim.cpp -o libautosim.o
// g++ -shared -Wl,-soname,libautosim.so -o libautosim.so libautosim.o

#include <string>
#include <sstream>
#include <iostream>
#include <list>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <algorithm>
#include <float.h>
#include <limits>
#include <cmath>
#include <limits>

#include <bitset>
#include <boost/math/distributions/binomial.hpp>
#include "cpp_libs/libOrsa/orsa_fundamental.hpp"
#include "cpp_libs/libOrsa/orsa_homography.hpp"

#define FullDescDim 6272
#define VecDescDim 128 // as in (x,x,128) output of the network
#define SameKPThres 4

struct TargetNode
{
  const int TargetIdx; float sim_with_query;
  TargetNode(int Idx, float sim):TargetIdx(Idx){ this->sim_with_query = sim; };
  bool operator >(const TargetNode & kp) {return ( this->sim_with_query>kp.sim_with_query );};
  bool operator ==(const TargetNode & kp) {return ( this->sim_with_query==kp.sim_with_query );};
};

struct QueryNode
{
  const int QueryIdx;
  std::list<QueryNode>::iterator thisQueryNodeOnList; //pointer on list
  float first_sim, last_sim;
  std::list<TargetNode> MostSimilar_TargetNodes;

  void Add_TargetNode(int it, float sim, int MaxTnodes_num)
  {
    TargetNode tn(it,sim);
    std::list<TargetNode>::iterator target_iter;
    for(target_iter = MostSimilar_TargetNodes.begin(); target_iter != MostSimilar_TargetNodes.end(); ++target_iter)
      if ( tn > *target_iter )
        break;

    MostSimilar_TargetNodes.insert( target_iter, tn );
    if (MaxTnodes_num>0 && MostSimilar_TargetNodes.size()>MaxTnodes_num)
      MostSimilar_TargetNodes.pop_back();
      last_sim = (--MostSimilar_TargetNodes.end())->sim_with_query;
    first_sim = MostSimilar_TargetNodes.begin()->sim_with_query;
  };

  QueryNode(int Idx):QueryIdx(Idx){ first_sim = -1; last_sim = -1; };
};

struct DescStatsClass
{
  float norm, max, min, mean, sigma;
  std::bitset<FullDescDim> AID;
  std::bitset<VecDescDim> AIDbyVec [FullDescDim/VecDescDim];

  DescStatsClass()
  {
    norm = 0.0f, mean = 0.0f, sigma = 0.0f;
    max = -std::numeric_limits<float>::infinity();
    min = std::numeric_limits<float>::infinity();
  };
};

int CountItFast(const DescStatsClass & q, const DescStatsClass & t, int thres)
{
  int xor_opp = 0;
  for (int v = 0; (v < FullDescDim/VecDescDim && (xor_opp < thres)); v++)
    xor_opp +=  (q.AIDbyVec[v] ^ t.AIDbyVec[v]).count();
  return(xor_opp);
}

float FastSimi(int iq, float* DescsQuery, DescStatsClass* QueryStats, int it, float* DescsTarget, DescStatsClass* TargetStats, float simi_thres)
{
  float m_sq = QueryStats[iq].max * TargetStats[it].max;
  float norms_prod = QueryStats[iq].norm * TargetStats[it].norm;
  float dynamic_thres = (simi_thres * norms_prod) - FullDescDim * m_sq;
  float SP = 0.0;
  int qpos = iq*FullDescDim, tpos = it*FullDescDim;
  for (int i = 0; (i < FullDescDim); i++)
  {
    SP +=  DescsQuery[qpos + i] * DescsTarget[tpos + i];
  }
  return(SP/norms_prod);
};

DescStatsClass LoadInfo(int iq, float* DescsQuery)
{
  DescStatsClass ds;
  float val;
  int qpos = iq*FullDescDim;
  for (int i = 0; (i < FullDescDim); i++)
  {
    val = DescsQuery[qpos + i];
    ds.norm += val * val;
  }

  for (int i = 0; (i < FullDescDim); i++)
    if (DescsQuery[qpos + i]>=0)
    {
      ds.AID.set(i);
    }

  for (int v = 0; (v < FullDescDim/VecDescDim); v++)
  {
    int vpos = VecDescDim*v;
    for (int i = 0; (i < VecDescDim); i++)
      {
        if (DescsQuery[qpos + vpos + i]>=0)
          ds.AIDbyVec[v].set(i);
      }
  }

  ds.norm = std::sqrt(ds.norm);
  return(ds);
};

// this gives P(X>=k) when X is a binomial distribution of parameters N and p
float* BinomialSurvivals(int N, float p)
{
  float * survivals = new float[N+1];
  // binomial distribution object:
  boost::math::binomial bino(N,p);
  // survival values
  for (int x=0; x<=N; x++)
    survivals[x] = cdf(complement(bino, x));
  return(survivals);
}

struct KeypointClass{
  float x,y;
};

struct MatchersClass
{
  std::list<QueryNode> QueryNodes;
  const int k; // k as in knn, if <=0 then we store all matches having a sim above sim_thres
  const float sim_thres;
  const int Nvec;
  std::vector<KeypointClass> QueryKPs, TargetKPs;

  int* FilteredIdxMatches;
  int N_FilteredMatches;

  int BinDesc_GetAlarmThreshold(int Nt)
  {
    float *survivals = BinomialSurvivals(VecDescDim, 0.5);
    int alarmThres = 0.0;
    for (int i = 0; i <= VecDescDim; i++)
      if (Nt * survivals[i] <= 1)
      {
        alarmThres = (int)(i * Nvec);
        break;
      }
    return (alarmThres);
  }

  MatchersClass(int knn_num, float sim_thres) : k(knn_num), sim_thres(sim_thres), Nvec((int)(FullDescDim / VecDescDim)){};

  void KnnMatcher(float *DescsQuery, int Nquery, float *DescsTarget, int Ntarget, int FastCode)
  {
    DescStatsClass *QueryStats = new DescStatsClass[Nquery], *TargetStats = new DescStatsClass[Ntarget];
#pragma omp parallel for default(shared)
    for (int iq = 0; iq < Nquery; iq++)
      QueryStats[iq] = LoadInfo(iq, DescsQuery);
#pragma omp parallel for default(shared)
    for (int it = 0; it < Ntarget; it++)
      TargetStats[it] = LoadInfo(it, DescsTarget);

    switch (FastCode)
    {
      case 0:
      { // BigAID
        // std::cout<<"---> Brute Force Angle Comparisons"<<std::endl;
  #pragma omp parallel for default(shared)
        for (int iq = 0; iq < Nquery; iq++)
        {
          QueryNode qn(iq);
          for (int it = 0; it < Ntarget; it++)
          {
            float updated_sim_thres = (this->k > 0 && qn.last_sim > sim_thres) ? qn.last_sim : sim_thres;
            float simi = FastSimi(iq, DescsQuery, QueryStats, it, DescsTarget, TargetStats, updated_sim_thres);
            if (simi > updated_sim_thres)
              qn.Add_TargetNode(it, simi, this->k);
          }
          if (qn.first_sim > sim_thres)
  #pragma omp critical
          {
            QueryNodes.push_back(qn);
            std::list<QueryNode>::iterator itqn = --QueryNodes.end();
            itqn->thisQueryNodeOnList = itqn;
          }
        }
        break;
      } // end of BigAID
      case 1:
      { // model new AID
        // std::cout << "---> Full sign comparisons with bitset!!!" << std::endl;
  #pragma omp parallel for default(shared)
        for (int iq = 0; iq < Nquery; iq++)
        {
          QueryNode qn(iq);
          float updated_sim_thres;
          for (int it = 0; it < Ntarget; it++)
          {
            // This is like counting bits after an XNOR opperation on both binary descriptors
            updated_sim_thres = (this->k > 0 && qn.last_sim > sim_thres) ? qn.last_sim : sim_thres;
            float simi = (float) ( FullDescDim - CountItFast(QueryStats[iq], TargetStats[it], FullDescDim - updated_sim_thres) );
            if (simi > updated_sim_thres)
              qn.Add_TargetNode(it, simi, this->k);
          }
          if (qn.first_sim > sim_thres)
          {
  #pragma omp critical
            {
              QueryNodes.push_back(qn);
              std::list<QueryNode>::iterator itqn = --QueryNodes.end();
              itqn->thisQueryNodeOnList = itqn;
            }
          }
        }
        break;
      } // end of AID
      case 2:
      { // model AID
        // std::cout << "---> Full sign comparisons with bitset!!!" << std::endl;
  #pragma omp parallel for default(shared)
        for (int iq = 0; iq < Nquery; iq++)
        {
          QueryNode qn(iq);
          for (int it = 0; it < Ntarget; it++)
          {
            // This is like counting bits after an XNOR opperation on both binary descriptors
            int concor = FullDescDim - (QueryStats[iq].AID ^ TargetStats[it].AID).count();
            float updated_sim_thres = (this->k > 0 && qn.last_sim > sim_thres) ? qn.last_sim : sim_thres;
            float simi = (float)concor;
            if (simi > updated_sim_thres)
              qn.Add_TargetNode(it, simi, this->k);
          }
          if (qn.first_sim > sim_thres)
          {
  #pragma omp critical
            {
              QueryNodes.push_back(qn);
              std::list<QueryNode>::iterator itqn = --QueryNodes.end();
              itqn->thisQueryNodeOnList = itqn;
            }
          }
        }
        break;
      } // end of AID
    } // end of the switch
  }

private:
  MatchersClass() : k(0), sim_thres(0.0), Nvec(0){};
};

float max_euclidean_dist(const Match & m1, const Match & m2)
{
  float left_dist = std::sqrt( std::pow(m1.x1-m2.x1,2.0) + std::pow(m1.y1-m2.y1,2.0) );
  float right_dist = std::sqrt( std::pow(m1.x2-m2.x2,2.0) + std::pow(m1.y2-m2.y2,2.0) );
  if (left_dist>right_dist)
    return left_dist;
  else
    return right_dist;
}

std::vector<Match> UniqueFilter(const std::vector<Match>& matches)
{
  std::vector<Match> uniqueM;
  bool *duplicatedM = new bool[matches.size()];
  float best_sim;
  int bestidx;
  for (int i =0; i<matches.size(); i++)
    duplicatedM[i] = false;
  for (int i =0; i<matches.size(); i++)
  {
    if (duplicatedM[i])
      continue;
    best_sim = matches[i].similarity;
    bestidx = i;
    for(int j=i+1; j<matches.size();j++)
    {
      // std::cout<< max_euclidean_dist(matches[i],matches[j])<<std::endl;
      if ( !duplicatedM[j] && max_euclidean_dist(matches[i],matches[j])<SameKPThres )
      {
        duplicatedM[j] = true;
        if (best_sim<matches[j].similarity)
        {
          bestidx = j;
          best_sim = matches[j].similarity;
        }
      }
    }
    uniqueM.push_back(matches[bestidx]);
  }
  return uniqueM;
}

void ORSA_Filter(std::vector<Match>& matches, bool* MatchMask, float* T, int w1,int h1,int w2,int h2, bool Fundamental, const double & precision, bool verb)
{
  libNumerics::matrix<double> H(3,3);
  std::vector<int> vec_inliers;
  double nfa;
  const float nfa_max = -2;
  const int ITER_ORSA=10000;
  if (Fundamental)
    orsa::orsa_fundamental(matches, w1,h1,w2,h2, precision, ITER_ORSA,H, vec_inliers,nfa,verb);
  else
     orsa::ORSA_homography(matches, w1,h1,w2,h2, precision, ITER_ORSA,H, vec_inliers,nfa,verb);

  for (int cc = 0; cc < matches.size(); cc++ )
    MatchMask[cc] = false;

  if ( nfa < nfa_max )
  {
    for (int vi = 0; vi < vec_inliers.size(); vi++ )
      MatchMask[vec_inliers[vi]] = true;
    H /= H(2,2);

    int t = 0;
    for(int i = 0; i < H.nrow(); ++i)
        for (int j = 0; j < H.ncol(); ++j)
            T[t++] = H(i,j);
    if (verb)
    {
        printf("The two images match! %d matchings are identified. log(nfa)=%.2f.\n", (int) vec_inliers.size(), nfa);
        if (Fundamental)
          std::cout << "*************** Fundamental **************"<< std::endl;
        else
          std::cout << "*************** Homography ***************"<< std::endl;
        std::cout << H <<std::endl;
        std::cout << "******************************************"<< std::endl;
    }
  }
  else
  {
    if (verb)
        printf("The two images do not match. The matching is not significant:  log(nfa)=%.2f.\n", nfa);
  }
}

// Define C functions for the C++ class - as ctypes can only talk to C...
extern "C"
{

  int NumberOfFilteredMatches(MatchersClass* M)
  {
    return M->N_FilteredMatches;
  }

  void ArrayOfFilteredMatches(MatchersClass* M, int* arr)
  {
    for (int i=0; i<3*M->N_FilteredMatches;i++)
      arr[i] = M->FilteredIdxMatches[i];
  }

  void GeometricFilterFromNodes(MatchersClass* M, float* T, int w1,int h1,int w2,int h2, int type, float precision, bool verb)
  {
    std::vector<Match> matches;
    for(std::list<QueryNode>::const_iterator iq = M->QueryNodes.begin(); iq != M->QueryNodes.end(); ++iq)
      for(std::list<TargetNode>::const_iterator it = iq->MostSimilar_TargetNodes.begin(); it != iq->MostSimilar_TargetNodes.end(); ++it)
    {
      Match match1;
      match1.x1 = M->QueryKPs[iq->QueryIdx].x;
      match1.y1 = M->QueryKPs[iq->QueryIdx].y;
      match1.x2 = M->TargetKPs[it->TargetIdx].x;
      match1.y2 = M->TargetKPs[it->TargetIdx].y;
      match1.similarity = it->sim_with_query;
      match1.Qidx = iq->QueryIdx;
      match1.Tidx = it->TargetIdx;
      matches.push_back(match1);
    }

    matches = UniqueFilter(matches);
    bool* MatchMask = new bool[matches.size()];
    switch (type)
    {
      case 0: //Homography
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, false, (double)precision, verb);
        break;
      }
      case 1: // Fundamental
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, true, (double)precision, verb);
        break;
      }
    }

    M->N_FilteredMatches = 0;
    for (int cc = 0; cc < matches.size(); cc++ )
      if (MatchMask[cc])
        M->N_FilteredMatches++;
    M->FilteredIdxMatches = new int[3*M->N_FilteredMatches];
    int fcc = 0;
    for (int cc = 0; cc < matches.size(); cc++ )
      if (MatchMask[cc])
        {
          M->FilteredIdxMatches[3*fcc] = matches[cc].Qidx;
          M->FilteredIdxMatches[3*fcc+1] = matches[cc].Tidx;
          M->FilteredIdxMatches[3*fcc+2] = matches[cc].similarity;
          fcc++;
        }
  }

  void GeometricFilter(float* scr_pts, float* dts_pts, bool* MatchMask, float* T, int N, int w1,int h1,int w2,int h2, int type, float precision, bool verb)
  {
    std::vector<Match> matches;
    for (int cc = 0; cc < N; cc++ )
    {
        Match match1;
        match1.x1 = scr_pts[cc*2];
        match1.y1 = scr_pts[cc*2+1];
        match1.x2 = dts_pts[cc*2];
        match1.y2 = dts_pts[cc*2+1];

        matches.push_back(match1);
    }

    switch (type)
    {
      case 0: //Homography
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, false, (double)precision, verb);
        break;
      }
      case 1: // Fundamental
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, true, (double)precision, verb);
        break;
      }
    }
  }

  MatchersClass* newMatcher(int k, int full_desc_dim, float sim_thres)
  {
    if (full_desc_dim!=FullDescDim)
      std::cout<<"Desc dims don't match ("<<full_desc_dim<<"!="<<FullDescDim<<")"<<std::endl;
    return ( new MatchersClass(k, (float) sim_thres) );
  }

  void KnnMatcher(MatchersClass* M, float* Query_pts, float* DescsQuery, int Nquery, float* Target_pts, float* DescsTarget, int Ntarget, int FastCode)
  {
    M->QueryNodes.clear();
    M->QueryKPs.clear();
    M->TargetKPs.clear();
    KeypointClass kp;
    for (int cc = 0; cc < Nquery; cc++ ){
      kp.x = Query_pts[cc*2]; kp.y = Query_pts[cc*2+1];
      M->QueryKPs.push_back( kp );
    }
    for (int cc = 0; cc < Ntarget; cc++ ){
      kp.x = Target_pts[cc*2]; kp.y = Target_pts[cc*2+1];
      M->TargetKPs.push_back( kp );
    }

    M->KnnMatcher(DescsQuery, Nquery, DescsTarget, Ntarget, FastCode);
  }

  int GetQueryNodeLength(QueryNode* qn)
  {
    return(qn->MostSimilar_TargetNodes.size());
  }

  QueryNode* LastQueryNode(MatchersClass* M)
  {
    if (M->QueryNodes.begin()!=M->QueryNodes.end())
      return(&*(--M->QueryNodes.end()));
    else
      return(0);
  }

  QueryNode* FirstQueryNode(MatchersClass* M)
  {
    if (M->QueryNodes.begin()!=M->QueryNodes.end())
      return(&*M->QueryNodes.begin());
    else
      return(0);
  }

  QueryNode* NextQueryNode(MatchersClass* M, QueryNode* qn)
  {
    if (qn!=0 && ++qn->thisQueryNodeOnList!=M->QueryNodes.end())
      return(&*(++qn->thisQueryNodeOnList));
    else
      return(0);
  }

  QueryNode* PrevQueryNode(MatchersClass* M, QueryNode* qn)
  {
    if (qn!=0 && qn->thisQueryNodeOnList!=M->QueryNodes.begin())
      return(&*(--qn->thisQueryNodeOnList));
    else
      return(0);
  }

  void GetData_from_QueryNode(QueryNode* qn, int* QueryIdx, int *TargetIdxes, float* simis)
  {
    QueryIdx[0] = qn->QueryIdx;
    int i = 0;
    for(std::list<TargetNode>::const_iterator it = qn->MostSimilar_TargetNodes.begin(); it != qn->MostSimilar_TargetNodes.end(); ++it)
    {
      TargetIdxes[i] = it->TargetIdx;
      simis[i] = it->sim_with_query;
      i++;
    }
  }

  void FastMatCombi(int N, float* bP, int* i1_list, int *i2_list, float *patches1, float *patches2, int MemStepImg, int* last_i1_list, int *last_i2_list)
  {
    int MemStepBlock = 2*MemStepImg;
    #pragma omp parallel for firstprivate(MemStepImg, MemStepBlock, N)
    for (int k = 0; k<N; k++)
    {
      int i1 = i1_list[k];
      int i2 = i2_list[k];

      if (last_i1_list[k]!=i1)
        for (int i = 0; i<MemStepImg;i++)
          bP[k*MemStepBlock + 2*i] = patches1[i1*MemStepImg + i];

      if (last_i2_list[k]!=i2)
        for (int i = 0; i<MemStepImg;i++)
          bP[k*MemStepBlock + 2*i+1] = patches2[i2*MemStepImg + i];
    }
  }
}


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

#include "elasticregistration.hpp"

#include <boost/format.hpp>

#include "basewriter.hpp"
#include "exceptions.hpp"
#include "file_utils.hpp"
#include "image.hpp"
#include "mapmanager.hpp"
#include "mask.hpp"
#include "math_utils.hpp"
#include "tmatrix.hpp"

ElasticRegistration::ElasticRegistration(std::shared_ptr<Image> fixed, std::shared_ptr<Image> moved,
                                         std::shared_ptr<Mask> mask, const intcoord& target_spacing,
                                         const ConfigurationBase& config)
  : _comm(fixed->comm()), _fixed(std::move(fixed)), _moved(std::move(moved)), _mask(std::move(mask)),
    _target_spacing(target_spacing), _configuration(config),
    _mapmanager(std::make_unique<MapManager>(target_spacing, *_mask)),
    _registered(GridVariable::copy(*_moved))
{
}

void ElasticRegistration::set_initial_guess(const MapBase& initial_guess __attribute__((unused)))
{
  // Query manager for map closest in shape to initial_guess
  // Set Mapmanager index to that shape
  // Interpolate initial_guess to current map
  // Carry on from there...
  throw InternalError("Not yet implemented");
}

void ElasticRegistration::autoregister(integer max_iterations)
{
  for (_current_map = _mapmanager->get_current_map(); _current_map != nullptr;
       _current_map = _mapmanager->get_next_map())
  {
    registration_inner_loop(max_iterations);
  }
  _current_map = _mapmanager->get_current_map(); // Replace nullptr with final valid map
}

void ElasticRegistration::registration_inner_loop(integer max_iterations)
{
  integer iteration = 0;
  if (_configuration.grab<bool>("save_intermediate_frames"))
  {
    save_debug_frame(iteration);
  }
  for (iteration = 1; iteration <= max_iterations; iteration++)
  {
    this->_previous_mis.push_back(this->_fixed->mutual_information(*this->_moved));

    // Do solver step, recalculating lambda if it is the first iteration of the loop
    Vec_unique map_delta = solver_step(iteration, iteration == 1);

    if (this->is_converged(*map_delta))
    {
      break;
    }
  }
  if (iteration == max_iterations)
  {
    PetscPrintf(_comm, "Warning: Maximum iteration limit reached (%d)\n", max_iterations);
  }
}

Vec_unique ElasticRegistration::solver_step(floating iternum, bool recalculate_lambda)
{
  auto tmat2_and_tmatfm = build_tmat2_and_tmatfm(*this->_fixed, *_registered, *this->_current_map);
  Mat_unique tmat2 = std::move(tmat2_and_tmatfm.first);
  Vec_unique tmatfm = std::move(tmat2_and_tmatfm.second);

  floating lapl2_diag_sum = diagonal_sum(_current_map->laplacian2());
  floating tmat2_diag_sum = diagonal_sum(*tmat2);
  floating lapl_premult = tmat2_diag_sum / lapl2_diag_sum;
  PetscPrintf(_comm, "lapl_mult = %f, laplsum= %f, tmatsum = %f\n", lapl_premult, lapl2_diag_sum,
              tmat2_diag_sum);

  // Recalculate lambda if requested
  if (recalculate_lambda)
  {
    _lambda = approximate_optimum_lambda(*tmat2, _current_map->laplacian2(), lapl_premult, _lambda,
                                         _initial_search_width, _lambda_search_maxiter, _lambda_min);
    PetscPrintf(_comm, "Calculated lambda = %f\n", _lambda);
  }

  // Add lambda*L^tL to T^t T
  PetscErrorCode perr = MatAXPY(*tmat2, _lambda * lapl_premult, _current_map->laplacian2(),
                                DIFFERENT_NONZERO_PATTERN);
  CHKERRXX(perr);

  // Subtract lambda*L^tL*a from T^t(f-m)
  Vec_unique lambda_l2_a = create_unique_vec();
  perr = VecDuplicate(*tmatfm, lambda_l2_a.get());
  CHKERRXX(perr);
  perr = MatMult(_current_map->laplacian2(), _current_map->displacement_vector(), *lambda_l2_a);
  CHKERRXX(perr);
  //  perr = VecAXPY(*tmatfm, -_lambda * lapl_premult, *lambda_l2_a);
  // CHKERRXX(perr);

  // Co-opt now defunct lambda_l2_a as solution vector
  Vec_unique delta = std::move(lambda_l2_a);

  // Now do actual solve
  KSP_unique m_ksp = create_unique_ksp();
  perr = KSPCreate(_comm, m_ksp.get());
  CHKERRXX(perr);
  perr = KSPSetOperators(*m_ksp, *tmat2, *tmat2);
  CHKERRXX(perr);
  perr = KSPSetUp(*m_ksp);
  CHKERRXX(perr);
  perr = KSPSetFromOptions(*m_ksp);
  CHKERRXX(perr);
  perr = KSPSetUp(*m_ksp);
  CHKERRXX(perr);
  perr = KSPSolve(*m_ksp, *tmatfm, *delta);
  CHKERRXX(perr);

  // update map
  _current_map->update(*delta);

  // warp image
  _registered = _moved->warp(*_current_map);
  _registered->normalize();

  floating mutinf = _registered->mutual_information(*_fixed);
  _previous_mis.push_back(mutinf);
  PetscPrintf(_comm, "Mutual information: %f\n", mutinf);

  if (_configuration.grab<bool>("save_intermediate_frames"))
  {
    save_debug_frame(iternum);
  }

  return delta;
}

bool ElasticRegistration::is_converged(const Vec& map_delta, floating threshold)
{
  // Roll our own because we need to avoid intensity values
  std::array<floating, 2> avg_data;
  auto& avg_sum = avg_data[0];
  auto& avg_num = avg_data[1];

  floating localmax = PETSC_MIN_REAL;
  integer local_lo, local_hi;
  PetscErrorCode perr = VecGetOwnershipRange(map_delta, &local_lo, &local_hi);
  CHKERRXX(perr);
  avg_num = local_hi - local_lo;

  const floating* delta_data;
  perr = VecGetArrayRead(map_delta, &delta_data);
  CHKERRXX(perr);
  for (integer iidx = local_lo; iidx < local_hi; iidx++)
  {
    if (iidx % _current_map->ndof() == 0) // skip intensity values
    {
      continue;
    }
    floating absval = std::abs(delta_data[iidx]);
    localmax = absval > localmax ? absval : localmax;
    avg_sum += absval;
  }
  perr = VecRestoreArrayRead(map_delta, &delta_data);
  CHKERRXX(perr);

  MPI_Allreduce(MPI_IN_PLACE, &localmax, 1, MPIU_SCALAR, MPI_MAX, _comm);
  MPI_Allreduce(MPI_IN_PLACE, avg_data.data(), 2, MPIU_SCALAR, MPI_SUM, _comm);

  floating average = avg_sum / avg_num;

  PetscPrintf(_comm, "Displacement delta: %f / %f (avg / max)\n", average, localmax);

  bool disp_conv = average < threshold;

  // Now calculate gradient of MI over last n iterations
  // First check if we need to discard values
  while (this->_previous_mis.size() > this->_mi_window)
  {
    this->_previous_mis.pop_front();
  }

  // Now calculate simple linear regression
  intvector xpoints(this->_previous_mis.size());
  std::iota(xpoints.begin(), xpoints.end(), 1);
  floating num_points = static_cast<double>(xpoints.size());

  floating s_x = 0.5 * num_points * (num_points + 1);
  floating s_xx = (1.0 / 6.0) * (num_points * (num_points + 1) * (2 * num_points + 1));
  floating s_y = std::accumulate(this->_previous_mis.cbegin(), this->_previous_mis.cend(), 0.);
  floating s_xy = accumulate(xpoints.cbegin(), xpoints.cend(), this->_previous_mis.cbegin(), 0.,
                             [](floating x, integer y, floating z) -> floating { return x + y * z; });

  floating mi_grad = (num_points*s_xy - s_x*s_y)/(num_points*s_xx - s_x*s_x);

  PetscPrintf(_comm, "Mutual Information gradient: %f\n", mi_grad);

  bool mi_conv = false;
  if(this->_previous_mis.size() >= this->_mi_window)
  {
    mi_conv = mi_grad < 0;
  }

  return disp_conv || mi_conv;
}

void ElasticRegistration::save_debug_frame(integer iteration_num)
{
  integer outer_count = _mapmanager->step_number();
  std::ostringstream outname;
  std::string file_str(_configuration.grab<std::string>("intermediate_template"));
  bf::path registered_path(_configuration.grab<std::string>("registered"));

  boost::format pad2("%02d");
  replace_token(file_str, ConfigurationBase::k_outer_token, (pad2 % outer_count).str());

  boost::format pad3("%03d");
  replace_token(file_str, ConfigurationBase::k_inner_token, (pad3 % iteration_num).str());

  replace_token(file_str, ConfigurationBase::k_stem_token, registered_path.filename().stem().string());

  replace_token(file_str, ConfigurationBase::k_extension_token, registered_path.extension().string());

  bf::path output_path(registered_path.parent_path());

  bf::path intermediates_path(_configuration.grab<std::string>("intermediate_directory"));
  if (intermediates_path.is_absolute())
  {
    if (!bf::exists(intermediates_path))
    {
      std::ostringstream errss;
      errss << "Intermediate frame output path " << intermediates_path << " does not exist.";
      throw std::runtime_error(errss.str());
    }
    output_path = intermediates_path;
  }
  else
  {
    // Don't create directories we don't say we will, error instead
    // (Pre-flight checks mean this should never throw)
    if (!output_path.empty() && !bf::exists(output_path))
    {
      std::ostringstream errss;
      errss << "Output path " << output_path << " does not exist.";
      throw std::runtime_error(errss.str());
    }

    // Append intermediates dir
    output_path /= _configuration.grab<std::string>("intermediate_directory");

    // Create intermediates dir as needed
    bf::create_directories(output_path);
  }

  // Finally append filename and any group extension
  output_path /= file_str;
  std::string output_path_str(output_path.string());
  output_path_str.append(":");
  output_path_str.append(_configuration.grab<std::string>("registered_h5_path"));

  BaseWriter_unique wtr = BaseWriter::get_writer_for_filename(output_path_str, _comm);
  wtr->write_image(*_registered);
}

floating ElasticRegistration::approximate_optimum_lambda(const Mat& mat_a, const Mat& mat_b,
                                                         floating lambda_mult, floating initial_guess,
                                                         floating search_width, uinteger max_iter,
                                                         floating lambda_min)
{
  floating x_lo = initial_guess - search_width;
  x_lo = x_lo > lambda_min ? x_lo : lambda_min;
  floating x_mid = initial_guess;
  x_mid = x_mid > x_lo ? x_mid : x_lo + search_width/2;
  floating x_hi = initial_guess + search_width;
  x_hi = x_hi > x_mid ? x_hi : x_mid + search_width;

  floating y_lo(0), y_mid(0), y_hi(0);
  for (uinteger iter = 0; iter < max_iter; iter++)
  {
    Mat_unique mat_c = create_unique_mat();
    PetscErrorCode perr = MatDuplicate(mat_a, MAT_COPY_VALUES, mat_c.get());

    // calculate at lower value
    perr = MatAXPY(*mat_c, lambda_mult * x_lo, mat_b, DIFFERENT_NONZERO_PATTERN);
    CHKERRXX(perr);
    y_lo = get_condnum_by_poweriter(*mat_c, 0.01, 100);

    // Calculate at initial guess
    perr = MatAXPY(*mat_c, lambda_mult * x_mid, mat_b, DIFFERENT_NONZERO_PATTERN);
    CHKERRXX(perr);
    y_mid = get_condnum_by_poweriter(*mat_c, 0.01, 100);

    // calculate at higher value
    perr = MatAXPY(*mat_c, lambda_mult * x_hi, mat_b, DIFFERENT_NONZERO_PATTERN);
    CHKERRXX(perr);
    y_hi = get_condnum_by_poweriter(*mat_c, 0.01, 100);

    PetscPrintf(_comm, " lo: %g(%g) mid: %g(%g) hi: %g(%g)\n", x_lo, y_lo, x_mid, y_mid, x_hi, y_hi);

    // check initial lower than either side
    if (y_lo >= y_mid)
    {
      // two options, either close to a minimum or need to search to higher x
      if (y_hi < y_mid)
      {
        // All points are negative gradient: keep searching to higher values of x:
        x_mid *= 2;
        x_hi *= 2;
        continue;
      }
      // Otherwise we are close to a local minimum and are done
      break;
    }

    // If we already reached lambda_min we need to be done at that
    if (x_lo == lambda_min)
    {
      break;
    }

    // Otherwise it is a positive gradient so keep searching left
    x_lo = x_lo >= 2.0 * lambda_min ? x_lo / 2 : 1.0 * lambda_min;
    x_mid = x_mid >= 2.0 * x_lo ? x_mid / 2 : x_lo + 1.0;
    x_hi = x_hi >= 2.0 * x_mid ? x_hi / 2 : x_mid + 1.0;
  }
  // fit quadratic and return minimum
  floating a, b, c;
  quadratic_from_points(x_lo, x_mid, x_hi, y_lo, y_mid, y_hi, a, b, c);

  if (a < 0)
  {
    PetscPrintf(_comm, "Warning: convex function in smoothing parameter estimation.\n");
    return lambda_min;
  }

  floating x, y;
  quadratic_vertex(a, b, c, x, y);

  if (x < lambda_min)
  {
    return lambda_min;
  }

  return x;
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

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

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

#include <boost/geometry/strategies/cartesian/disjoint_segment_box.hpp>
#include <boost/geometry/strategies/cartesian/envelope.hpp>
#include <boost/geometry/strategies/cartesian/point_in_point.hpp>
#include <boost/geometry/strategies/compare.hpp>
#include <boost/geometry/strategies/side.hpp>

#include <boost/geometry/algorithms/detail/equals/point_point.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a segment a point lies:
    left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_triangle
{
    template <typename Policy>
    struct eps_policy
    {
        eps_policy() {}
        template <typename Type>
        eps_policy(Type const& a, Type const& b, Type const& c, Type const& d)
            : policy(a, b, c, d)
        {}
        Policy policy;
    };

    struct eps_empty
    {
        eps_empty() {}
        template <typename Type>
        eps_empty(Type const&, Type const&, Type const&, Type const&) {}
    };

public :
    typedef strategy::envelope::cartesian<CalculationType> envelope_strategy_type;

    static inline envelope_strategy_type get_envelope_strategy()
    {
        return envelope_strategy_type();
    }

    typedef strategy::disjoint::segment_box disjoint_strategy_type;

    static inline disjoint_strategy_type get_disjoint_strategy()
    {
        return disjoint_strategy_type();
    }

    typedef strategy::within::cartesian_point_point equals_point_point_strategy_type;
    static inline equals_point_point_strategy_type get_equals_point_point_strategy()
    {
        return equals_point_point_strategy_type();
    }

    // Template member function, because it is not always trivial
    // or convenient to explicitly mention the typenames in the
    // strategy-struct itself.

    // Types can be all three different. Therefore it is
    // not implemented (anymore) as "segment"

    template
    <
        typename CoordinateType,
        typename PromotedType,
        typename P1,
        typename P2,
        typename P,
        typename EpsPolicy
    >
    static inline
    PromotedType side_value(P1 const& p1, P2 const& p2, P const& p, EpsPolicy & eps_policy)
    {
        CoordinateType const x = get<0>(p);
        CoordinateType const y = get<1>(p);

        CoordinateType const sx1 = get<0>(p1);
        CoordinateType const sy1 = get<1>(p1);
        CoordinateType const sx2 = get<0>(p2);
        CoordinateType const sy2 = get<1>(p2);

        PromotedType const dx = sx2 - sx1;
        PromotedType const dy = sy2 - sy1;
        PromotedType const dpx = x - sx1;
        PromotedType const dpy = y - sy1;

        eps_policy = EpsPolicy(dx, dy, dpx, dpy);

        return geometry::detail::determinant<PromotedType>
                (
                    dx, dy,
                    dpx, dpy
                );

    }

    template
    <
        typename CoordinateType,
        typename PromotedType,
        typename P1,
        typename P2,
        typename P
    >
    static inline
    PromotedType side_value(P1 const& p1, P2 const& p2, P const& p)
    {
        eps_empty dummy;
        return side_value<CoordinateType, PromotedType>(p1, p2, p, dummy);
    }

    template
    <
        typename CoordinateType,
        typename PromotedType,
        bool AreAllIntegralCoordinates
    >
    struct compute_side_value
    {
        template <typename P1, typename P2, typename P, typename EpsPolicy>
        static inline PromotedType apply(P1 const& p1, P2 const& p2, P const& p, EpsPolicy & epsp)
        {
            return side_value<CoordinateType, PromotedType>(p1, p2, p, epsp);
        }
    };

    template <typename CoordinateType, typename PromotedType>
    struct compute_side_value<CoordinateType, PromotedType, false>
    {
        template <typename P1, typename P2, typename P, typename EpsPolicy>
        static inline PromotedType apply(P1 const& p1, P2 const& p2, P const& p, EpsPolicy & epsp)
        {
            // For robustness purposes, first check if any two points are
            // the same; in this case simply return that the points are
            // collinear
            if (equals_point_point(p1, p2)
                || equals_point_point(p1, p)
                || equals_point_point(p2, p))
            {
                return PromotedType(0);
            }

            // The side_by_triangle strategy computes the signed area of
            // the point triplet (p1, p2, p); as such it is (in theory)
            // invariant under cyclic permutations of its three arguments.
            //
            // In the context of numerical errors that arise in
            // floating-point computations, and in order to make the strategy
            // consistent with respect to cyclic permutations of its three
            // arguments, we cyclically permute them so that the first
            // argument is always the lexicographically smallest point.

            typedef compare::cartesian<compare::less> less;

            if (less::apply(p, p1))
            {
                if (less::apply(p, p2))
                {
                    // p is the lexicographically smallest
                    return side_value<CoordinateType, PromotedType>(p, p1, p2, epsp);
                }
                else
                {
                    // p2 is the lexicographically smallest
                    return side_value<CoordinateType, PromotedType>(p2, p, p1, epsp);
                }
            }

            if (less::apply(p1, p2))
            {
                // p1 is the lexicographically smallest
                return side_value<CoordinateType, PromotedType>(p1, p2, p, epsp);
            }
            else
            {
                // p2 is the lexicographically smallest
                return side_value<CoordinateType, PromotedType>(p2, p, p1, epsp);
            }
        }
    };

    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename coordinate_type<P1>::type coordinate_type1;
        typedef typename coordinate_type<P2>::type coordinate_type2;
        typedef typename coordinate_type<P>::type coordinate_type3;

        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                coordinate_type1, coordinate_type2
                            >::type,
                        coordinate_type3
                    >::type,
                CalculationType
            >::type coordinate_type;

        // Promote float->double, small int->int
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        bool const are_all_integral_coordinates =
            boost::is_integral<coordinate_type1>::value
            && boost::is_integral<coordinate_type2>::value
            && boost::is_integral<coordinate_type3>::value;

        eps_policy< math::detail::equals_factor_policy<promoted_type> > epsp;
        promoted_type s = compute_side_value
            <
                coordinate_type, promoted_type, are_all_integral_coordinates
            >::apply(p1, p2, p, epsp);

        promoted_type const zero = promoted_type();
        return math::detail::equals_by_policy(s, zero, epsp.policy) ? 0
            : s > zero ? 1
            : -1;
    }

private:
    template <typename P1, typename P2>
    static inline bool equals_point_point(P1 const& p1, P2 const& p2)
    {
        typedef equals_point_point_strategy_type strategy_t;
        return geometry::detail::equals::equals_point_point(p1, p2, strategy_t());
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct default_strategy<cartesian_tag, CalculationType>
{
    typedef side_by_triangle<CalculationType> type;
};

}
#endif

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "BHE_2U.h"

#include <boost/math/constants/constants.hpp>
#include "FlowAndTemperatureControl.h"
#include "Physics.h"
#include "ThermoMechanicalFlowProperties.h"

namespace ProcessLib
{
namespace HeatTransportBHE
{
namespace BHE
{
BHE_2U::BHE_2U(BoreholeGeometry const& borehole,
               RefrigerantProperties const& refrigerant,
               GroutParameters const& grout,
               FlowAndTemperatureControl const& flowAndTemperatureControl,
               PipeConfigurationUType const& pipes,
               bool const use_python_bcs)
    : BHECommonUType{borehole, refrigerant,   grout, flowAndTemperatureControl,
                     pipes,    use_python_bcs}
{
    _thermal_resistances.fill(std::numeric_limits<double>::quiet_NaN());

    // Initialize thermal resistances.
    auto values = visit(
        [&](auto const& control) {
            return control(refrigerant.reference_temperature,
                           0. /* initial time */);
        },
        flowAndTemperatureControl);
    updateHeatTransferCoefficients(values.flow_rate);
}

std::array<double, BHE_2U::number_of_unknowns> BHE_2U::pipeHeatCapacities()
    const
{
    double const rho_r = refrigerant.density;
    double const specific_heat_capacity = refrigerant.specific_heat_capacity;
    double const rho_g = grout.rho_g;
    double const porosity_g = grout.porosity_g;
    double const heat_cap_g = grout.heat_cap_g;

    return {{/*i1*/ rho_r * specific_heat_capacity,
             /*i2*/ rho_r * specific_heat_capacity,
             /*o1*/ rho_r * specific_heat_capacity,
             /*o2*/ rho_r * specific_heat_capacity,
             /*g1*/ (1.0 - porosity_g) * rho_g * heat_cap_g,
             /*g2*/ (1.0 - porosity_g) * rho_g * heat_cap_g,
             /*g3*/ (1.0 - porosity_g) * rho_g * heat_cap_g,
             /*g4*/ (1.0 - porosity_g) * rho_g * heat_cap_g}};
}

std::array<double, BHE_2U::number_of_unknowns> BHE_2U::pipeHeatConductions()
    const
{
    double const lambda_r = refrigerant.thermal_conductivity;
    double const rho_r = refrigerant.density;
    double const Cp_r = refrigerant.specific_heat_capacity;
    double const alpha_L = _pipes.longitudinal_dispersion_length;
    double const porosity_g = grout.porosity_g;
    double const lambda_g = grout.lambda_g;

    // Here we calculate the laplace coefficients in the governing
    // equations of BHE. These governing equations can be found in
    // 1) Diersch (2013) FEFLOW book on page 952, M.120-122, or
    // 2) Diersch (2011) Comp & Geosci 37:1122-1135, Eq. 19-22.
    return {{// pipe i1
             (lambda_r + rho_r * Cp_r * alpha_L * _flow_velocity),
             // pipe i2
             (lambda_r + rho_r * Cp_r * alpha_L * _flow_velocity),
             // pipe o1
             (lambda_r + rho_r * Cp_r * alpha_L * _flow_velocity),
             // pipe o2
             (lambda_r + rho_r * Cp_r * alpha_L * _flow_velocity),
             // pipe g1
             (1.0 - porosity_g) * lambda_g,
             // pipe g2
             (1.0 - porosity_g) * lambda_g,
             // pipe g3
             (1.0 - porosity_g) * lambda_g,
             // pipe g4
             (1.0 - porosity_g) * lambda_g}};
}

std::array<Eigen::Vector3d, BHE_2U::number_of_unknowns>
BHE_2U::pipeAdvectionVectors(Eigen::Vector3d const& /*elem_direction*/) const
{
    double const rho_r = refrigerant.density;
    double const Cp_r = refrigerant.specific_heat_capacity;

    return {{// pipe i1
             {0, 0, -rho_r * Cp_r * _flow_velocity},
             // pipe i2
             {0, 0, -rho_r * Cp_r * _flow_velocity},
             // pipe o1
             {0, 0, rho_r * Cp_r * _flow_velocity},
             // pipe o2
             {0, 0, rho_r * Cp_r * _flow_velocity},
             // grout g1
             {0, 0, 0},
             // grout g2
             {0, 0, 0},
             // grout g3
             {0, 0, 0},
             // grout g4
             {0, 0, 0}}};
}

double compute_R_gs_2U(double const chi, double const R_g)
{
    return (1 - chi) * R_g;
}

double compute_R_gg_2U(double const chi, double const R_gs, double const R_ar,
                       double const R_g)
{
    double const R_gg = 2.0 * R_gs * (R_ar - 2.0 * chi * R_g) /
                        (2.0 * R_gs - R_ar + 2.0 * chi * R_g);
    if (!std::isfinite(R_gg))
    {
        OGS_FATAL(
            "Error!!! Grout Thermal Resistance is an infinite number! The "
            "simulation will be stopped!");
    }

    return R_gg;
}

/// Thermal resistances due to grout-soil exchange.
///
/// Check if constraints regarding negative thermal resistances are violated
/// apply correction procedure.
/// Section (1.5.5) in FEFLOW White Papers Vol V.
std::vector<double> thermalResistancesGroutSoil2U(double chi,
                                                  double const R_ar_1,
                                                  double const R_ar_2,
                                                  double const R_g)
{
    double R_gs = compute_R_gs_2U(chi, R_g);
    double R_gg_1 = compute_R_gg_2U(chi, R_gs, R_ar_1, R_g);
    double R_gg_2 = compute_R_gg_2U(chi, R_gs, R_ar_2,
                                    R_g);  // Resulting thermal resistances.

    auto constraint = [&]() {
        return 1.0 / ((1.0 / R_gg_1) + (1.0 / (2.0 * R_gs)));
    };

    std::array<double, 3> const multiplier{chi * 0.66, chi * 0.5 * 0.66, 0.0};
    for (double m_chi : multiplier)
    {
        if (constraint() >= 0)
        {
            break;
        }
        DBUG(
            "Warning! Correction procedure was applied due to negative thermal "
            "resistance! Chi = {:f}.\n",
            m_chi);
        R_gs = compute_R_gs_2U(m_chi, R_g);
        R_gg_1 = compute_R_gg_2U(m_chi, R_gs, R_ar_1, R_g);
        R_gg_2 = compute_R_gg_2U(m_chi, R_gs, R_ar_2, R_g);
    }

    return {R_gg_1, R_gg_2, R_gs};
}

void BHE_2U::updateHeatTransferCoefficients(double const flow_rate)

{
    auto const tm_flow_properties = calculateThermoMechanicalFlowPropertiesPipe(
        _pipes.inlet, borehole_geometry.length, refrigerant, flow_rate);

    _flow_velocity = tm_flow_properties.velocity;
    _thermal_resistances =
        calcThermalResistances(tm_flow_properties.nusselt_number);
}

/// Nu is the Nusselt number.
std::array<double, BHE_2U::number_of_unknowns> BHE_2U::calcThermalResistances(
    double const Nu)
{
    constexpr double pi = boost::math::constants::pi<double>();

    double const lambda_r = refrigerant.thermal_conductivity;
    double const lambda_g = grout.lambda_g;
    double const lambda_p = _pipes.inlet.wall_thermal_conductivity;

    // thermal resistances due to advective flow of refrigerant in the _pipes
    // Eq. 36 in Diersch_2011_CG
    double const R_adv_i = 1.0 / (Nu * lambda_r * pi);
    double const R_adv_o = 1.0 / (Nu * lambda_r * pi);

    // thermal resistance due to thermal conductivity of the pipe wall material
    // Eq. 49
    double const R_con_a =
        std::log(_pipes.inlet.outsideDiameter() / _pipes.inlet.diameter) /
        (2.0 * pi * lambda_p);

    // the average outer diameter of the _pipes
    double const d0 = _pipes.outlet.outsideDiameter();
    double const D = borehole_geometry.diameter;
    // Eq. 38
    double const chi =
        std::log(std::sqrt(D * D + 4 * d0 * d0) / 2 / std::sqrt(2) / d0) /
        std::log(D / 2 / d0);
    // Eq. 39
    // thermal resistances of the grout
    double const R_g =
        std::acosh((D * D + d0 * d0 - 2 * _pipes.distance * _pipes.distance) /
                   (2 * D * d0)) /
        (2 * pi * lambda_g) *
        (3.098 - 4.432 * std::sqrt(2) * _pipes.distance / D +
         2.364 * 2 * _pipes.distance * _pipes.distance / D / D);
    // thermal resistance due to the grout transition.
    double const R_con_b = chi * R_g;

    // Eq. 29 and 30
    double const R_fig = 2 * R_adv_i + 2 * R_con_a + R_con_b;
    double const R_fog = 2 * R_adv_o + 2 * R_con_a + R_con_b;

    // thermal resistance due to inter-grout exchange
    double const R_ar_1 =
        std::acosh((2.0 * _pipes.distance * _pipes.distance - d0 * d0) / d0 /
                   d0) /
        (2.0 * pi * lambda_g);

    double const R_ar_2 =
        std::acosh((2.0 * 2.0 * _pipes.distance * _pipes.distance - d0 * d0) /
                   d0 / d0) /
        (2.0 * pi * lambda_g);

    std::vector<double> _intergrout_thermal_exchange;
    _intergrout_thermal_exchange =
        thermalResistancesGroutSoil2U(chi, R_ar_1, R_ar_2, R_g);

    return {{R_fig, R_fog, _intergrout_thermal_exchange[0],
             _intergrout_thermal_exchange[1], _intergrout_thermal_exchange[2]}};

    // keep the following lines------------------------------------------------
    // when debugging the code, printing the R and phi values are needed--------
    // std::cout << "Rfig =" << R_fig << " Rfog =" << R_fog << " Rgg =" <<
    // R_gg << " Rgs =" << R_gs << "\n"; double phi_fig = 1.0 / (R_fig *
    // S_i); double phi_fog = 1.0 / (R_fog * S_o); double phi_gg = 1.0 / (R_gg
    // * S_g1); double phi_gs = 1.0 / (R_gs * S_gs); std::cout << "phi_fig ="
    // << phi_fig << " phi_fog =" << phi_fog << " phi_gg =" << phi_gg << "
    // phi_gs =" << phi_gs << "\n";
    // -------------------------------------------------------------------------
}

std::array<std::pair<std::size_t /*node_id*/, int /*component*/>, 2>
BHE_2U::getBHEInflowDirichletBCNodesAndComponents(
    std::size_t const top_node_id,
    std::size_t const /*bottom_node_id*/,
    int const in_component_id) const
{
    return {std::make_pair(top_node_id, in_component_id),
            std::make_pair(top_node_id, in_component_id + 2)};
}

std::optional<
    std::array<std::pair<std::size_t /*node_id*/, int /*component*/>, 2>>
BHE_2U::getBHEBottomDirichletBCNodesAndComponents(
    std::size_t const bottom_node_id,
    int const in_component_id,
    int const out_component_id) const
{
    return {{std::make_pair(bottom_node_id, in_component_id),
             std::make_pair(bottom_node_id, out_component_id)}};
}

std::array<double, BHE_2U::number_of_unknowns> BHE_2U::crossSectionAreas() const
{
    return {{
        _pipes.inlet.area(),
        _pipes.inlet.area(),
        _pipes.outlet.area(),
        _pipes.outlet.area(),
        borehole_geometry.area() / 4 - _pipes.inlet.outsideArea(),
        borehole_geometry.area() / 4 - _pipes.inlet.outsideArea(),
        borehole_geometry.area() / 4 - _pipes.outlet.outsideArea(),
        borehole_geometry.area() / 4 - _pipes.outlet.outsideArea(),
    }};
}

double BHE_2U::updateFlowRateAndTemperature(double const T_out,
                                            double const current_time)
{
    auto values =
        visit([&](auto const& control) { return control(T_out, current_time); },
              flowAndTemperatureControl);
    updateHeatTransferCoefficients(values.flow_rate);
    return values.temperature;
}
}  // namespace BHE
}  // namespace HeatTransportBHE
}  // namespace ProcessLib