// 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 "bulletproofs.h"

#undef ARQMA_DEFAULT_LOG_CATEGORY
#define ARQMA_DEFAULT_LOG_CATEGORY "bulletproofs"

//#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(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 rct::key Hi[maxN], Gi[maxN];
static ge_dsmp Gprecomp[64], Hprecomp[64];
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_powers(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 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;
  for (size_t i = 0; i < maxN; ++i)
  {
    Hi[i] = get_exponent(rct::H, i * 2);
    rct::precomp(Hprecomp[i], Hi[i]);
    Gi[i] = get_exponent(rct::H, i * 2 + 1);
    rct::precomp(Gprecomp[i], Gi[i]);
  }
  init_done = true;
}

static bool is_reduced(const rct::key &scalar)
{
  rct::key reduced = scalar;
  sc_reduce32(reduced.bytes);
  return scalar == reduced;
}

/* 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, "Incompatible sizes of a and maxN");
  rct::key res = rct::identity();
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::key term;
    rct::addKeys3(term, a[i], Gprecomp[i], b[i], Hprecomp[i]);
    rct::addKeys(res, res, term);
  }
  return res;
}

/* 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, "Incompatible sizes of a and maxN");
  rct::key res = rct::identity();
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::key term;
#if 0
    // we happen to know where A and B might fall, so don't bother checking the rest
    ge_dsmp *Acache = NULL, *Bcache = NULL;
    ge_dsmp Acache_custom[1], Bcache_custom[1];
    if (Gi[i] == A[i])
      Acache = Gprecomp + i;
    else if (i<32 && Gi[i+32] == A[i])
      Acache = Gprecomp + i + 32;
    else
    {
      rct::precomp(Acache_custom[0], A[i]);
      Acache = Acache_custom;
    }
    if (i == 0 && B[i] == Hi[0])
      Bcache = Hprecomp;
    else
    {
      rct::precomp(Bcache_custom[0], B[i]);
      Bcache = Bcache_custom;
    }
    rct::addKeys3(term, a[i], *Acache, b[i], *Bcache);
#else
    ge_dsmp Acache, Bcache;
    rct::precomp(Bcache, B[i]);
    rct::addKeys3(term, a[i], A[i], b[i], Bcache);
#endif
    rct::addKeys(res, res, term);
  }
  return res;
}

/* Given a scalar, construct a vector of powers */
static rct::keyV vector_powers(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 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;
}

/* 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;
}

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(const rct::key &sv, const rct::key &gamma)
{
  CHECK_AND_ASSERT_THROW_MES(is_reduced(sv), "Invalid sv input");
  CHECK_AND_ASSERT_THROW_MES(is_reduced(gamma), "Invalid gamma input");

  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 = inner_product(oneN, 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_scalar(oneN, z);
  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(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(sv, gamma);
}

/* Given a range proof, determine if it is valid */
bool bulletproof_VERIFY(const Bulletproof &proof)
{
  init_exponents();

  CHECK_AND_ASSERT_MES(proof.V.size() == 1, false, "V does not have exactly 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");
  CHECK_AND_ASSERT_MES(proof.L.size() == 6, false, "Proof is not for 64 bits");

  for (const rct::key &k: proof.V)
    CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(k), false, "Input point not in subgroup");
  for (const rct::key &k: proof.L)
    CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(k), false, "Input point not in subgroup");
  for (const rct::key &k: proof.R)
    CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(k), false, "Input point not in subgroup");

  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.A), false, "Input point not in subgroup");
  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.S), false, "Input point not in subgroup");
  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.T1), false, "Input point not in subgroup");
  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.T2), false, "Input point not in subgroup");

  // check scalar range
  CHECK_AND_ASSERT_MES(is_reduced(proof.taux), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.mu), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.a), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.b), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.t), false, "Input scalar not in range");

  const size_t logN = proof.L.size();
  const size_t N = 1 << logN;

  // Reconstruct the challenges
  PERF_TIMER_START_BP(VERIFY);
  PERF_TIMER_START_BP(VERIFY_start);
  rct::key hash_cache = rct::hash_to_scalar(proof.V[0]);
  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);
  PERF_TIMER_STOP(VERIFY_start);

  PERF_TIMER_START_BP(VERIFY_line_60);
  // Reconstruct the challenges
  rct::key x_ip = hash_cache_mash(hash_cache, x, proof.taux, proof.mu, proof.t);
  PERF_TIMER_STOP(VERIFY_line_60);

  PERF_TIMER_START_BP(VERIFY_line_61);
  // PAPER LINE 61
  rct::key L61Left = rct::addKeys(rct::scalarmultBase(proof.taux), rct::scalarmultKey(rct::H, proof.t));

  rct::key k = rct::zero();
  const auto yN = vector_powers(y, N);
  rct::key ip1y = inner_product(oneN, yN);
  rct::key zsq;
  sc_mul(zsq.bytes, z.bytes, z.bytes);
  rct::key tmp, tmp2;
  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);
  PERF_TIMER_STOP(VERIFY_line_61);

  PERF_TIMER_START_BP(VERIFY_line_61rl);
  sc_muladd(tmp.bytes, z.bytes, ip1y.bytes, k.bytes);
  rct::key L61Right = rct::scalarmultKey(rct::H, tmp);

  CHECK_AND_ASSERT_MES(proof.V.size() == 1, false, "proof.V does not have exactly one element");
  tmp = rct::scalarmultKey(proof.V[0], zsq);
  rct::addKeys(L61Right, L61Right, tmp);

  tmp = rct::scalarmultKey(proof.T1, x);
  rct::addKeys(L61Right, L61Right, tmp);

  rct::key xsq;
  sc_mul(xsq.bytes, x.bytes, x.bytes);
  tmp = rct::scalarmultKey(proof.T2, xsq);
  rct::addKeys(L61Right, L61Right, tmp);
  PERF_TIMER_STOP(VERIFY_line_61rl);

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

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

  // Compute the number of rounds for the inner product
  const size_t rounds = proof.L.size();
  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 inner_prod = rct::identity();
  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 < N; ++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);
    sc_mul(tmp.bytes, zsq.bytes, twoN[i].bytes);
    sc_muladd(tmp.bytes, z.bytes, ypow.bytes, tmp.bytes);
    sc_mulsub(h_scalar.bytes, tmp.bytes, yinvpow.bytes, h_scalar.bytes);

    // Now compute the basepoint's scalar multiplication
    // Each of these could be written as a multiexp operation instead
    rct::addKeys3(tmp, g_scalar, Gprecomp[i], h_scalar, Hprecomp[i]);
    rct::addKeys(inner_prod, inner_prod, tmp);

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

  PERF_TIMER_START_BP(VERIFY_line_26);
  // PAPER LINE 26
  rct::key pprime;
  sc_sub(tmp.bytes, rct::zero().bytes, proof.mu.bytes);
  rct::addKeys(pprime, P, rct::scalarmultBase(tmp));

  for (size_t i = 0; i < rounds; ++i)
  {
    sc_mul(tmp.bytes, w[i].bytes, w[i].bytes);
    sc_mul(tmp2.bytes, winv[i].bytes, winv[i].bytes);
#if 1
    ge_dsmp cacheL, cacheR;
    rct::precomp(cacheL, proof.L[i]);
    rct::precomp(cacheR, proof.R[i]);
    rct::addKeys3(tmp, tmp, cacheL, tmp2, cacheR);
    rct::addKeys(pprime, pprime, tmp);
#else
    rct::addKeys(pprime, pprime, rct::scalarmultKey(proof.L[i], tmp));
    rct::addKeys(pprime, pprime, rct::scalarmultKey(proof.R[i], tmp2));
#endif
  }
  sc_mul(tmp.bytes, proof.t.bytes, x_ip.bytes);
  rct::addKeys(pprime, pprime, rct::scalarmultKey(rct::H, tmp));
  PERF_TIMER_STOP(VERIFY_line_26);

  PERF_TIMER_START_BP(VERIFY_step2_check);
  sc_mul(tmp.bytes, proof.a.bytes, proof.b.bytes);
  sc_mul(tmp.bytes, tmp.bytes, x_ip.bytes);
  tmp = rct::scalarmultKey(rct::H, tmp);
  rct::addKeys(tmp, tmp, inner_prod);
  PERF_TIMER_STOP(VERIFY_step2_check);
  if (!(pprime == tmp))
  {
    MERROR("Verification failure at step 2");
    return false;
  }

  PERF_TIMER_STOP(VERIFY);
  return true;
}

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_PROD_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_prod.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/two_split.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,A0 const& b,
                                  A0 & r0,A0 & r1) const
    {
      r0  = a*b;
      if (is_invalid(r0))
      {
        r1 = Zero<A0>();
      }
      else
      {
        A0 a1, a2, b1, b2;
        two_split(a, a1, a2);
        two_split(b, b1, b2);
        r1 = a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2);
      }
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,A0 & a3) const
    {
      A0 a2;
      two_prod(a0, a1, a2, a3);
      return a2;
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                           (A0),
                           ((scalar_<floating_<A0> >))
                           ((scalar_<floating_<A0> >))
                          )
  {
    typedef typename boost::fusion::tuple<A0,A0> result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      result_type res;
      two_prod(a0,a1, boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }

#endif


#pragma once
#include <Eigen/Core>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>

namespace gtsam
{
  class LidarEdgeFactor2 : public NoiseModelFactor2<Pose3, Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor2<Pose3, Pose3>;
    using This = LidarEdgeFactor2;

  public:
    LidarEdgeFactor2(Key key1, Key key2, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key1, key2), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarEdgeFactor2() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    Vector evaluateError(const X &pose1, const X &pose2,
                         boost::optional<Matrix &> H1 = boost::none,
                         boost::optional<Matrix &> H2 = boost::none) const
    {
      const auto &rotation1 = pose1.rotation().matrix();
      const auto &rotation2 = pose2.rotation().matrix();
      const auto p12w = pose2.transformFrom(p2_) - pose1.transformFrom(p1_);
      const auto uw = rotation1 * u_;
      if (H1)
        *H1 = skewSymmetric(p12w[0], p12w[1], p12w[2]) * rotation1 * (Matrix36() << skewSymmetric(-u_[0], -u_[1], -u_[2]), Z_3x3).finished()
            + skewSymmetric(uw[0], uw[1], uw[2]) * rotation1 * (Matrix36() << skewSymmetric(-p1_[0], -p1_[1], -p1_[2]), I_3x3).finished();
      if (H2)
        *H2 = skewSymmetric(-uw[0], -uw[1], -uw[2]) * rotation2 * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return p12w.cross(uw);
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeFactor2 on (" << keyFormatter(key1())
           << ", " << keyFormatter(key2()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeFactor2

  class LidarEdgeFactor1 : public NoiseModelFactor1<Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor1<Pose3>;
    using This = LidarEdgeFactor1;

  public:
    LidarEdgeFactor1(Key key, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarEdgeFactor1() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    Vector evaluateError(const X &pose,
                         boost::optional<Matrix &> H = boost::none) const
    {
      const auto &rotation = pose.rotation();
      if (H)
        *H = skewSymmetric(-u_[0], -u_[1], -u_[2]) * rotation.matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return (pose.transformFrom(p2_) - p1_).cross(u_);
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeFactor1 on (" << keyFormatter(key()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeFactor1

  class LidarEdgeProjectedFactor1 : public NoiseModelFactor1<Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor1<Pose3>;
    using This = LidarEdgeProjectedFactor1;

  public:
    LidarEdgeProjectedFactor1(Key key, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key), p1_(point1), p2_(point2), u_(unit.normalized())
    {
      // std::cout << "unit:" << u_.transpose()<<"\n";
    }

    virtual ~LidarEdgeProjectedFactor1() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    /*
    d(T^-1*p)/dT = [[T^-1*p]x -I3X3]
    d(R^-1*u)/dT = [[R^T*u]x   03x3]

    line_err = (p2 - p1).cross(u)
             = (p2 - T2^-1*p1).cross(R2^T*u1)

    point_err = (p2 - T2^-1*p1)[0,1] * R2^T*u1[2] = (p2 - c2)[0,1] * c1[2]

    dline_err/dT2 = [R2^-1*u1]x*d(T2^-1*p1)/dT2 + d(R2^-1*u1)/dT2
                  = [R2^T*u1]x*[[T2^-1*p1]x -I3X3] + [[R2^T*u1]x   03x3]

    c1 = R2^T*u1
    c2 = T2^-1*p1
    dpoint_err/dT2 = -dc2/dT2[0,1] * c1[2] + (p2 - c2)[0,1] * dc1/dT2[2]
                   = -[[c2]x -I3X3][0,1] * c1[2] +  (p2 - c2)[0,1]  *  [[c1]x  03x3][2]
    dc1/dT2 = [[R2^T*u1]x   03x3]
    dc2/dT2 = [[T2^-1*p1]x -I3X3]

   */

    Vector evaluateError(const X &pose,
                         boost::optional<Matrix &> H = boost::none) const
    {
      // const auto &rotation = pose.rotation();
      // if (H)
      // {
      //   *H = skewSymmetric(-u_[0], -u_[1], -u_[2]) * rotation.matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      // H->col(0).setZero();
      // H->col(1).setZero();
      // H->col(5).setZero();
      // }
      // return (pose.transformFrom(p2_) - p1_).cross(u_);

      const auto c1 = pose.rotation().inverse() * u_;
      const auto c2 = pose.inverse().transformFrom(p1_);

      if (H)
      {
        // *H = skewSymmetric(c1[0], c1[1], c1[2]) * (Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished()
            // + (Matrix36() << skewSymmetric(c1[0], c1[1], c1[2]), Z_3x3).finished();
        //  *H = (Matrix13() << -c1[1], +c1[0], 0.0).finished() * (Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished() + (Matrix16() << -c1[1], +c1[0], 0.0, 0.0, 0.0, 0.0).finished();
        *H = (Matrix36() << (Matrix13() << -c1[1], +c1[0], 0.0).finished() * (Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished() + (Matrix16() << -c1[1], +c1[0], 0.0, 0.0, 0.0, 0.0).finished(),
             -(Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished().topRows(2) * c1[2] +  (p2_ - c2).topRows(2)  *  (Matrix16() << -c1[1], +c1[0], 0.0, 0.0, 0.0, 0.0).finished()).finished();
      }

      // return Vector1((p2_[0] - c2[0]) * c1[1] - (p2_[1] - c2[1]) * c1[0]);
      return (Vector3() << (p2_[0] - c2[0]) * c1[1] - (p2_[1] - c2[1]) * c1[0], (p2_ - c2).topRows(2) * c1[2]).finished();
      // return (p2_ - c2).cross(c1);
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeProjectedFactor1 on (" << keyFormatter(key()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeProjectedFactor1

class LidarEdgeProjectedFactor2 : public NoiseModelFactor2<Pose3, Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor2<Pose3, Pose3>;
    using This = LidarEdgeProjectedFactor2;

  public:
    LidarEdgeProjectedFactor2(Key key1, Key key2, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key1, key2), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarEdgeProjectedFactor2() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    /*
    d(T^-1*p)/dT = [[T^-1*p]x -I3X3]
    d(R^-1*u)/dT = [[R^T*u]x   03x3]

    line_err = (T1^-1*T2*p2 - p1).cross(u1)

    point_err = (T1^-1*T2*p2 - p1)[0,1] * u1[2]

    dline_err/dT1 = -[u1]x*d(T1^-1*T2*p2)/dT1
                  = -[u1]x*[[T1^-1*T2*p2]x -I3X3]
    dline_err/dT2 = -[u1]x*d(T1^-1*T2*p2)/dT2
                  = -[u1]x*T1^-1*R2*[[-p2]x I3x3]
                  = -[u1]x*R(T1^-1)*R2*[[-p2]x I3x3]
                  = -[u1]x*R1^-1*R2*[[-p2]x I3x3]
    dpoint_err/dT1 = u1[2]*d(T1^-1*T2*p2)/dT1[0,1]
                   = u1[2]*[[T1^-1*T2*p2]x -I3X3][0,1]
    dpoint_err/dT2 = u1[2]*d(T1^-1*T2*p2)/dT2[0,1]
                   = u1[2]*T1^-1*R2*[[-p2]x I3x3][0,1]
                   = u1[2]*R(T1^-1)*R2*[[-p2]x I3x3][0,1]
                   = u1[2]*R1^-1*R2*[[-p2]x I3x3][0,1]
   */

    Vector evaluateError(const X &pose1, const X &pose2,
                         boost::optional<Matrix &> H1 = boost::none,
                         boost::optional<Matrix &> H2 = boost::none) const
    {
      // const auto ux = skewSymmetric(u_[0], u_[1], u_[2]);
      const auto p2_transformed = (pose1.inverse()*pose2).transformFrom(p2_);

      if (H1)
      {
        // *H1 = -ux * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished();
        // *H1 = -(Matrix13() << -u_[1], u_[0], 0.0).finished() * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished();
        *H1 = (Matrix36() << -(Matrix13() << -u_[1], u_[0], 0.0).finished() * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished(),
                             u_[2] * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished().topRows(2)).finished();
          //  *H1 = u_[2] * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished().topRows(2);

      }
      if (H2)
      {
        // *H2 = -ux * pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
        // *H2 = -(Matrix13() << -u_[1], u_[0], 0.0).finished() * pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
        *H2 = (Matrix36() << -(Matrix13() << -u_[1], u_[0], 0.0).finished() * pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished(),
              u_[2] * (pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished()).topRows(2)).finished();
        // *H2 = u_[2] * (pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished()).topRows(2);

      }

      // return (p2_transformed - p1_).cross(u_);
      // return Vector1((p2_transformed[0] - p1_[0]) * u_[1] - (p2_transformed[1] - p1_[1]) * u_[0]);
      return (Vector3() << (p2_transformed[0] - p1_[0]) * u_[1] - (p2_transformed[1] - p1_[1]) * u_[0],
                           (p2_transformed - p1_).topRows(2)  * u_(2)).finished();
      // return Vector2((p2_transformed - p1_).topRows(2)  * u_(2));

    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeProjectedFactor2 on (" << keyFormatter(key1())
           << ", " << keyFormatter(key2()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeProjectedFactor2

} // namespace gtsam


#ifndef OpticalElementInterface_hpp
#define OpticalElementInterface_hpp

/** @file OpticalElementInterface.hpp
 * @brief Definition of the optical element interface.
 * @author C.D. Clark III
 * @date 06/29/16
 */

#include <memory>

#include <Eigen/Dense>

#include "../Units.hpp"

/** @class OpticalElementInterface
 * @brief Abstract class that defines the interface an optical element must
 * implement.
 * @author C.D. Clark III
 *
 * An optical element tranforms a Gaussian beam. Before a beam enters the
 * element, it has a beam waist size and position. After it exists the element,
 * it has a new waist size and position. The theory of Gaussian beam propagation
 * based on the paraxial wave equation accounts for the action of an optical
 * element with Ray Transfer Matircies (RTM)
 * (https://en.wikipedia.org/wiki/Ray_transfer_matrix_analysis). These are the
 * same matricies that are used to transform optical rays when ray tracing
 * through an optical system and they are convienent because the action of
 * multiple elements can be accounted for my simply multiplying their RTM
 * together.
 *
 * This method is often referred to as the "ABCD" law for Gaussian beams.
 * Basically, the RTM is a \f$2\times2\f$ matrix that can be written as \f[
 * \left(
 * \begin{matrix}
 * A & B \\
 * C & D
 * \end{matrix}
 * \right)
 * \f]
 * The action of the element is then given by its affect on the complex beam
 * parameter (https://en.wikipedia.org/wiki/Complex_beam_parameter) \f[ q_f =
 * \frac{Aq_i + B}{Cq_i + D} \f]
 *
 * Where $q_i$ and $q_f$ are the complex beam parameters of the beam before and
 * after it passes through the lens. The complex beam parameter itself is
 * defined as \f$q = z + iz_R\f$ where \f$z\f$ is the distance from the beam
 * waist and \f$z_R\f$ is the Rayleigh range (\f$z_R = \frac{\pi
 * \omega_0^2}{\lambda}\f$, where \f$\omega_0\f$ is the beam waist radius
 * (\f$1/e^2\f$) and \f$\lambda\f$ is the wavelength.).
 *
 * One important point to make here is that the usual treatment of this method
 * assumes that the beam waist is always located at \f$z = 0\f$. In other words,
 * the origin of the coordinate system is always taken to be at the beam waist
 * which means that when the beam waist position changes (after it goes through
 * an optical element), the coordinate system changes.
 *
 * This library performs calculations in a fixed coordinate system. The beam
 * waist position is explicitly tracked, and when the beam passes through an
 * optical element, the beam waist position is updated. This introduces a few
 * subtle details that must be handled correctly. Some of these only affect the
 * GaussianBeam class. However, a few also affect optical elements.
 *
 * The optical element class is intended to encapsulate all of the information
 * necessary for a GaussianBeam instance to transform itself. In order to do
 * that, the beam will need the RTM representing the element. No surprise there.
 * However, it is possible for an element to separate two different media (more
 * specifically, two materials with different refractive indices. When this is
 * the case, the wavelength of the beam will change (since \f$\lambda =
 * \frac{\lambda_0}{n}\f$) and this change must be known in order for the beam
 * to update the beam waist size since the imaginary portion of the complex beam
 * parameter contains both the wavelength and beam waist radius. So, an optical
 * element must provide a method that the GaussianBeam instance can use to
 * determine the new wavelength.
 *
 * The second detail that arises from the uses of a fixed coordinate system is
 * the possibility that the z position of the complex beam parameter may shift
 * when it passes through the element. In other words, the z position of the
 * initial parameter may not coincide with the position of the final parameter.
 * When the coordinate system is always centered on the beam waist, this does
 * not matter. The real part of the complex beam parameter gives the distance to
 * the beam waist. However, for a fixed coordinate system, this possible shift
 * needs to be accounted for. The best example of this is a thick lens. The RTM
 * for a thick lens includes the refraction across the front surface,
 * propagation through the lens, and refraction across the back surface. When
 * this matrix is used to transform the complex beam parameter, the initial
 * parameter is evaluated at the front surface of the lens and the new parameter
 * actually corresponds to the position of the back surface of the lens.
 *
 * This shift needs to be known in order for the beam waist position to be
 * determined, so an optical element must provide a method to that the
 * GaussianBeam instance can use to determine the new beam waist position.
 *
 * @tparam LengthUnitType the length unit that will be used for calculating
 * elements of the RTM and position shifts.
 */

template<typename LengthUnitType>
class OpticalElementInterface
{
 public:
  virtual Eigen::Matrix<double, 2, 2> getRTMatrix()
      const = 0;  ///< return the Ray Transfer matrix for the element. NOTE:
                  ///< elements MUST be returned in units of LengthUnitType or
                  ///< inverse LengthUnitType.
  virtual boost::units::quantity<LengthUnitType> getPositionShift()
      const = 0;  ///< return the difference in the position (z coordinate) that
                  ///< the complex beam parameter corresponds to after it passes
                  ///< through the element.
  virtual double getPowerLoss()
      const = 0;  ///< return the power loss (fraction) through the element
  virtual double getWavelengthScaleFactor()
      const = 0;  ///< return the scaling factor through the element. i.e. if
                  ///< the wavelenght changes when light passes through the
                  ///< element.
};

template<typename T, typename U>
class OpticalElementAdapter : public OpticalElementInterface<U>
{
 private:
  T &t;

 public:
  OpticalElementAdapter(T &ref) : t(ref) {}
  Eigen::Matrix<double, 2, 2> getRTMatrix() const { return t.getRTMatrix(); }
  boost::units::quantity<U> getPositionShift() const { return t.getPositionShift(); }
  double      getPowerLoss() const { return t.getPowerLoss(); }
  double      getWavelengthScaleFactor() const
  {
    return t.getWavelengthScaleFactor();
  }
};

template<typename T>
using OpticalElement_ptr = std::shared_ptr<OpticalElementInterface<T> >;

#endif  // include protector


//			Copyright Gavin Band 2008 - 2012.
// Distributed under 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 INTEGRATION_MODIFIED_CHOLESKY_HPP
#define INTEGRATION_MODIFIED_CHOLESKY_HPP

#include <iostream>
#include <boost/noncopyable.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>

// #define DEBUG_MODIFIED_CHOLESKY 1

namespace metro {
	// Utility functions
	namespace {
		template< typename Real >
		Real max( Real const a, Real const b ) {
			return std::max( a, b ) ;
		}

		template< typename Real >
		Real max( Real const a, Real const b, Real const c ) {
			return std::max( a, std::max( b, c )) ;
		}
	}

	//
	// Implements the 'MC' algorithm from Gill & Murray, Practical Optimisation.
	// Also expressed as Algorithm 6.5, "Modified Cholesky algorithm" in
	// Nocedal & Wright, "Numerical Optimisation".
	// See also Fang & Leary 2006
	//
	// I have borrowed a little bit from Eigen's LDLT.h here to get the permutations and types right.
	//
	// This algorithm only touches the lower-diagonal of the matrix.
	// As described in the above references, we use the lower-diagonal of the matrix
	// to store the non-unity entries of L, and the diagonal to store the entries of D.
	// The auxiliary variables c_ij are stored in the lower diagonal too until they
	// are overwritten by entries of L and D.
	//
	// At step j the matrix looks like:
	//
	//	  0 . . j . .
	// 0  d
	// .  l d
	// .  l l d
	// j  c c c c
	// .  c c c a c
	// .  c c c a a c
	//
	// Where a refers to an entry of the original matrix (after the possible permutations) and l refers to an entry of
	// the computed matrix L, d refers to an entry of D, and c to one of the auxiliary c_ijs.
	//
	// At the jth step we update this to become
	// 1: d
	// .  l d
	// .  l l d
	// j: l l l d
	// .  c c c c c
	// .  c c c c a c
	//
	// i.e. we compute the jth row of L, the jth entry of D, and the c's in
	// the jth column and on the diagonal below j.
	template< typename Matrix >
	struct ModifiedCholesky {
	public:
		enum {
		  RowsAtCompileTime = Matrix::RowsAtCompileTime,
		  ColsAtCompileTime = Matrix::ColsAtCompileTime,
		  Options = Matrix::Options & ~Eigen::RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here!
		  MaxRowsAtCompileTime = Matrix::MaxRowsAtCompileTime,
		  MaxColsAtCompileTime = Matrix::MaxColsAtCompileTime,
		  UpLo = Eigen::Lower
		} ;
		typedef typename Matrix::Scalar Scalar;
		typedef typename Eigen::NumTraits<typename Matrix::Scalar>::Real RealScalar;
		typedef typename Matrix::Index Index;

		typedef Eigen::Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> Transpositions;
		typedef Eigen::PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> Permutations;
		typedef Eigen::TriangularView< Matrix const, Eigen::UnitLower > const MatrixL ;
		typedef Eigen::Diagonal< Matrix const > Diagonal ;

	public:
		ModifiedCholesky() {}

		ModifiedCholesky( ModifiedCholesky const& other ):
			m_matrix( other.m_matrix ),
			m_transpositions( other.m_transpositions )
		{}

		ModifiedCholesky& operator=( ModifiedCholesky const& other ) {
			m_matrix = other.m_matrix ;
			m_transpositions = other.m_transpositions ;
			return *this ;
		}

		ModifiedCholesky& compute( Matrix const& matrix ) {
			m_matrix = matrix ;
			compute_inplace( m_matrix ) ;
			return *this ;
		}

		MatrixL matrixL() const {
			return m_matrix.template triangularView< Eigen::UnitLower >() ;
		}

		Diagonal vectorD() const {
			return m_matrix.diagonal();
		}

		Permutations const matrixP() const {
			return Permutations( m_transpositions ) ;
		}

		Matrix solve( Matrix const& rhs ) const
		{
			Matrix result = rhs ;
			assert( result.rows() == m_matrix.rows() ) ;
			// result = P rhs
			result = m_transpositions * result ;
			// result = L^-1 (P rhs)
			matrixL().solveInPlace( result );
			// result = D^-1 (L^-1 P rhs)
			for( Index i = 0; i < m_matrix.rows(); ++i ) {
				result.row(i) /= m_matrix(i,i) ;
			}
			// result = L^-T (D^-1 L^-1 P rhs)
			matrixL().transpose().solveInPlace( result ) ;
			// result = P^-1 L^-T (D^-1 L^-1 P rhs)
			result = m_transpositions.transpose() * result ;
			return result ;
		}

		// "Half" solve the rhs
		// solve the rhs, i.e compute
		// S^-1 (rhs)
		// Where S is the 'square root' of the matrix, given by
		// S = P^-1 L sqrt(D)
		// such that
		// S S^t = P^-1
		Matrix halfSolve( Matrix const& rhs ) const
		{
			Matrix result = rhs ;
			assert( result.rows() == m_matrix.rows() ) ;
			// result = P rhs
			result = m_transpositions * result ;
			// result = L^-1 P rhs
			matrixL().solveInPlace( result );
			// result = sqrt(D^-1) L^-1 P rhs
			for( Index i = 0; i < m_matrix.rows(); ++i ) {
				result.row(i) /= std::sqrt( m_matrix(i,i) ) ;
			}
			return result ;
		}

	private:
		Matrix m_matrix ;
		Transpositions m_transpositions;

	private:

		void compute_inplace( Matrix& matrix ) {
			assert( matrix.rows() == matrix.cols() ) ;
			m_transpositions.resize( matrix.rows() ) ;
			Index const size = matrix.rows() ;

#if DEBUG_MODIFIED_CHOLESKY
			std::cerr << "ModifiedCholesky::compute_inplace(): computing with matrix:\n" << matrix << ".\n" ;
#endif

			Scalar biggestOnDiagonal ;
			RealScalar betaSquared ;
			RealScalar delta ;

			for( Index j = 0; j < size; ++j ) {
				// Find largest diagonal element
				Index indexOfBiggestOnDiagonal ;
				biggestOnDiagonal = matrix.diagonal().tail( size - j ).cwiseAbs().maxCoeff( &indexOfBiggestOnDiagonal ) ;
				indexOfBiggestOnDiagonal += j ;

				// Initialise beta and delta if we are starting.
				if( j == 0 ) {
					Scalar biggestOffDiagonal = 0.0 ;
					for( Index i = 0; i < size; ++i ) {
						for( Index j = i+1; j < size; ++j ) {
							biggestOffDiagonal = std::max( biggestOffDiagonal, std::abs( matrix( i, j )) ) ;
						}
					}
					// Had this before:
					//delta = std::numeric_limits< Scalar >::epsilon() * max( biggestOnDiagonal + biggestOffDiagonal, Scalar( 1 ) ) ;
					// ..but most authors e.g. (Fang & Leary 2006) say:
					delta = std::numeric_limits< Scalar >::epsilon() ;
					// Fang & Leary (2006), formula (5).
					betaSquared = max(
						std::numeric_limits< Scalar >::epsilon(),
						biggestOnDiagonal,
						biggestOffDiagonal / std::sqrt( ( size * size ) - 1 )
					) ;

#if DEBUG_MODIFIED_CHOLESKY
					std::cerr << "ModifiedCholesky::compute_inplace(): initialised with delta = "
						<< delta << ", beta^2 = " << betaSquared << ".\n" ;
#endif

					// Modified cholesky of A is cholesky of A+E for some diagonal matrix E
					// Bound on elements of E can be computed as in Fang & Leary (2006)
					//bound = (
					//	std::pow((biggestOffDiagonal/std::sqrt(betaSquared)) + (size-1)*std::sqrt(betaSquared), 2 )
					//	+ 2 * ( biggestOnDiagonal + (size-1)*betaSquared )
					//	+ std::numeric_limits< Scalar >::epsilon()
					//) ;
				}

#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): at iteration "
					<< j
						<< ": largest entry on diagonal = " << biggestOnDiagonal
					<< " at index " << indexOfBiggestOnDiagonal << ".\n" ;
#endif

				// Swap rows and columns corresponding to the jth and largest diagonal element.
				m_transpositions.coeffRef( j ) = indexOfBiggestOnDiagonal ;
				if( j != indexOfBiggestOnDiagonal ) {
					// indexOfbiggestOnDiagonal is always >= j by construction
					// we only touch the lower triangular part of the matrix.
					Index const tailSize = size - indexOfBiggestOnDiagonal - 1 ;
					matrix.row( j ).head( j ).swap( matrix.row( indexOfBiggestOnDiagonal ).head( j ) ) ;
					matrix.col( j ).tail( tailSize ).swap( matrix.col( indexOfBiggestOnDiagonal ).tail( tailSize ) ) ;
					std::swap( matrix.coeffRef(j,j), matrix.coeffRef( indexOfBiggestOnDiagonal, indexOfBiggestOnDiagonal ) );
					for( int i = j+1; i < indexOfBiggestOnDiagonal; ++i ) {
						std::swap( matrix.coeffRef( i, j ), matrix.coeffRef( indexOfBiggestOnDiagonal, i ) ) ;
					}
				}

				Index const tailSize = size - j - 1 ;
				// We first compute the jth row of L to produce:
				// 1: d
				// .  l d
				// .  l l d
				// j: l l l c
				// .  c c c a c
				// .  c c c a a c
				//
				// by formula: l_js = c_js / d_s for s = 0,...,j-1.
				if( j > 0 ) {
					matrix.row(j).head( j ).array() /= matrix.diagonal().head( j ).array() ;
				}
#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): after computing ls, matrix =\n"
					<< matrix << ".\n" ;
#endif
				// We next compute jth column of c_ijs to get:
				// 1: d
				// .  l d
				// .  l l d
				// j: l l l c
				// .  c c c c c
				// .  c c c c a c
				//
				// by formula c_ij = a_ij - sum_s l_ks c_is for s=j+1...n
				for( Index i = j+1; i < size; ++i ) {
					matrix(i,j) -= ( matrix.row( j ).head( j ) * matrix.row( i ).head( j ).transpose() ) ;
				}
				Scalar const theta = ( (j+1) == size ) ? 0.0 : ( matrix.col( j ).tail( tailSize ).maxCoeff() ) ;
#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): after computing cs, matrix =\n"
					<< matrix << ",\n"
					<< "theta = " << theta << ".\n" ;
#endif

				// compute d_jj to produce
				// 1: d
				// .  l d
				// .  l l d
				// j: l l l d
				// .  c c c c c
				// .  c c c c a c
				double new_dj = max(
					delta,
					std::abs( matrix(j,j) ),
					(theta*theta) / betaSquared
				) ;
				matrix(j,j) = new_dj ;
				//
				// Finally update the c_ii's
				matrix.diagonal().tail( tailSize ).array() -= ( matrix.col(j).tail( tailSize ).array().square() ) / matrix(j,j) ;

#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): after iteration " << j << ", matrix is:\n"
					<< matrix << ".\n" ;
#endif
			}
		}
	} ;
}

#endif


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SINH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SINH_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/arch/common/detail/generic/sinh_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sinh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinh is computed using a polynomial(float)
      // respectively rational(double) approx from cephes.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is (e-rec(e))/2
      // * in the second     sinh is (e/2)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      if( x < One<A0>())
      {
       A0 x2 = sqr(x);
       return detail::sinh_kernel<A0>::compute(a0, x2);
      }
      else
      {
        A0 r;
        if (BOOST_UNLIKELY( x > Maxlog<A0>()-Log_2<A0>()))
        {
          A0 tmp = exp(Half<A0>()*x);
          r = (Half<A0>()*tmp)*tmp;
        }
        else
        {
          A0 tmp = exp(x);
          r =  fms(tmp, Half<A0>(), Half<A0>()*rec(tmp));
        }
        return bitwise_xor(r, bitofsign(a0));
      }
    }
  };
  BOOST_DISPATCH_OVERLOAD ( sinh_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
      return std::sinh(a0);
    }
  };
} } }

#endif


#define DEBUG 1
/**
 * File    : C.cpp
 * Author  : Kazune Takahashi
 * Created : 6/16/2020, 4:51:02 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Solve -----

class Solve
{

public:
  Solve()
  {
  }

  void flush()
  {
  }

private:
};

// ----- SegTree -----
// Referring to the following great materials.
//  - tsutaj-san's article: https://tsutaj.hatenablog.com/entry/2017/03/30/224339
//  - drken-san's article: https://drken1215.hatenablog.com/entry/2019/02/19/110200
//  - tsutaj-san's libary: https://tsutaj.github.io/cpp_library/library/structure/strc_021_dynamic_lazy_segtree.cpp.html
// Many thanks to them.

template <typename Monoid, typename Action>
class SegTree
{
  struct SegNode
  {
    bool need_update;
    unique_ptr<SegNode> left, right;
    Monoid value;
    Action lazy_value;

    SegNode() {}
    SegNode(Monoid value, Action lazy_value) : need_update{false}, left{nullptr}, right{nullptr}, value{value}, lazy_value{lazy_value} {}
  };

  using FuncAction = function<void(Monoid &, Action)>;
  using FuncMonoid = function<Monoid(Monoid, Monoid)>;
  using FuncLazy = function<void(Action &, Action)>;
  using FuncIndex = function<Action(Action, int)>;

  // fields
  int N;
  unique_ptr<SegNode> root;
  // unities
  Monoid unity_monoid;
  Action unity_action;
  // functions
  FuncAction func_update;
  FuncMonoid func_combine;
  FuncLazy func_lazy;
  FuncIndex func_accumulate;

public:
  SegTree() {}
  SegTree(
      int n, Monoid unity_monoid, Action unity_action,
      FuncAction func_update,
      FuncMonoid func_combine,
      FuncLazy func_lazy,
      FuncIndex func_accumulate)
      : N{1}, root{make_unique<SegNode>(unity_monoid, unity_action)},
        unity_monoid(unity_monoid), unity_action(unity_action),
        func_update(func_update),
        func_combine(func_combine),
        func_lazy(func_lazy),
        func_accumulate(func_accumulate)
  {
    while (N < n)
    {
      N <<= 1;
    }
  }

  void update(int a, int b, Action const &x) { update(root.get(), a, b, x, 0, N); }
  void update(int a, Action const &x) { update(a, a + 1, x); }
  Monoid query(int a, int b) { return query(root.get(), a, b, 0, N); }
  Monoid query(int a) { return query(a, a + 1); }
  Monoid operator[](size_t i) { return query(static_cast<int>(i)); }

private:
  void node_maker(unique_ptr<SegNode> &pt) const
  {
    if (!pt)
    {
      pt = make_unique<SegNode>(unity_monoid, unity_action);
    }
  }

  void evaluate(SegNode *node, int l, int r)
  {
    if (!node->need_update)
    {
      return;
    }
    func_update(node->value, func_accumulate(node->lazy_value, r - l));
    if (r - l > 1)
    {
      node_maker(node->left);
      func_lazy(node->left->lazy_value, node->lazy_value);
      node->left->need_update = true;
      node_maker(node->right);
      func_lazy(node->right->lazy_value, node->lazy_value);
      node->right->need_update = true;
    }
    node->lazy_value = unity_action;
    node->need_update = false;
  }

  void update(SegNode *node, int a, int b, Action const &x, int l, int r)
  {
    evaluate(node, l, r);
    if (b <= l || r <= a)
    {
      return;
    }
    if (a <= l && r <= b)
    {
      func_lazy(node->lazy_value, x);
      node->need_update = true;
      evaluate(node, l, r);
    }
    else
    {
      auto mid{(l + r) >> 1};
      node_maker(node->left);
      update(node->left.get(), a, b, x, l, mid);
      node_maker(node->right);
      update(node->right.get(), a, b, x, mid, r);
      node->value = func_combine(node->left->value, node->right->value);
    }
  }

  Monoid query(SegNode *node, int a, int b, int l, int r)
  {
    if (b <= l || r <= a)
    {
      return unity_monoid;
    }
    evaluate(node, l, r);
    if (a <= l && r <= b)
    {
      return node->value;
    }
    auto mid{(l + r) >> 1};
    auto vl{(node->left ? query(node->left.get(), a, b, l, mid) : unity_monoid)};
    auto vr{(node->right ? query(node->right.get(), a, b, mid, r) : unity_monoid)};
    return func_combine(vl, vr);
  }
};

// ----- RangePlusQuery -----
//  - update(i, x) : a[i] += x;,
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(i) : return a[i];,
//  - query(s, t) : return the sum a[i] where i runs on [s, t).

// ----- RangeSumQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_B&lang=ja
//  - update(i, x) : a[i] += x;,
//  - query(s, t) : return the sum a[i] where i runs on [s, t).

// ----- RangeAddQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_E&lang=ja
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(i) : return a[i].

// ----- RSU_RAU -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_G&lang=ja
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(s, t) : return the sum a[i] where i runs on [s, t).

template <typename Monoid>
SegTree<Monoid, Monoid> RangePlusQuery(int N, Monoid const &monoid_zero)
{
  using Action = Monoid;
  return SegTree<Monoid, Action>{
      N, monoid_zero, monoid_zero,
      [](Monoid &x, Action y) { x += y; },
      [](Monoid x, Monoid y) { return x + y; },
      [](Action &x, Action y) { return x += y; },
      [](Action x, int y) { return x * static_cast<Action>(y); }};
}

template <typename Monoid>
SegTree<Monoid, Monoid> RangePlusQuery(int N)
{
  return RangePlusQuery<Monoid>(N, 0);
}

// ----- RangeMinQuery -----
//  - update(i, x) : a[i] = x;,
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(i) : return a[i];,
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).

// ----- RangeMinimumQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_A&lang=ja
//  - update(i, x) : a[i] = x;,
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).

// ----- RangeUpdateQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_D&lang=ja
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(i) : return a[i].

// ----- RMQ_RUQ -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_F&lang=ja
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).

template <typename Monoid>
SegTree<Monoid, Monoid> RangeMinQuery(int N, Monoid const &monoid_infty)
{
  using Action = Monoid;
  return SegTree<Monoid, Action>{
      N, monoid_infty, monoid_infty,
      [](Monoid &x, Action y) { x = y; },
      [](Monoid x, Monoid y) { return min(x, y); },
      [](Action &x, Action y) { return x = y; },
      [](Action x, int) { return x; }};
}

template <typename Monoid>
SegTree<Monoid, Monoid> RangeMinQuery(int N)
{
  return RangeMinQuery<Monoid>(N, numeric_limits<Monoid>::max());
}

// ----- RMQ_RAQ -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_H&lang=ja
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).
// update should be called as follows.
// tree.update(s, t, make_tuple(x, true));

template <typename Monoid>
SegTree<Monoid, tuple<Monoid, bool>> RMQ_RAQ(int N, Monoid const &monoid_zero, Monoid const &monoid_infty)
{
  using Action = tuple<Monoid, bool>;
  auto tree{SegTree<Monoid, Action>{
      N, monoid_infty, Action{monoid_zero, true},
      [](Monoid &x, Action y) {
        if (get<1>(y))
        {
          x += get<0>(y);
        }
        else
        {
          x = get<0>(y);
        }
      },
      [](Monoid x, Monoid y) { return min(x, y); },
      [](Action &x, Action y) {
        if (get<1>(y))
        {
          get<0>(x) += get<0>(y);
        }
        else
        {
          x = y;
        }
      },
      [](Action x, int) { return x; }}};
  tree.update(0, N, Action{monoid_zero, false});
  return tree;
}

template <typename Monoid>
SegTree<Monoid, tuple<Monoid, bool>> RMQ_RAQ(int N)
{
  return RMQ_RAQ<Monoid>(N, 0, numeric_limits<Monoid>::max());
}

// ----- RSQ_RUQ -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_I&lang=ja
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(s, t) : return the sum of a[i] where i runs on [s, t).

template <typename Monoid>
SegTree<Monoid, Monoid> RSQ_RUQ(int N, Monoid const &monoid_zero)
{
  using Action = Monoid;
  return SegTree<Monoid, Action>{
      N, monoid_zero, monoid_zero,
      [](Monoid &x, Action y) { x = y; },
      [](Monoid x, Monoid y) { return x + y; },
      [](Action &x, Action y) { return x = y; },
      [](Action x, int y) { return x * static_cast<Action>(y); }};
}

template <typename Monoid>
SegTree<Monoid, Monoid> RSQ_RUQ(int N)
{
  return RSQ_RUQ<Monoid>(N, 0);
}

// ----- main() -----

/*
int main()
{
  Solve solve;
  solve.flush();
}
*/

int main()
{
  int N;
  cin >> N;
  vector<int> A(N);
  for (auto i{0}; i < N; ++i)
  {
    cin >> A[i];
    A[i]--;
  }
  auto tree{RangePlusQuery<int>(N)};
  int ans{0};
  for (auto i{0}; i < N; ++i)
  {
    if (tree.query(0, A[i]) == 0)
    {
      ++ans;
    }
    tree.update(A[i], 1);
  }
  cout << ans << endl;
}