#include <cassert>
#include <NTL/RR.h>
#include "ring.h"

/**********************************************************************/
ostream& operator<< (ostream& s, const ring& rg) {
  s << rg.label << ": type = " << rg.type << ", mR = " << rg.mR << ", pR = " << rg.pR
    << ", gR = " << rg.gR << ", tR = " << rg.tR << ", depth = " << rg.depth << endl;

  return s;
}

ostream& operator<< (ostream& s, const param& prm) {
  s << endl << "<" << prm.label << ">" << endl;
  s << "l = " << prm.l << ", ";
  s << "r = " << prm.r << ", ";
  s << "s = " << prm.s << ", ";
  s << "lw = " << prm.lw << endl;
  s << prm.rg << prm.ns;

  return s;
}

/**********************************************************************/
void ring::to_eta (ZZ_pX& w, vec_ZZ_p& a) {
  if (type != prime) {
    cout << "Error: to_eta" << endl;
    cout << "- not yet implemented for " << type << "-type" << endl;
  }

  for (long i = 0; i < gR; ++i) w += a[i] * conv<ZZ_pX>(eta(i));
}

void ring::to_delta (ZZ_pX& w, vec_ZZ_p& a) {
  if (type != prime) {
    cout << "Error: to_eta" << endl;
    cout << "- not yet implemented for " << type << "-type" << endl;
  }

  for (long i = 0; i < gR; ++i) w += a[i] * delta(i);
}

void mult (vec_ZZ_p& x, const vec_ZZ_p& a, const vec_ZZ_p& b) {
  for (long i = 0; i < a.length(); ++i) x[i] = a[i] * b[i];
}

void sqr (vec_ZZ_p& x, const vec_ZZ_p& a) {
  for (long i = 0; i < a.length(); ++i) x[i] = sqr(a[i]);
}

void add_scalar (vec_ZZ_p& x, const vec_ZZ_p& a, const ZZ_p& b) {
  for (long i = 0; i < a.length(); ++i) x[i] = a[i] + b;
}

void add_scalar (vec_ZZ_p& x, const ZZ_p& a, const vec_ZZ_p& b) {
  for (long i = 0; i < b.length(); ++i) x[i] = a + b[i];
}

void zzx_convolution (vec_ZZ_p& c, const vec_ZZ_p& a, const vec_ZZ_p& b) {
  ZZ_pX aX, bX;
  long g = a.length();

  aX.SetLength(g);
  bX.SetLength(g);
  c.SetLength(g);

  aX[0] = a[0];
  for (long i = 1; i < g; ++i) aX[i] = a[g - i];
  for (long i = 0; i < g; ++i) bX[i] = b[i];

  ZZ_pX w = aX * bX;
  w.SetLength(2*g-1);

  // c = w mod m, m = x^gR - 1
  for (long i = 0; i < g-1; i++) c[i] = w[i] + w[i + g];
  c[g-1] = w[g-1];
}

// Transpose a (m*n)-dim. vector 'a'
// into a m x n matrix (m-dim vector of n-dim vector)'w'
void mat (Vec<vec_ZZ_p>& aa, const vec_ZZ_p& a, const long n, const long m) {
  aa.SetLength(m);
  for (long j = 0; j < m; ++j) aa[j].SetLength(n);

  for (long j = 0; j < m; ++j)
    for (long i = 0; i < n; ++i)
      aa[j][i] = a[i + n*j];
}

// returns m-dim vector of n-dim vectors
void mat (Vec<vec_ZZ>& aa, const vec_ZZ& a, const long n, const long m) {
  aa.SetLength(m);
  for (long j = 0; j < m; ++j) aa[j].SetLength(n);

  for (long j = 0; j < m; ++j)
    for (long i = 0; i < n; ++i)
      aa[j][i] = a[i + n*j];
}

void transpose (Vec<vec_ZZ_p>& b, const Vec<vec_ZZ_p>& a) {
  long m = a.length();
  long n = a[0].length();

  b.SetLength(n);
  for (long j = 0; j < n; ++j) b[j].SetLength(m);

  for (long j = 0; j < n; ++j)
    for (long i = 0; i < m; ++i)
      b[j][i] = a[i][j];
}

void transpose (Vec<vec_ZZ>& b, const Vec<vec_ZZ>& a) {
  long m = a.length();
  long n = a[0].length();

  b.SetLength(n);
  for (long j = 0; j < n; ++j) b[j].SetLength(m);

  for (long i = 0; i < m; ++i)
    for (long j = 0; j < n; ++j)
      b[j][i] = a[i][j];
}

void vec (vec_ZZ_p& b, const Vec<vec_ZZ_p>& a) {
  long m = a.length();
  long n = a[0].length();

  b.SetLength(m*n);

  for (long i = 0; i < m; ++i)
    for (long j = 0; j < n; ++j)
      b[i*n+j] = a[i][j];
}

void transpose_vec (vec_ZZ_p& b, const Vec<vec_ZZ_p>& a) {
  long m = a.length();
  long n = a[0].length();

  b.SetLength(m*n);

  for (long i = 0; i < n; ++i)
    for (long j = 0; j < m; ++j)
      b[i*m+j] = a[j][i];
}

void transpose_vec (vec_ZZ& b, const Vec<vec_ZZ>& a) {
  long m = a.length();
  long n = a[0].length();

  b.SetLength(m*n);

  for (long i = 0; i < n; ++i)
    for (long j = 0; j < m; ++j)
      b[i*m+j] = a[j][i];
}

void diagonal (vec_ZZ& MD, const long k, const vec_ZZ& M) {
  long dim = M.length();
  MD.SetLength(dim);

  for (long i = 0; i < dim; ++i) MD[i] = M[(i+(i+k)) % dim];
}

void diagonal (vec_ZZ_p& MD, const long k, const vec_ZZ_p& M) {
  long dim = M.length();
  MD.SetLength(dim);

  for (long i = 0; i < dim; ++i) MD[i] = M[(i+(i+k)) % dim];
}

void naiveApplyMatrix (vec_ZZ_p& b, const vec_ZZ_p& m, const vec_ZZ_p& a) {
  long dim = m.length();
  vec_ZZ_p S; S.SetLength(dim);

  vec_ZZ_p aa; //aa.SetLength(dim);
  VectorCopy(aa, a, dim);

  for (long i=0; i < dim; ++i) {
    vec_ZZ_p md;
    diagonal(md, i, m);

    vec_ZZ_p w; w.SetLength(dim);
    mult(w, md, aa);

    shift(aa);
    S += w;
  }

  b = S;
}

void applyMatrix (vec_ZZ_p& b, const vec_ZZ_p& m, const vec_ZZ_p& a) {
  zzx_convolution(b, a, m);
  //naiveApplyMatrix(b, m, a);  // slow, for debug only
}

void ring::applyOmega (vec_ZZ_p& b, const vec_ZZ_p& a) {
  if (type == prime) {
    vec_ZZ_p w = conv<vec_ZZ_p>(omega);
    applyMatrix(b, w, a);
  }
  else {  // type == composite
    long gl = left->gR;
    long gr = right->gR;

    Vec<vec_ZZ_p> w;
    mat(w, a, gr, gl);
    for (long i = 0; i < gl; ++i) right->applyOmega(w[i], w[i]);

    Vec<vec_ZZ_p> wt;
    transpose(wt, w);
    for (long j = 0; j < gr; ++j) left->applyOmega(wt[j], wt[j]);

    transpose_vec(b, wt);
  }
}

void ring::applyOmega (vec_ZZ& b, const vec_ZZ& a) {
  vec_ZZ_p _b;
  applyOmega(_b, conv<vec_ZZ_p>(a));
  b = conv<vec_ZZ>(_b);
}

void ring::applyOmegaInv (vec_ZZ_p& b, const vec_ZZ_p& a) {
  if (type == prime) {
    vec_ZZ_p w = conv<vec_ZZ_p>(omega_inv);
    applyMatrix(b, w, a);
  }
  else {  // type == composite
    long gl = left->gR;
    long gr = right->gR;

    Vec<vec_ZZ_p> w;
    mat(w, a, gr, gl);
    for (long i = 0; i < gl; ++i) right->applyOmegaInv(w[i], w[i]);

    Vec<vec_ZZ_p> wt;
    transpose(wt, w);
    for (long j = 0; j < gr; ++j) left->applyOmegaInv(wt[j], wt[j]);

    transpose_vec(b, wt);
  }
}

void ring::applyOmegaInv (vec_ZZ& b, const vec_ZZ& a) {
  vec_ZZ_p _b;
  applyOmegaInv(_b, conv<vec_ZZ_p>(a));
  b = conv<vec_ZZ>(_b);
}

/**********************************************************************/
ZZ inf_norm (const vec_ZZ& a) {
  ZZ w(0);
  for (long i = 0; i < a.length(); ++i)
    if (w < abs(a[i])) w = abs(a[i]);
  return w;
}

void center_lift (vec_ZZ& b, const vec_ZZ_p& a) {
  b.SetLength(a.length());
  ZZ q = ZZ_p::modulus();
  for (long i=0; i < a.length(); ++i) {
    if (conv<ZZ>(a[i]) >= q/2) b[i] = conv<ZZ>(a[i]) - q;
    else b[i] = conv<ZZ>(a[i]);
  }
}

void center_lift (vec_ZZ& b, const vec_ZZ& a, ZZ& p) {
  ZZ_pContext context;
  context.save();

  ZZ_p::init(p);
  center_lift(b, conv<vec_ZZ_p>(a));

  context.restore();
}

vec_ZZ center_lift (const vec_ZZ_p& a) {
  vec_ZZ b;
  center_lift(b, a);
  return b;
}

vec_ZZ center_lift (const vec_ZZ& a, ZZ& p) {
  vec_ZZ b;
  center_lift(b, a, p);
  return b;
}

// q -> q1
void rescale (vec_ZZ &b, const vec_ZZ &a, const ZZ& q, const ZZ& q1) {
  b.SetLength(a.length());
  for (long i = 0; i < a.length(); ++i) {
    b[i] = (a[i] * q1 + q/2) / q;
    if (b[i] >= q1) b[i] -= q1;
    //b[i] = b[i] % q1;  /*@@*/
  }
}

void lsd_rescale (vec_ZZ &b, const vec_ZZ &a, const ZZ& P, const ZZ& t) {
  long dim = a.length();
  b.SetLength(dim);
  vec_ZZ delta;
  delta.SetLength(dim);

  ZZ _g, u, v;
  XGCD(_g, u, v, P, t);  // 1 = w = u*P + v*t
  ZZ w1 = v*t; // = 1 mod P, = 0 mod t

  for (long i = 0; i < dim; ++i) {
    delta[i] = (((- a[i]) % P) * w1) % (P*t);
    b[i] = (a[i] + delta[i]) / P;
  }
}

/***********************************************************************/
/* returns the power decomp of x mod 2^n  */
void power_decomp (Vec<vec_ZZ>& z, long n, const vec_ZZ& x) {
  long dim = x.length();
  vec_ZZ zero; zero.SetLength(dim);

  z.SetLength(n);
  for (long j = 0; j < n; ++j) z[j] = zero;

  Vec<Vec<vec_ZZ>> y;
  y.SetLength(n);
  for (long i = 0; i < n; ++i) {
    y[i].SetLength(n-i);
    for (long j = 0; j < n-i; ++j) y[i][j] = zero;
  }

  for (long i = 0; i < n; ++i) {
    // y[i][0] = (x - sum_{j=0}^{i-1} 2^j * y[j][i-j]) / 2^i
    y[i][0] = x;
    for (long j = 0; j < i; ++j) {
      for (long k = 0; k < dim; ++k) {
	y[i][0][k] = (y[i][0][k] - y[j][i-j][k]) % power(ZZ(2), n);
	y[i][0][k] /= ZZ(2);
      }
    }

    // y[i][j] = y[i][j-1]^2  (j = 1..(n-1-i))
    for (long j = 1; j < n-i; ++j) {
      for (long k = 0; k < dim; ++k) y[i][j][k] = (y[i][j-1][k] * y[i][j-1][k]) % power(ZZ(2), n);
    }
  }

  for (long i = 0; i < n; ++i) {
    z[i] = y[i][n-1-i];
    for (long k = 0; k < dim; ++k) z[i][k] = z[i][k] % power(ZZ(2), n-i);
  }
}

// returns (x >> m) mod 2^(n-m) given (x mod 2^n)
void right_shift (vec_ZZ& z, const vec_ZZ& x, long m, long n) {
  long dim = x.length();
  vec_ZZ zero; zero.SetLength(dim);

  long l = n - m;

  Vec<vec_ZZ> y; y.SetLength(n);
  for (long j = 0; j < n; ++j) y[j] = zero;
  power_decomp(y, n, x);

  // Drop the least m bits (LowerClear)
  for (long i = 0; i < l; ++i) y[i] = y[m+i];

  // z = y[0] + 2*y[1] + ... + 2^(l-1)*y[l-1]
  z = zero;
  for (long i = 0; i < l; ++i) {
    // z = (z + (2^i)*y[i]) % 2^l
    vec_ZZ w = zero;
    for (long j = 0; j < dim; ++j) {
      w[j] = (power(ZZ(2),i) * y[i][j]) % power(ZZ(2),l);
      z[j] = (z[j] + w[j]) % power(ZZ(2),l);
    }
  }
}

void right_shift (vec_ZZ_p& z, const vec_ZZ_p& x, long m, long n) {
  vec_ZZ _z; _z.SetLength(x.length());
  right_shift(_z, conv<vec_ZZ>(x), m, n);
  z = conv<vec_ZZ_p>(_z);
}

// returns the (i,j)-th component of the cyclic matrix with the first row of 'm'
ZZ_p to_mat (const vec_ZZ_p& m, long i, long j) {
  long dim = m.length();
  assert(0 <= i && i < dim && 0 <= j && j < dim);
  return m[(i + j) % dim];
}

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

/* returns the tensor product of a and a1 */
void tensor_prod (vec_ZZ& A, const vec_ZZ& a, const vec_ZZ& a1, const ZZ& q) {
  long gout = a.length();
  long gin = a1.length();

  for (long i = 0; i < gout; ++i) {
    for (long j = 0; j < gin; ++j) {
      A[i*gin + j] = (a[i] * a1[j]) % q;
    }
  }
}

// left shift of vectors
void shift (vec_ZZ& b, const long i, const vec_ZZ& a) {
  long dim = a.length();
  vec_ZZ w;
  w.SetLength(dim);
  for (long j = 0; j < dim; ++j) w[j] = a[(j + i) % dim];
  b = w;
}

void shift (vec_ZZ_p& b, const long i, const vec_ZZ_p& a) {
  long dim = a.length();
  vec_ZZ_p w;
  w.SetLength(dim);
  for (long j = 0; j < dim; ++j) w[j] = a[(j + i) % dim];
  b = w;
}

vec_ZZ shift (const long i, const vec_ZZ& a) {
  vec_ZZ w;
  shift(w, i, a);
  return w;
}

// one-position left shift
void shift (vec_ZZ_p& a) {
  long dim = a.length();
  ZZ_p w = a[0];
  for (long i = 1; i < dim; ++i) a[i-1] = a[i];
  a[dim-1] = w;
}

void shift_right (vec_ZZ& b, const long i, const vec_ZZ& a) {
  shift(b, a.length()-i, a);
}

void shift_right (vec_ZZ_p& b, const long i, const vec_ZZ_p& a) {
  shift(b, a.length()-i, a);
}

// one-position right shift
void right_shift (vec_ZZ_p& a) {
  long dim = a.length();
  ZZ_p w = a[dim-1];
  for (long i = dim-1; i > 0; --i) a[i] = a[i-1];
  a[0] = w;
}

void ring::deep_shift (string _label, vec_ZZ& v, long i) {
  if ((type == prime) && (label == _label)) {
    shift(v, (i<0)?(i+gR):i, v);
  }
  else if ((type == prime) && (label != _label)) {
    return;
  }
  else {  // composite ring
    long gl = left->gR;
    long gr = right->gR;

    Vec<vec_ZZ> w;
    mat(w, v, gr, gl);
    for (long j = 0; j < gl; ++j) right->deep_shift(_label, w[j], i);

    Vec<vec_ZZ> wt;
    transpose(wt, w);
    for (long j = 0; j < gr; ++j) left->deep_shift(_label, wt[j], i);

    transpose_vec(v, wt);
  }
}

void ring::deep_shift_right (string _label, vec_ZZ& v, long i) {
  deep_shift(_label, v, -i);
}

/***********************************************************************/
void get_subrings (ring& rg, Vec<ring>& subrings) {
  if (rg.type == prime) {
    append(subrings, rg);
  }
  else {  // type == composite
    get_subrings(*(rg.right), subrings);
    get_subrings(*(rg.left), subrings);
  }
}

long ring::get_subrings (Vec<ring>& subrings) {
  ::get_subrings(*this, subrings);
  return subrings.length();
}

long param::get_subrings (Vec<ring>& subrings) {
  return rg.get_subrings(subrings);
}

long ring::get_subring_index (string label) {
  Vec<ring> subrings;
  long depth = get_subrings(subrings);

    for (long i = 0; i < depth; ++i) {
      if (subrings[i].label == label) return i;
    }

    return -1;
}

vec_ZZ ring::index_convert (long j) {
  vec_ZZ tensor_index;
  tensor_index.SetLength(depth);

  long jj = j;

  for (long i = 0; i < depth; ++i) {
    long a = jj % subrings[i].gR;
    tensor_index[i] = a;
    jj = (jj - a) / subrings[i].gR;
  }

  return tensor_index;
}

// y = (x, x, ..., x) where x is assumed to be an element of the right subring
void ring::embed_1 (vec_ZZ& y, vec_ZZ& x) {
  y.SetLength(gR);
  for (long i = 0; i < left->gR; ++i) {
    for (long j = 0; j < right->gR; ++j) {
      y[i*right->gR + j] = x[j];
    }
  }
}

vec_ZZ ring::embed_1 (vec_ZZ& x) {
  vec_ZZ y;
  embed_1(y, x);
  return y;
}

// y = (x, 0, 0, ..., 0) where x is assumed to be an element of the right subring
void ring::embed_0 (vec_ZZ& y, vec_ZZ& x) {
  y.SetLength(gR);
  for (long j = 0; j < right->gR; ++j) {
    y[j] = x[j];
  }
}

vec_ZZ ring::embed_0 (vec_ZZ& x) {
  vec_ZZ y;
  embed_0(y, x);
  return y;
}

// y = (x, 0, 0, ..., 0) where x is assumed to be an element of the prime subring with label subring_label
void ring::embed_0 (vec_ZZ& y, string subring_label, vec_ZZ& x) {
  y.SetLength(gR);
  long d = get_subring_index(subring_label);

  for (long j = 0; j < gR; ++j) {
    vec_ZZ tensor_index = index_convert(j);
    long skip = 0;
    for (long k = 0; k < tensor_index.length(); ++k) {
      if (k != d && tensor_index[k] != 0) {
        skip = 1;
        break;
      }
    }
    if (!skip) y[j] = x[conv<long>(tensor_index[d])];
  }
}

vec_ZZ ring::embed_0 (string subring_label, vec_ZZ& x) {
  vec_ZZ y;
  embed_0(y, subring_label, x);
  return y;
}

// y = (x, x, ..., x) where x is assumed to be an element of the prime subring with label subring_label
void ring::embed_1 (vec_ZZ& y, string subring_label, vec_ZZ& x) {
  y.SetLength(gR);
  long d = get_subring_index(subring_label);

  for (long j = 0; j < gR; ++j) {
    vec_ZZ tensor_index = index_convert(j);
    y[j] = x[conv<long>(tensor_index[d])];
  }
}

vec_ZZ ring::embed_1 (string subring_label, vec_ZZ& x) {
  vec_ZZ y;
  embed_1(y, subring_label, x);
  return y;
}

// y = (-x, -x, ..., -x) where x is assumed to be an element of the prime subring with label subring_label
void ring::embed_11 (vec_ZZ& y, string subring_label, vec_ZZ& x) {
  y.SetLength(gR);
  long d = get_subring_index(subring_label);

  for (long j = 0; j < gR; ++j) {
    vec_ZZ tensor_index = index_convert(j);
    y[j] = -x[conv<long>(tensor_index[d])];
  }
}

vec_ZZ ring::embed_11 (string subring_label, vec_ZZ& x) {
  vec_ZZ y;
  embed_11(y, subring_label, x);
  return y;
}

// trace to the right ring
void ring::trace (vec_ZZ& y, vec_ZZ& x, ZZ& q) {
  y.SetLength(right->gR);
  for (long j = 0; j < x.length(); ++j) {
    long i = j % right->gR;
    y[i] = (y[i] + x[j]) % q;
  }
}

// trace to the subring of label 'subring_label'
// -- the subring must be prime
void ring::trace (vec_ZZ& y, string subring_label, vec_ZZ& x, ZZ& q) {
  //y.SetLength(gR);
  long d = get_subring_index(subring_label);
  y.SetLength(subrings[d].gR);

  for (long j = 0; j < x.length(); ++j) {
    vec_ZZ tensor_index = index_convert(j);
    long i = conv<long>(tensor_index[d]);
    y[i] = (y[i] + x[j]) % q;
  }
}

// returns the i-th diagonal vector of the extended Omega matrix,
// corresponding to the subring with label '_label'
void ring::deep_omega (vec_ZZ& extended_omega, string _label, long i) {
  Vec<ring> subrings;
  get_subrings(subrings);
  long d = get_subring_index(_label);

  vec_ZZ omg;
  diagonal(omg, i, subrings[d].omega);

  //embed_0(extended_omega, _label, omg); /*@@*/
  embed_1(extended_omega, _label, omg); /*@@*/
}

// returns the i-th diagonal vector of the extended Omega_inv matrix,
// corresponding to the subring with label '_label'
void ring::deep_omega_inv (vec_ZZ& extended_omega_inv, string _label, long i) {
  Vec<ring> subrings;
  get_subrings(subrings);
  long d = get_subring_index(_label);

  vec_ZZ omg_inv;
  diagonal(omg_inv, i, subrings[d].omega_inv);

  //embed_0(extended_omega_inv, _label, omg_inv); /*@@*/
  embed_1(extended_omega_inv, _label, omg_inv); /*@@*/
}


#define BOOST_TEST_MODULE circuit_test
#include <boost/test/included/unit_test.hpp>

#include "../bin/types.h"
#include "../bin/circuit.h"

using namespace nil::crypto3;
using namespace nil::crypto3::zk;

template<typename FieldType>
bool test_bp_input(float minLat, float maxLat, float minLng, float maxLng, float posLat, float posLng) {
    blueprint<FieldType> bp;
    LocationCircuit<FieldType> circuit(bp);
    circuit.generate_r1cs_constraints(bp);
    circuit.generate_r1cs_witness(bp, minLat, maxLat, minLng, maxLng, posLat, posLng);
    return bp.is_satisfied();
}

void test_circuit_range(bool isSatisfied, float minLat, float maxLat, float minLng, float maxLng, float posLat, float posLng) {
    std::cout << "Testing " << posLat <<" in range (" << minLat << ", " << maxLat << ")" << std::endl;
    std::cout << "Testing " << posLng <<" in range (" << minLng << ", " << maxLng << ")" << std::endl;

    std::cout << "Testing latitude and longtitude range..." << std::endl;
    BOOST_CHECK(test_bp_input<field_type>(minLng, maxLng, minLat, maxLat, posLng, posLat) == isSatisfied);
}

void test_circuit_range_boundaries(bool isSatisfied, float minLat, float maxLat, float minLng, float maxLng) {
    std::cout << "Testing boundaries of range (" << minLat << ", " << maxLat << ")" << std::endl;
    std::cout << "Testing boundaries of range (" << minLng << ", " << maxLng << ")" << std::endl;

    std::cout << "Testing latitude and longtitude range min boundaries..." << std::endl;
    BOOST_CHECK(test_bp_input<field_type>(minLng, maxLng, minLat, maxLat, minLng, minLat) == isSatisfied);

    std::cout << "Testing latitude and longtitude range max boundaries..." << std::endl;
    BOOST_CHECK(test_bp_input<field_type>(minLng, maxLng, minLat, maxLat, maxLng, maxLat) == isSatisfied);
}

BOOST_AUTO_TEST_SUITE(circuit_test_suite)

float minLat = 150.987654,
      maxLat = 160.1234567,
      posLatInside = 155.7593730,
      posLatOutsideLess = 150.0593730,
      posLatOutsideMore = 161.7593730;

float minLng = 10.123456,
      maxLng = 10.234567,
      posLngInside = 10.181234,
      posLngOutsideLess = 10.101324,
      posLngOutsideMore = 10.351324;

BOOST_AUTO_TEST_CASE(circuit_test_valid_ranges) {
    std::cout << setprecision(6);

    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatOutsideLess, posLngOutsideLess);
    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatOutsideMore, posLngOutsideMore);
    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatOutsideLess, posLngOutsideMore);
    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatOutsideMore, posLngOutsideLess);

    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatOutsideLess, posLngInside);
    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatOutsideMore, posLngInside);

    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatInside, posLngOutsideLess);
    test_circuit_range(true, minLat, maxLat, minLng, maxLng, posLatInside, posLngOutsideMore);

    // test negative values
    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatOutsideLess, -posLngOutsideLess);
    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatOutsideMore, -posLngOutsideMore);
    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatOutsideLess, -posLngOutsideMore);
    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatOutsideMore, -posLngOutsideLess);

    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatOutsideLess, -posLngInside);
    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatOutsideMore, -posLngInside);

    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatInside, -posLngOutsideLess);
    test_circuit_range(true, -maxLat, -minLat, -maxLng, -minLng, -posLatInside, -posLngOutsideMore);

}

BOOST_AUTO_TEST_CASE(circuit_test_invalid_ranges) {
    std::cout << setprecision(6);

    test_circuit_range(false, minLat, maxLat, minLng, maxLng, posLatInside, posLngInside);

    // test negative values
    test_circuit_range(false, -maxLat, -minLat, -maxLng, -minLng, -posLatInside, -posLngInside);
}

BOOST_AUTO_TEST_CASE(circuit_test_boundaries) {
    std::cout << setprecision(6);

    test_circuit_range_boundaries(true, minLat, maxLat, minLng, maxLng);

    // test negative values
    test_circuit_range_boundaries(true, -maxLat, -minLat, -maxLng, -minLng);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
// #include <boost/gil.hpp>
#include <boost/gil/extension/histogram/histogram.hpp>
#include <boost/gil/extension/histogram/histogram_algorithms.hpp>
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil/color_base.hpp>
#include <type_traits>
#include <boost/gil/detail/mp11.hpp>
#include <boost/mp11.hpp>

using namespace boost::gil;

int main() {
    rgb8_image_t img;
    read_and_convert_image("test.png", img, png_tag());
    histogram<gray8_pixel_t> hist;
    fill_histogram<rgb8_view_t, gray8_pixel_t>(view(img), hist);
    rgb8_image_t equalized_img(img.dimensions());
    rgb8_image_t ref_img;
    histogram<gray8_pixel_t> ref_hist;
    read_and_convert_image("test2.png", ref_img, png_tag());
    fill_histogram<rgb8_view_t, gray8_pixel_t>(view(ref_img), ref_hist);
    float ref_cdf[256];
    ref_cdf[0] = (float)ref_hist[gray8_pixel_t(0)] / ref_img.width() / ref_img.height();
    for (int i = 1; i < 256; ++i) ref_cdf[i] = ref_cdf[i - 1] + (float)ref_hist[gray8_pixel_t(i)] / ref_img.width() / ref_img.height();

    match_histograms(view(img), ref_cdf, view(equalized_img));
    write_view("test_histogram_matched.jpg", view(equalized_img), jpeg_tag{});
    return 0;
}


// test_find_scale.cpp

// Copyright John Maddock 2007.
// Copyright Paul A. Bristow 2007.

// Use, modification and distribution are 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)

// Basic sanity test for find_scale function.

// Default distribution domain error policy is
// #define BOOST_MATH_DOMAIN_ERROR_POLICY throw_on_error

#include <pch.hpp>

#include <boost/math/tools/test.hpp>
#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // Default type double.
  using boost::math::normal_distribution; // All floating-point types.
#include <boost/math/distributions/cauchy.hpp> // for cauchy_distribution
  using boost::math::cauchy;
#include <boost/math/distributions/pareto.hpp> // for cauchy_distribution
  using boost::math::pareto;
#include <boost/math/distributions/find_scale.hpp>
  using boost::math::find_scale;
  using boost::math::complement;// will be needed by users who want complement,
#include <boost/math/policies/policy.hpp>
  using boost::math::policies::policy;

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // for test_main
#include <boost/test/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE_FRACTION, BOOST_CHECK_EQUAL...

#include <iostream>
#include <iomanip>
  using std::cout; using std::endl; using std::fixed;
  using std::right; using std::left; using std::showpoint;
  using std::showpos; using std::setw; using std::setprecision;

#include <limits>
  using std::numeric_limits;

template <class RealType> // Any floating-point type RealType.
void test_spots(RealType)
{ // Parameter only provides the type, float, double... value ignored.

  // Basic sanity checks, test data may be to double precision only
  // so set tolerance to 100 eps expressed as a fraction,
  // or 100 eps of type double expressed as a fraction,
  // whichever is the larger.

  RealType tolerance = (std::max)
      (boost::math::tools::epsilon<RealType>(),
      static_cast<RealType>(std::numeric_limits<double>::epsilon()));
   tolerance *= 100; // 100 eps as a fraction.

  cout << "Tolerance for type " << typeid(RealType).name()  << " is "
    << setprecision(3) << tolerance  << " (or " << tolerance * 100 << "%)." << endl;

  BOOST_MATH_CHECK_THROW( // Probability outside 0 to 1.
       find_scale<normal_distribution<RealType> >(
       static_cast<RealType>(0.), static_cast<RealType>(-1.), static_cast<RealType>(0.) ),
       std::domain_error);

  normal_distribution<RealType> n; // standard N(0,1)
  BOOST_CHECK_EQUAL(n.location(), 0); // aka mean.
  BOOST_CHECK_EQUAL(n.scale(), 1); // aka standard_deviation.

   // Check for 'bad' arguments.
  BOOST_MATH_CHECK_THROW(find_scale<normal>(0., -1., 0.), std::domain_error); // p below 0 to 1.
  BOOST_MATH_CHECK_THROW(find_scale<normal>(0., 2., 0.), std::domain_error); // p above 0 to 1.
  BOOST_MATH_CHECK_THROW(find_scale<normal>(numeric_limits<double>::infinity(), 0.5, 0.),
    std::domain_error); // z not finite.
  BOOST_MATH_CHECK_THROW(find_scale<normal>(numeric_limits<double>::quiet_NaN(), -1., 0.),
    std::domain_error); // z not finite
  BOOST_MATH_CHECK_THROW(find_scale<normal>(0., -1., numeric_limits<double>::quiet_NaN()),
    std::domain_error); // scale not finite

  BOOST_MATH_CHECK_THROW(find_scale<normal>(complement(0., -1., 0.)), std::domain_error); // p below 0 to 1.
  BOOST_MATH_CHECK_THROW(find_scale<normal>(complement(0., 2., 0.)), std::domain_error); // p above 0 to 1.
  BOOST_MATH_CHECK_THROW(find_scale<normal>(complement(numeric_limits<double>::infinity(), 0.5, 0.)),
    std::domain_error); // z not finite.
  BOOST_MATH_CHECK_THROW(find_scale<normal>(complement(numeric_limits<double>::quiet_NaN(), -1., 0.)),
    std::domain_error); // z not finite
  BOOST_MATH_CHECK_THROW(find_scale<normal>(complement(0., -1., numeric_limits<double>::quiet_NaN())),
    std::domain_error); // scale not finite

  BOOST_MATH_CHECK_THROW(find_scale<normal>(complement(0., -1., 0.)), std::domain_error); // p below 0 to 1.

  // Check for ab-use with unsuitable distribution(s), for example,
  // pareto distribution (and most others) can't be used with find_scale (or find_location)
  // because they lack the scale and location attributes.
  // BOOST_MATH_CHECK_THROW(find_scale<pareto>(0., 0.5, 0.), std::domain_error);
  // correctly fails to compile in find_scale() at
  // BOOST_STATIC_ASSERT(::boost::math::tools::is_scaled_distribution<Dist>::value);

  // Check doesn't throw when an ignore_error for domain_error policy is used.
  using boost::math::policies::policy;
  using boost::math::policies::domain_error;
  using boost::math::policies::ignore_error;

  // Define a (bad?) policy to ignore domain errors ('bad' arguments):
  typedef policy<domain_error<ignore_error> > ignore_domain_policy;
  // Using a typedef is convenient, especially if it is re-used.
#ifndef BOOST_NO_EXCEPTIONS
  BOOST_CHECK_NO_THROW(find_scale<normal>(0, -1, 1,
    ignore_domain_policy())); // probability outside [0, 1]
  BOOST_CHECK_NO_THROW(find_scale<normal>(numeric_limits<double>::infinity(), -1, 1,
    ignore_domain_policy())); // z not finite.
  BOOST_CHECK_NO_THROW(find_scale<normal>(complement(0, -1, 1, ignore_domain_policy()))); // probability outside [0, 1]
  BOOST_CHECK_NO_THROW(find_scale<normal>(complement(numeric_limits<double>::infinity(), -1, 1,
    ignore_domain_policy()))); // z not finite.
#endif
  RealType l = 0.; // standard normal distribution.
  RealType sd = static_cast<RealType>(1); // normal default standard deviation = 1.
  normal_distribution<RealType> n01(l, sd); // mean(location) = 0, standard_deviation (scale) = 1.
  RealType z = static_cast<RealType>(-2); // z to give prob p
  //cout << "Standard normal distribution with standard deviation = " << sd
  //  << " has " << "fraction <= " << z << " = "  << cdf(n01, z) << endl;
  // Standard normal distribution with standard deviation = 1 has fraction <= -2 = 0.0227501

  //normal_distribution<RealType> np001pc(l, sd); // Same mean(location) but with standard_deviation (scale) changed.
  //cout << "Normal distribution with standard deviation = " << s
  //  << " has " << "fraction <= " << z << " = "  << cdf(np001pc, z) << endl;

  // Find scale to give a probability p (0.001) of z (-2)
  RealType p = static_cast<RealType>(0.001); // only 0.1% to be below z (-2).
  // location (mean) remains at zero.
  RealType s = find_scale<normal_distribution<RealType> >(z, p, l);
  //cout << "Mean " << l << ", z " << z << ",  p " << p
  //  << ", sd " << sd << ", find_scale " << s
  //  << ", difference in sd " << s - sd << endl;
  // Mean 0, z -2,  p 0.001, sd 1, find_scale 0.64720053440907599, difference in sd -0.352799

  cout.precision(17);
  BOOST_CHECK_CLOSE_FRACTION(s, static_cast<RealType>(0.64720053440907599L), tolerance);

  normal_distribution<RealType> np001pc(l, s); // Same mean(location) but with standard_deviation (scale) changed.
  //cout << "Normal distribution with standard deviation = " << s
  //  << " has " << "fraction <= " << z << " = "  << cdf(np001pc, z) << endl;
  // Normal distribution with standard deviation = 0.647201 has fraction <= -2 = 0.001

  // Check cdf such that only fraction p really is below changed standard deviation s.
  BOOST_CHECK_CLOSE_FRACTION(p, cdf(np001pc, z), tolerance);

  // Check that some policies can be applied (though results not used here).
  s = find_scale<normal_distribution<RealType> >(z, p, l, policy<>()); // Default policy, needs using boost::math::policies::policy;
  s = find_scale<normal_distribution<RealType> >(z, p, l, boost::math::policies::policy<>()); // Default policy, fully specified.
  s = find_scale<normal_distribution<RealType> >(z, p, l, ignore_domain_policy()); // find_scale with new policy, using typedef.
  s = find_scale<normal_distribution<RealType> >(z, p, l, policy<domain_error<ignore_error> >()); // New policy, without typedef.

  // Check that can use the complement version too.
  RealType q = 1 - p; // complement.
  s = find_scale<normal_distribution<RealType> >(complement(z, q, l)); // Implicit default policy.
  BOOST_CHECK_CLOSE_FRACTION(s, static_cast<RealType>(0.64720053440907599L), tolerance);
  s = find_scale<normal_distribution<RealType> >(complement(z, q, l, policy<>())); // Explicit default policy.
  BOOST_CHECK_CLOSE_FRACTION(s, static_cast<RealType>(0.64720053440907599L), tolerance);

  normal_distribution<RealType> np95pc(l, s); // Same mean(location) but with new standard_deviation (scale).

  //cout << "Mean " << l << ", z " << z << ",  q " << q
  //<< ", sd " << sd << ", find_scale " << s
  //<< ", difference in sd " << s - sd << endl;

  //cout << "Normal distribution with standard deviation = " << s
  //  << " has " << "fraction <= " << z << " = "  << cdf(np001pc, z) << endl;
  BOOST_CHECK_CLOSE_FRACTION(q, cdf(complement(np95pc, z)), tolerance);

} // template <class RealType>void test_spots(RealType)

BOOST_AUTO_TEST_CASE( test_main )
{
  // Basic sanity-check spot values.

   // (Parameter value, arbitrarily zero, only communicates the floating-point type).
  test_spots(0.0F); // Test float.
  test_spots(0.0); // Test double.
  #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_spots(0.0L); // Test long double.
    #if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x0582))
    test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
  #endif
  #else
     std::cout << "<note>The long double tests have been disabled on this platform "
        "either because the long double overloads of the usual math functions are "
        "not available at all, or because they are too inaccurate for these tests "
        "to pass.</note>" << std::endl;
  #endif

} // BOOST_AUTO_TEST_CASE( test_main )

/*

Output is:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_find_scale.exe"
Running 1 test case...
Tolerance for type float is 1.19e-005 (or 0.00119%).
Tolerance for type double is 2.22e-014 (or 2.22e-012%).
Tolerance for type long double is 2.22e-014 (or 2.22e-012%).
Tolerance for type class boost::math::concepts::real_concept is 2.22e-014 (or 2.22e-012%).
*** No errors detected

*/


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkPointCloudLinearProjector.cxx
  Author: Pierre Guilbert

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

// LOCAL
#include "vtkPointCloudLinearProjector.h"
#include "vtkEigenTools.h"

// STD
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>

// VTK
#include <vtkObjectFactory.h>
#include <vtkImageData.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkPointData.h>
#include <vtkPolyData.h>
#include <vtkStreamingDemandDrivenPipeline.h>

// BOOST
#include <boost/algorithm/string.hpp>

// Eigen
#include <Eigen/Dense>

// Implementation of the New function
vtkStandardNewMacro(vtkPointCloudLinearProjector)

//-----------------------------------------------------------------------------
int vtkPointCloudLinearProjector::FillInputPortInformation(int port, vtkInformation *info)
{
  if (port == 0)
  {
    info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData" );
    return 1;
  }
  return 0;
}

//-----------------------------------------------------------------------------
int vtkPointCloudLinearProjector::RequestInformation(vtkInformation *vtkNotUsed(request),
                                                vtkInformationVector **vtkNotUsed(inputVector),
                                                vtkInformationVector *outputVector)
{
  vtkInformation* outInfo = outputVector->GetInformationObject(0);
  outInfo->Set(vtkStreamingDemandDrivenPipeline::WHOLE_EXTENT(),
               0, this->Dimensions[0] - 1,
               0, this->Dimensions[1] - 1,
               0, 0);

  outInfo->Set(vtkDataObject::ORIGIN(),this->Origin, 3);
  outInfo->Set(vtkDataObject::SPACING(),this->Spacing, 3);

  vtkDataObject::SetPointDataActiveScalarInfo(outInfo, VTK_DOUBLE, 1);
  return VTK_OK;
}

//-----------------------------------------------------------------------------
int vtkPointCloudLinearProjector::RequestData(vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector, vtkInformationVector *outputVector)
{
  // Get the input
  vtkPolyData * input = vtkPolyData::GetData(inputVector[0]->GetInformationObject(0));

  double point[3];
  Eigen::Vector3d X;

  auto transformedPoints = vtkSmartPointer<vtkPoints>::New();
  transformedPoints->Resize(input->GetNumberOfPoints());
  // Transform the input polydata
  for (vtkIdType pointIndex = 0; pointIndex < input->GetNumberOfPoints(); ++pointIndex)
  {
    input->GetPoint(pointIndex, point);
    X << point[0], point[1], point[2];
    X = this->Projector * X;
    point[0] = X(0); point[1] = X(1); point[2] = X(2);
    transformedPoints->SetPoint(pointIndex, point);
  }
  transformedPoints->Modified();

  // Get the point cloud bounding box parameters
  double boundingBox[6];
  transformedPoints->GetBounds(boundingBox);
  this->Spacing[0] = (boundingBox[1] - boundingBox[0]) / static_cast<double>(this->Dimensions[0]);
  this->Spacing[1] = (boundingBox[3] - boundingBox[2]) / static_cast<double>(this->Dimensions[1]);

  // Get the output image and fill with zeros
  vtkSmartPointer<vtkImageData> image = vtkSmartPointer<vtkImageData>::New();
  image->SetDimensions(this->Dimensions[0], this->Dimensions[1], 1);
  image->SetSpacing(this->Spacing);
  image->SetOrigin(this->Origin);
  image->AllocateScalars(VTK_DOUBLE, 1);
  unsigned char* dataPointer = reinterpret_cast<unsigned char*>(image->GetScalarPointer());
  std::fill(dataPointer, dataPointer + this->Dimensions[0] * this->Dimensions[1], 0);

  // Compute distribution about the height of the data lying in a pixel
  std::vector<std::vector<double> > perPixelDistribution(this->Dimensions[0] * this->Dimensions[1]);
  double scaleX = (this->Dimensions[0] - 1) / (boundingBox[1] - boundingBox[0]);
  double scaleY = (this->Dimensions[1] - 1) / (boundingBox[3] - boundingBox[2]);
  for (vtkIdType pointIndex = 0; pointIndex < input->GetNumberOfPoints(); ++pointIndex)
  {
    transformedPoints->GetPoint(pointIndex, point);
    int xPixelCoord = std::floor((point[0] - boundingBox[0]) * scaleX);
    int yPixelCoord = std::floor((point[1] - boundingBox[2]) * scaleY);
    perPixelDistribution[xPixelCoord + this->Dimensions[0] * yPixelCoord].push_back(point[2]);
  }

  // fill the image
  for (int x = 0; x < this->Dimensions[0]; ++x)
  {
    for (int y = 0; y < this->Dimensions[1]; ++y)
    {
      // if the pixel is empty, skip it
      if (perPixelDistribution[x + this->Dimensions[0] * y].size() == 0)
      {
        continue;
      }

      // sort the heights values
      std::sort(perPixelDistribution[x + this->Dimensions[0] * y].begin(),
                perPixelDistribution[x + this->Dimensions[0] * y].end());

      int rankIndex = std::floor((perPixelDistribution[x + this->Dimensions[0] * y].size() - 1) * this->RankPercentil);
      double rankValue = perPixelDistribution[x + this->Dimensions[0] * y][rankIndex];
      double value = rankValue - boundingBox[4];
      image->SetScalarComponentFromDouble(x, y, 0, 0, value);
    }
  }

  vtkImageData* outputImage = vtkImageData::GetData(outputVector->GetInformationObject(0));
  outputImage->ShallowCopy(image);
  return 1;
}

//-----------------------------------------------------------------------------
void vtkPointCloudLinearProjector::SetPlaneNormal(double w0, double w1, double w2)
{
  // Here we will construct a new base of R3 using
  // the normal of the plane as the Z-axis. To proceed,
  // we will compute the rotation that map ez toward n
  // and its axis being cross(ez, n)
  Eigen::Vector3d ez(0, 0, 1);
  Eigen::Vector3d n(w0, w1, w2);

  // check that the plane normal is not the null pointer
  if (n.norm() < std::numeric_limits<float>::epsilon())
  {
    vtkGenericWarningMacro("The plane normal should not be the null vector");
    return;
  }
  n.normalize();

  Eigen::Vector3d u = ez.cross(n);
  // it means that n and ez are colinear
  if (u.norm() < std::numeric_limits<float>::epsilon())
  {
    return;
  }
  u.normalize();

  double angle = SignedAngle(ez, n);
  Eigen::Matrix3d R(Eigen::AngleAxisd(angle, u));
  this->ChangeOfBasis = R;
  this->Projector = this->DiagonalizedProjector * this->ChangeOfBasis.transpose();
}


//  (C) Copyright Matt Borland 2022.
//  Use, modification and distribution are 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)

#include <cmath>
#include <limits>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/constants/constants.hpp>

namespace boost { namespace math {

// Calculates log(exp(x1) + exp(x2))
template <typename Real>
Real logaddexp(Real x1, Real x2) noexcept
{
    using std::log1p;
    using std::exp;
    using std::abs;

    // Validate inputs first
    if (!(boost::math::isfinite)(x1))
    {
        return x1;
    }
    else if (!(boost::math::isfinite)(x2))
    {
        return x2;
    }

    const Real temp = x1 - x2;

    if (temp > 0)
    {
        return x1 + log1p(exp(-temp));
    }

    return x2 + log1p(exp(temp));
}

}} // Namespace boost::math


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#include <boost/math/special_functions/relative_difference.hpp>
#include <popart/op/scale.hpp>
#include <popart/patterns/negativeonescalepattern.hpp>
#include <popart/tensor.hpp>
#include <popart/tensorinfo.hpp>

using boost::math::epsilon_difference;

namespace popart {

bool NegativeOneScalePattern::matches(Op *op) const {
  if (!(op->isConvertibleTo<ScaleOp>())) {
    return false;
  }

  float scale_factor = dynamic_cast<ScaleOp *>(op)->getScaleFactor();
  return epsilon_difference(scale_factor, -1.0f) < 1.0f;
}

// output = neg(x)
std::vector<std::unique_ptr<Op>>
NegativeOneScalePattern::sequence(Op *op) const {
  std::vector<std::unique_ptr<Op>> seq;

  seq.push_back(makeReplacementOp(Onnx::AiOnnx::OpSet9::Neg, op));

  return seq;
}

namespace {
static PatternCreator<NegativeOneScalePattern>
    negativeOneScalePattern("NegativeOneScale");
}

} // namespace popart


#ifndef __solver_iHQP_solver_h__
#define __solver_iHQP_solver_h__

#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "weighted_hqp/HCod.hpp"
#include "weighted_hqp/ehqp_primal.hpp"
#include "weighted_hqp/step_length.hpp"
#include "weighted_hqp/Up.hpp"

namespace hcod{
    class iHQP_solver{
        public:
            iHQP_solver(const std::vector<Eigen::MatrixXd> &A, const std::vector<Eigen::MatrixXd> &b, const std::vector<Eigen::VectorXi> &btype, const std::vector<Eigen::VectorXi> &aset_init, const std::vector<Eigen::VectorXi> &aset_bound);
            iHQP_solver(const std::vector<Eigen::MatrixXd> &A, const std::vector<Eigen::MatrixXd> &b, const std::vector<Eigen::VectorXi> &btype, const std::vector<Eigen::VectorXi> &aset_init, const std::vector<Eigen::VectorXi> &aset_bound, const std::vector<Eigen::MatrixXd> &W);
            ~iHQP_solver(){};

        private:
            void set_problem();

        public:
            Eigen::VectorXd solve();
            std::vector<H_structure> geth(){
                return h_;
            }

        private:
            std::vector<Eigen::MatrixXd> A_;
            std::vector<Eigen::MatrixXd> b_;
            std::vector<Eigen::VectorXi> btype_;
            std::vector<Eigen::VectorXi> aset_init_, aset_bound_;
            std::vector<Eigen::MatrixXd> W_;

            HCod* hcod_;
            Ehqp_primal* ehpq_primal_;
            Step_length* step_length_;
            Up* up_;

            Eigen::VectorXd x_opt_, y1_, x1_, x0_, y0_;
            int iter_, kcheck_, p_, nh_;

            std::vector<h_structure> h_;
            Eigen::MatrixXd Y_;
            bool _isweighted;

    };
}

#endif


/*
 * fft.cpp
 *
 *  Created on: 13.12.2012
 *      Author: stephaniebayer
 */

#include "fft.h"

#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "functions.h"
#include "multi_expo.h"

#include <NTL/ZZ.h>
#include <NTL/mat_ZZ.h>
NTL_CLIENT

fft::fft() {
	// TODO Auto-generated constructor stub

}

fft::~fft() {
	// TODO Auto-generated destructor stub
}

ZZ fft::r_o_u(ZZ gen, long m, ZZ ord){
//	ZZ ord = H.get_ord();
	ZZ rou;
	ZZ pow;
	ZZ temp;
	long t = 2*m;
	if ((ord-1) % t  == 0)
	{
		temp = (ord-1)/t;
		PowerMod(rou,gen,temp , ord);

			if(GCD(rou,ord)==to_ZZ(1))
			{

				if (t&1)
				{
					PowerMod(pow,rou,t, ord);
					if(pow==1){
						//cout<<"rou: "<<ord<<" "<<rou;
						return rou;
					}
				}
				else
				{
					PowerMod(pow,rou,t/2,ord);
					if(pow == (ord-1))
					{
						//cout<<"rou "<<ord<<" "<<rou;
						return rou;
					}
				}
			}
	}
	else
	{
		cout << "There is no" << 2*m <<"-th root of unity"<< endl;
		return to_ZZ(1);
	}
	return to_ZZ(1);
}

long fft::to_long(vector<int>* bit_r){

	long  t, length;
	double two,i;
	two = 2;

	length =bit_r->size();
	t=0;
	for(i = 0; i<length; i++ ){
		t =t +bit_r->at(i)*pow(two,i);
	}
	return t;
}

void fft::bitreverse(long& z, long x, long d){
	long i;
	vector<int>* temp=0;
	temp = new vector<int>(d);
	for(i = 0; i<d; i++){
		if(bit(x,i)==1){
			temp->at(d-i-1)=1;
		}
		else{
			temp->at(d-i-1)=0;
		}
	}
	z = to_long(temp);
	delete temp;
}

void  fft::brevorder(vector<ZZ>* ret, vector<ZZ>* v){

	long i,k,d,l, lv;
	ZZ temp;

	lv = v->size();
	l=ret->size();
	d = NumBits(l)-1;
	for(i = 0; i<lv; i++){
		ret->at(i)=v->at(i);
	}
	for(i = 0; i<l; i++){
		bitreverse(k,i,d);
		if(i<k){
			temp = ret->at(i);
			ret->at(i) = ret->at(k);
			ret->at(k)= temp;
		}
	}
}

void fft::FFT(vector<ZZ>* fft, vector<ZZ>* v , long N, ZZ rootofunity, ZZ ord){

	int n, m, m2, i, i0, i1, k, l;
	ZZ rho, rr, z;
	double two = 2;
	l= v->size();

	//cout<<fft->size()<< "  "<<N;
	brevorder(fft,v);

    n = NumBits(N)-1;	//(* N = 2**n *)
    for(k= 0; k<n; k++){
    	//cout<<k<<" ";
   		m =  pow(two,k);
    	m2 = 2*m;
    	rr = 1;
    	PowerMod(rho,rootofunity,pow(two, n-k-1),ord);
    //	cout<<m2<<" rho "<<rho<<" "<<m<<" ";
        for (i = 0; i<m; i++){
        	i0=i;
			//cout<<" rr "<<rr;
				while( i0 < N){
					i1=i0+m; //cout<<" : "<<i0<<" "<<i1<<" ";
					MulMod(z, fft->at(i1), rr, ord);        //(* rr = rho**i *)
					//cout<<" z "<<z<<" ";
					SubMod(fft->at(i1),fft->at(i0), z, ord);
					//cout<<fft->at(i0)<<" "<<fft->at(i1)<<" ";
					AddMod(fft->at(i0), fft->at(i0), z, ord);
					//cout<<fft->at(i0)<<" "<<fft->at(i1)<<" ";
					i0 = i0 + m2;
				}
				MulMod(rr ,rr,rho, ord) ;

        }
      //  cout<<endl;
    }

}

void fft::FFTinv(vector<ZZ>* ret, vector<ZZ>* points, long N, ZZ rootofunity, ZZ ord){
long  i;
ZZ s, omega;
	InvMod(omega, rootofunity, ord);
    InvMod(s,to_ZZ(N),ord);
    FFT(ret, points,N,omega,ord);
    for (i = 0; i< N; i++){
        MulMod(ret->at(i), s , ret->at(i), ord);
    }
}

void fft::fft_in(vector<ZZ>* ret, vector<ZZ>* v, ZZ rootofunity, ZZ ord, ZZ mod){
	int nb,t,t2,i,i0,i1,k,l;
	ZZ rho, rr, z;
	double two = 2;
	l = v->size();

	brevorder(ret, v);
	nb = NumBits(l)-1;

	for(k = 0 ;  k<nb; k++){
		t =  pow(two,k);
		t2 = 2*t;
		rr = 1;
		PowerMod(rho,rootofunity,pow(two, nb-k-1),ord);
		for(i = 0; i<t; i ++){
			i0 = i;
			if(rr != 1){
				while (i0 < l){
					i1=i0+t;
					PowerMod(z,ret->at(i1), rr, mod);
					MulMod(ret->at(i1),ret->at(i0), InvMod(z,mod),mod);
					MulMod(ret->at(i0),ret->at(i0), z, mod);
					i0 = i0 + t2;

				}
			}
			else{
				while (i0 < l){
					i1=i0+t;
					z = ret->at(i1);
					MulMod(ret->at(i1),ret->at(i0), InvMod(z,mod),mod);
					MulMod(ret->at(i0),ret->at(i0), z, mod);
					i0 = i0 + t2;

				}
			}
			 MulMod(rr, rr,rho,ord);
		}
	}
}

void fft::fft_sum_in(vector<ZZ>* ret, vector<ZZ>* v, ZZ rootofunity, ZZ ord){
	int nb,t,t2,i,i0,i1,k,l;
	ZZ rho, rr, z;
	double two = 2;
	l = v->size();

	brevorder(ret, v);
	nb = NumBits(l)-1;
	for(k = 0 ;  k<nb; k++){
		t =  pow(two,k);
		t2 = 2*t;
		rr = 1;
		PowerMod(rho,rootofunity,pow(two, nb-k-1),ord);
		for(i = 0; i<t; i ++){
			i0 = i;
			while (i0 < l){
				i1=i0+t;
				MulMod(z,ret->at(i1), rr, ord);
				SubMod(ret->at(i1),ret->at(i0), z, ord);
				AddMod(ret->at(i0),ret->at(i0), z, ord);
				i0 = i0 + t2;

			}
			 MulMod(rr,rr,rho,ord);
		}
	}
}

void fft::fft_mult_cipher(vector<vector<vector<ZZ>* >*>* ret, vector<vector<Cipher_elg>* >* v, ZZ rootofunity, ZZ ord, ZZ mod){

	vector<vector<ZZ>* >* fft = 0;
	vector<ZZ>* temp_u = 0;
	vector<ZZ>* temp_v = 0;
	long i,j, m, n,l;

	n= v->at(0)->size();
	m = v->size();
	l=2*m;

	for(i = 0; i<n; i++){
		fft =new vector<vector<ZZ>* >(2);
		temp_u = new vector<ZZ>(l);
		temp_v = new vector<ZZ>(l);
		temp_u ->at(0) = to_ZZ(1);
		temp_v ->at(0) = to_ZZ(1);
		for(j = 1; j <=m; j++){
			temp_u->at(j)=v->at(j-1)->at(i).get_u();
			temp_v->at(j)=v->at(j-1)->at(i).get_v();
		}
		for(j = m+1; j<l; j++){
			temp_u->at(j) = to_ZZ(1);
			temp_v->at(j) = to_ZZ(1);
		}
		fft->at(0)=new vector<ZZ>(l);
		fft->at(1)=new vector<ZZ>(l);

		fft_in(fft->at(0), temp_u, rootofunity, ord, mod);
		fft_in(fft->at(1), temp_v,rootofunity, ord, mod);
		ret->at(i)=fft;
		delete temp_u;
		delete temp_v;
	}
}

void fft::fft_matrix(vector<vector<ZZ>* >*  ret, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord){

	vector<ZZ>*  fft = 0;
	vector<ZZ>* temp = 0;
	long i,j, m, n,l;
	double two = 2;
	int e;
//	cout<<"in sumT "<< T->size();
	n= T->at(0)->size();
	m = T->size();
	//In the case of the Ciphertexts m is a power of 2, in the normal setting m is odd in our case
	if(m%2==0){
		e = NumBits(2*m+1)-	1;
	}
	else{
		e = NumBits(2*m+1);
	}
	l = pow(two, e);
	for(i = 0; i<n; i++){
		temp = new vector<ZZ>(m);
		for(j = 0; j <m; j++){
			temp->at(j)=T->at(j)->at(i);
		}
		fft=new vector<ZZ>(l);
		FFT(fft, temp, l, rootofunity, ord );
		//fft_sum_in(fft, temp, rootofunity, ord);
		ret->at(i)=fft;
		delete temp;
	}
//	cout<<endl;
}

void fft::fft_matrix_inv(vector<vector<ZZ>* >*  ret, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord){

	vector<ZZ>*  fft = 0;
	vector<ZZ>* temp = 0;
	long i,j, m, n,l;
	double two = 2;
	int e;
	//cout<<"in sumT "<< ret->size();
	//In the case of the Ciphertexts m is a power of 2, in the normal setting m is odd in our case
	n= T->at(0)->size();
	m = T->size();
	if(m%2==0){
		e = NumBits(2*m+1)-	1;
	}
	else{
		e = NumBits(2*m+1);
	}
	l = pow(two, e);
//	cout<<" l ist "<<l<<endl;
	for(i = 0; i<n; i++){
		temp = new vector<ZZ>(m);
		for(j = 0; j <m; j++){
			temp->at(j)=T->at(m-j-1)->at(i);
		}
		fft=new vector<ZZ>(l);
		FFT(fft, temp, l, rootofunity, ord );
		//fft_sum_in(fft, temp, rootofunity, ord);
		ret->at(i)=fft;
		delete temp;
	}
	//cout<<endl;
}

void fft::sum_t(vector<vector<ZZ>*>* ret, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord){
	vector<ZZ>* tem =  0;
	ZZ temp, t;
	long m,n,k,i,j,te,l;
	m = T->size();
	n = T->at(0)->size();
	l=2*m;
	for(k= 0; k<l; k++){
		tem = new vector<ZZ>(n);
		for(j = 0; j<n;j++){
			temp = 0;
			for (i = m-1; i>= 0; i--){
				PowerMod(t,rootofunity, i*(k+1),ord);
				te =fabs(i-m+1);
				MulMod(t,t,T->at(te)->at(j),ord);
				AddMod(temp, temp, t,ord);
			}
			tem->at(j) = temp;
		}
		ret->at(k) = tem;

	}
}

 void fft::calc_Pk(vector<vector<ZZ>*>* ret, vector<vector<Cipher_elg>* >* v, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord, ZZ mod, int omega_sw){
	long j,m,n,l;
	vector<vector<vector<ZZ>* >*>* fft_t=0;
	vector<vector<ZZ>* >* sumT=0;
	ZZ temp_u, te_u, temp_v, te_v,temp ;
	m = T->size();
	n= T->at(0)->size();
	vector<ZZ>* ret_u = new vector<ZZ>(2*m);
	vector<ZZ>* ret_v = new vector<ZZ>(2*m);

	fft_t = new vector<vector<vector<ZZ>* >*>(n);
	fft::fft_mult_cipher(fft_t, v,rootofunity, ord, mod);

	sumT = new vector<vector<ZZ>* >(n);
	fft::fft_matrix_inv(sumT, T, rootofunity, ord);

	l=2*m-1;
	for (j = 0; j<l; j++){

		multi_expo::multi_expo_LL(temp_u,fft_t, sumT, omega_sw ,j+1,0);
		multi_expo::multi_expo_LL(temp_v,fft_t, sumT, omega_sw ,j+1,1);
		ret_u->at(j) = temp_u;
		ret_v->at(j) = temp_v;
	}

	multi_expo::multi_expo_LL(temp_u ,fft_t, sumT, omega_sw ,0,0);
	multi_expo::multi_expo_LL(temp_v ,fft_t, sumT, omega_sw ,0,1);
	ret_u->at(l) = temp_u;
	ret_v->at(l) = temp_v;

	ret->at(0) = ret_u;
	ret->at(1) = ret_v;

	Functions::delete_vector(fft_t);
	Functions::delete_vector(sumT);

}

void fft::calc_m( vector<vector<ZZ>*>* M , long m, ZZ rootofunity, ZZ ord){
	vector<ZZ>* Rootofunity;
	vector<ZZ>* div;
	ZZ prod,temp,temp_1;
	long i,j,l;
	l=2*m;
	vector<ZZ>* Rootofun = new vector<ZZ>(l);
	div =new vector<ZZ>(l);

	Rootofun->at(0) = rootofunity;
	for(i = 1; i<l; i++){
		MulMod(Rootofun->at(i), Rootofun->at(i-1), rootofunity,ord);
	}
	for(i = 0 ; i<l; i++){
		prod = 1;
		for(j = 0; j<l; j++){
			if (i!=j){
				MulMod(prod,prod, SubMod(Rootofun->at(i),Rootofun->at(j),ord),ord);
			}
		}
		div->at(i)= prod;
	}
	for(i = 0; i<l; i++){
		Rootofunity = new vector<ZZ>(l);
		Rootofunity->at(2*m-1) = to_ZZ(1);
		PowerMod(temp,rootofunity, i+1,ord);

		for(j = l-2; j>=0; j--){
			 MulMod(Rootofunity->at(j),Rootofunity->at(j+1),temp,ord);
		}
		for(j = l-1; j>=0; j--){
			 MulMod(Rootofunity->at(j),Rootofunity->at(j), InvMod(div->at(i),ord),ord);
		}
		M->at(i) = Rootofunity;
	}
	delete Rootofun;
	delete div;
}


// source: http://dlib.net/dnn_semantic_segmentation_ex.cpp.html

#include <neuralnet/neuralnet.h>
#include <neuralnet/dnn_semantic_segmentation_ex.h>
#include <logger/logger.h>

#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

#include <numeric> // std::iota
#include <random>
#include <iostream>
#include <dlib/data_io.h>
#include <dlib/image_transforms.h>
#include <dlib/dir_nav.h>
#include <dlib/opencv.h>
#include <iterator>
#include <thread>

#include <dlib/gui_widgets.h>

using namespace std;
using namespace dlib;

neuralnet::neuralnet()
{
}

double neuralnet::train(std::vector<std::tuple<std::string,std::string>> image_label_tuples, int epochs){
    const auto listing = get_listing(image_label_tuples);
    Logger::getInstance().log("images in dataset: " + std::to_string( listing.size()));
    if (listing.size() == 0)
    {
        cout << "Didn't find the VOC2012 dataset. " << endl;
        return 0.0;
    }

    net_type net;
    dnn_trainer<net_type> trainer(net,sgd(weight_decay, momentum));
    trainer.be_verbose();
    trainer.set_learning_rate(initial_learning_rate);
    trainer.set_synchronization_file("trainer_state_file.dat", std::chrono::seconds(20));
    // This threshold is probably excessively large.
    trainer.set_iterations_without_progress_threshold(5000);
    // Since the progress threshold is so large might as well set the batch normalization
    // stats window to something big too.
    set_all_bn_running_stats_window_sizes(net, 1000);

    // Output training parameters.
    cout << endl << trainer << endl;

    std::vector<matrix<rgb_pixel>> samples;
    std::vector<matrix<uint16_t>> labels;

    // Start a bunch of threads that read images from disk and pull out random crops.  It's
    // important to be sure to feed the GPU fast enough to keep it busy.  Using multiple
    // thread for this kind of data preparation helps us do that.  Each thread puts the
    // crops into the data queue.
    dlib::pipe<training_sample> data(200);
    auto f = [&data, &listing](time_t seed)
    {
        dlib::rand rnd(time(0)+seed);
        matrix<rgb_pixel> input_image;
        //matrix<rgb_pixel> rgb_label_image;
        matrix<uint16_t> index_label_image;
        training_sample temp;
        while(data.is_enabled())
        {
            // Pick a random input image.
            const image_info& image_info = listing[rnd.get_random_32bit_number()%listing.size()];

            // Load the input image.
            load_image(input_image, image_info.image_filename);

            //dlib::matrix<rgb_pixel> resized_input_image;
            //resized_input_image.set_size(input_tile_height, input_tile_width);
            //dlib::resize_image(input_image, resized_input_image, dlib::interpolate_nearest_neighbor());
            //input_image = resized_input_image;
            //input_image.set_size(input_tile_height, input_tile_width);

            // Load the ground-truth (RGB) labels.
            //load_image(rgb_label_image, image_info.label_filename);
            load_image(index_label_image, image_info.label_filename);

            //dlib::matrix<uint16_t> resized_index_label_image;
            //resized_index_label_image.set_size(input_tile_height, input_tile_width);
            //dlib::resize_image(index_label_image, resized_index_label_image, dlib::interpolate_nearest_neighbor());
            //index_label_image = resized_index_label_image;
            //index_label_image.set_size(input_tile_height, input_tile_width);

            // Convert the indexes to RGB values.
            //neuralnet::rgb_label_image_to_index_label_image(rgb_label_image, index_label_image);

            // Randomly pick a part of the image.
            neuralnet::randomly_crop_image(input_image, index_label_image, temp, rnd);

            // no crop -> overfit
            //temp.input_image = resized_input_image;
            //temp.label_image = resized_index_label_image;

            // Push the result to be used by the trainer.
            data.enqueue(temp);
        }
    };
    std::thread data_loader1([f](){ f(1); });
    std::thread data_loader2([f](){ f(2); });
    //std::thread data_loader3([f](){ f(3); });
    //std::thread data_loader4([f](){ f(4); });
    unsigned int epoch = 0;
    // The main training loop.  Keep making mini-batches and giving them to the trainer.
    // We will run until the learning rate has dropped by a factor of 1e-4.
    while(trainer.get_learning_rate() >= 1e-4 &&
          epoch++ < epochs)
    {
        Logger::getInstance().log("epoch #" + std::to_string(epoch));
        samples.clear();
        labels.clear();

        // make a 30-image mini-batch
        training_sample temp;
        while(samples.size() < minibatch_size)
        {
            data.dequeue(temp);

            samples.push_back(std::move(temp.input_image));
            labels.push_back(std::move(temp.label_image));
        }

        trainer.train_one_step(samples, labels);
    }

    // Training done, tell threads to stop and make sure to wait for them to finish before
    // moving on.
    data.disable();
    data_loader1.join();
    data_loader2.join();
    //data_loader3.join();
    //data_loader4.join();

    // also wait for threaded processing to stop in the trainer.
    trainer.get_net();

    net.clean();

    Logger::getInstance().log("saving network");
    serialize("semantic_segmentation.dnn") << net;

    // Make a copy of the network to use it for inference.
    anet_type anet = net;

    Logger::getInstance().log( "Testing the network...");
    // Find the accuracy of the newly trained network on both the training and the validation sets.
    double accuracy = calculate_accuracy(anet, get_listing(image_label_tuples));

    Logger::getInstance().log( "train accuracy  :  " + std::to_string(accuracy));
    return accuracy;
}

cv::Mat neuralnet::predict(std::string imagefile)
{
    matrix<rgb_pixel> input_image;
    load_image(input_image, imagefile);

    return predict(input_image);
}

cv::Mat neuralnet::predict(cv::Mat image)
{
    matrix<rgb_pixel> input_image;
    dlib::assign_image(input_image, cv_image<rgb_pixel>(image));
    return predict(input_image);
}

cv::Mat neuralnet::predict(dlib::matrix<rgb_pixel> input_image)
{
    int rows = input_image.nr();
    int cols = input_image.nc();
    // Read the file containing the trained network from the working directory.
    if(!netLoaded){

        deserialize("semantic_segmentation.dnn") >> net;
        netLoaded = true;
    }

    dlib::matrix<rgb_pixel> resized_input_image;
    resized_input_image.set_size(2*input_tile_height, 2*input_tile_width);
    dlib::resize_image(input_image, resized_input_image, dlib::interpolate_bilinear());
    input_image = resized_input_image;

    matrix<uint16_t> index_label_image;
    matrix<rgb_pixel> rgb_label_image;

    // Create predictions for each pixel. At this point, the type of each prediction
    // is an index (a value between 0 and 20). Note that the net may return an image
    // that is not exactly the same size as the input.
    const matrix<uint16_t> temp = net(input_image);

    // Crop the returned image to be exactly the same size as the input.
    const chip_details chip_details(
                centered_rect(temp.nc() / 2, temp.nr() / 2, input_image.nc(), input_image.nr()),
                chip_dims(input_image.nr(), input_image.nc())
                );
    extract_image_chip(temp, chip_details, index_label_image, interpolate_nearest_neighbor());

    //dlib::matrix<uint16_t> resized_index_label_image;
    //resized_index_label_image.set_size(rows, cols);
    //dlib::resize_image(index_label_image, resized_index_label_image, dlib::interpolate_nearest_neighbor());
    //index_label_image = resized_index_label_image;

    // Convert the indexes to RGB values.
   //neuralnet::index_label_image_to_rgb_label_image(index_label_image, rgb_label_image);

    // Show the input image on the left, and the predicted RGB labels on the right.
    //image_window win;
    //win.set_image(join_rows(input_image, rgb_label_image));

    // Find the most prominent class label from amongst the per-pixel predictions.
    //const std::string classlabel = get_most_prominent_non_background_classlabel(index_label_image);
    //std::cout << classlabel << std::endl;
    return dlib::toMat(index_label_image);
}

dlib::rectangle neuralnet::make_random_cropping_rect_resnet(const dlib::matrix<dlib::rgb_pixel> &img, dlib::rand &rnd)
{
    // figure out what rectangle we want to crop from the image
    double mins = 0.466666666, maxs = 0.875;
    auto scale = mins + rnd.get_random_double()*(maxs-mins);
    auto size = scale*std::min(img.nr(), img.nc());
    rectangle rect(size, size);
    // randomly shift the box around
    point offset(rnd.get_random_32bit_number()%(img.nc()-rect.width()),
                 rnd.get_random_32bit_number()%(img.nr()-rect.height()));
    return move_rect(rect, offset);
}

void neuralnet::randomly_crop_image(const dlib::matrix<rgb_pixel> &input_image, const dlib::matrix<uint16_t> &label_image, neuralnet::training_sample &crop, dlib::rand &rnd)
{
    const auto rect = make_random_cropping_rect_resnet(input_image, rnd);

    const chip_details chip_details(rect, chip_dims(neuralnet::input_tile_height, neuralnet::input_tile_width));

    // Crop the input image.
    extract_image_chip(input_image, chip_details, crop.input_image, interpolate_bilinear());

    // Crop the labels correspondingly. However, note that here bilinear
    // interpolation would make absolutely no sense - you wouldn't say that
    // a bicycle is half-way between an aeroplane and a bird, would you?
    extract_image_chip(label_image, chip_details, crop.label_image, interpolate_nearest_neighbor());

    // Also randomly flip the input image and the labels.
    if (rnd.get_random_double() > 0.5)
    {
        crop.input_image = fliplr(crop.input_image);
        crop.label_image = fliplr(crop.label_image);
    }

    // And then randomly adjust the colors.
    apply_random_color_offset(crop.input_image, rnd);
}

std::vector<neuralnet::image_info> neuralnet::get_listing(std::vector<std::tuple<string, string> > image_label_tuples)
{
    std::vector<image_info> results;
    for(auto tup: image_label_tuples){
        image_info image_info;
        image_info.image_filename = std::get<0>(tup);
        image_info.label_filename = std::get<1>(tup);
        results.push_back(image_info);
    }
    return results;
}

const Voc2012class &neuralnet::find_index_class(const uint16_t &index_label)
{
    return find_voc2012_class(
                [&index_label](const Voc2012class& voc2012class)
    {
        return index_label == voc2012class.index;
    }
    );
}

const Voc2012class &neuralnet::find_label_class(const dlib::rgb_pixel &rgb_label)
{
    return find_voc2012_class(
                [&rgb_label](const Voc2012class& voc2012class)
    {
        return rgb_label == voc2012class.rgb_label;
    }
    );

}

void neuralnet::rgb_label_image_to_index_label_image(const dlib::matrix<rgb_pixel> &rgb_label_image, dlib::matrix<uint16_t> &index_label_image)
{
    const long nr = rgb_label_image.nr();
    const long nc = rgb_label_image.nc();

    index_label_image.set_size(nr, nc);

    for (long r = 0; r < nr; ++r)
    {
        for (long c = 0; c < nc; ++c)
        {
            index_label_image(r, c) = neuralnet::rgb_label_to_index_label(rgb_label_image(r, c));
        }
    }
}

void neuralnet::index_label_image_to_rgb_label_image(const dlib::matrix<uint16_t> &index_label_image, dlib::matrix<rgb_pixel> &rgb_label_image)
{
    const long nr = index_label_image.nr();
    const long nc = index_label_image.nc();

    rgb_label_image.set_size(nr, nc);

    for (long r = 0; r < nr; ++r)
    {
        for (long c = 0; c < nc; ++c)
        {
            rgb_label_image(r, c) = index_label_to_rgb_label(index_label_image(r, c));
        }
    }
}

double neuralnet::calculate_accuracy(anet_type &anet, const std::vector<neuralnet::image_info> &dataset)
{

    int num_right = 0;
    int num_wrong = 0;

    matrix<rgb_pixel> input_image;
    matrix<rgb_pixel> rgb_label_image;
    matrix<uint16_t> index_label_image;
    matrix<uint16_t> net_output;

    for (const auto& image_info : dataset)
    {
        // Load the input image.
        load_image(input_image, image_info.image_filename);

        dlib::matrix<rgb_pixel> resized_input_image;
        resized_input_image.set_size(input_tile_height, input_tile_width);
        dlib::resize_image(input_image, resized_input_image, dlib::interpolate_nearest_neighbor());
        input_image = resized_input_image;
        //input_image.set_size(input_tile_height, input_tile_width);

        // Load the ground-truth (RGB) labels.
        //load_image(rgb_label_image, image_info.label_filename);
        load_image(index_label_image, image_info.label_filename);

        dlib::matrix<uint16_t> resized_index_label_image;
        resized_index_label_image.set_size(input_tile_height, input_tile_width);
        dlib::resize_image(index_label_image, resized_index_label_image, dlib::interpolate_nearest_neighbor());
        index_label_image = resized_index_label_image;
        //rgb_label_image.set_size(input_tile_height, input_tile_width);

        // Create predictions for each pixel. At this point, the type of each prediction
        // is an index (a value between 0 and 20). Note that the net may return an image
        // that is not exactly the same size as the input.
        const matrix<uint16_t> temp = anet(input_image);

        // Convert the indexes to RGB values.
        //rgb_label_image_to_index_label_image(rgb_label_image, index_label_image);

        // Crop the net output to be exactly the same size as the input.
        const chip_details chip_details(
                    centered_rect(temp.nc() / 2, temp.nr() / 2, input_image.nc(), input_image.nr()),
                    chip_dims(input_image.nr(), input_image.nc())
                    );
        extract_image_chip(temp, chip_details, net_output, interpolate_nearest_neighbor());

        const long nr = index_label_image.nr();
        const long nc = index_label_image.nc();

        // Compare the predicted values to the ground-truth values.
        for (long r = 0; r < nr; ++r)
        {
            for (long c = 0; c < nc; ++c)
            {
                const uint16_t truth = index_label_image(r, c);
                if (truth != dlib::loss_multiclass_log_per_pixel_::label_to_ignore)
                {
                    const uint16_t prediction = net_output(r, c);
                    if (prediction == truth)
                    {
                        ++num_right;
                    }
                    else
                    {
                        ++num_wrong;
                    }
                }
            }
        }
    }

    // Return the accuracy estimate.
    return num_right / static_cast<double>(num_right + num_wrong);
}

string neuralnet::get_most_prominent_non_background_classlabel(const dlib::matrix<uint16_t> &index_label_image)
{
    const long nr = index_label_image.nr();
    const long nc = index_label_image.nc();

    std::vector<unsigned int> counters(class_count);

    for (long r = 0; r < nr; ++r)
    {
        for (long c = 0; c < nc; ++c)
        {
            const uint16_t label = index_label_image(r, c);
            ++counters[label];
        }
    }

    const auto max_element = std::max_element(counters.begin() + 1, counters.end());
    const uint16_t most_prominent_index_label = max_element - counters.begin();

    return neuralnet::find_index_class(most_prominent_index_label).classlabel;
}


#define BOOST_TEST_MODULE test1

#ifndef BOOST_TEST_DYN_LINK
#define BOOST_TEST_DYN_LINK
#endif

#define BOOST_TEST_MAIN

#define BOOST_TEST_ALTERNATIVE_INIT_API
#include <boost/test/unit_test.hpp>

#include "tut/ch4.hpp"

BOOST_AUTO_TEST_SUITE(CH4_TEST_SUITE)

BOOST_AUTO_TEST_CASE(area_of_a_pentagon_test)
{

  BOOST_TEST(tut::ch4::exe_4_01(5.5), 71.92);
}

BOOST_AUTO_TEST_SUITE_END()


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Row;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenRowVecGenDouble : public Expected {
    public:
      ExpectedInPlaceGenRowVecGenDouble() {
        cout << "Compute ExpectedInPlaceGenRowVecGenDouble(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenRowVec,
            InputClass::GenDouble
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genRowVec = *static_cast<Row<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _genDouble = *static_cast<double*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenRowVec = _genRowVec;
            _copyOfGenDouble = _genDouble;

			expectedRowVecFill();

			_genRowVec = _copyOfGenRowVec;
		    _genDouble = _copyOfGenDouble;
            expectedRowInPlacePlus();

            _genRowVec = _copyOfGenRowVec;
            _genDouble = _copyOfGenDouble;
            expectedRowInPlaceMinus();

			_genRowVec = _copyOfGenRowVec;
			_genDouble = _copyOfGenDouble;
			expectedRowInPlaceTimes();

			_genRowVec = _copyOfGenRowVec;
            _genDouble = _copyOfGenDouble;
            expectedRowInPlaceDivide();

		  }

          cout << "done." << endl;
        }

    protected:
      Row<double> _genRowVec;
      Row<double> _copyOfGenRowVec;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedRowVecFill() {
        cout << "- Compute expectedRowVecFill() ... ";

        _genRowVec.fill(_genDouble);
        save<double>("Row.fill", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlacePlus() {
        cout << "- Compute expectedRowInPlacePlus() ... ";

        _genRowVec += _genDouble;
        save<double>("Row.inPlacePlus", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlaceMinus() {
        cout << "- Compute expectedRowInPlaceMinus() ... ";

        _genRowVec -= _genDouble;
        save<double>("Row.inPlaceMinus", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlaceTimes() {
        cout << "- Compute expectedRowInPlaceTimes() ... ";

        _genRowVec *= _genDouble;
        save<double>("Row.inPlaceTimes", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlaceDivide() {
        cout << "- Compute expectedRowInPlaceDivide() ... ";

        _genRowVec /= _genDouble;
        save<double>("Row.inPlaceDivide", _genRowVec);

        cout << "done." << endl;
            }

  };
}


// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * 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.
//
// 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.

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

#ifdef G2O_USE_VENDORED_CERES
#include "g2o/EXTERNAL/ceres/autodiff.h"
#else
#include <ceres/internal/autodiff.h>
#endif

#include "g2o/core/auto_differentiation.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/solver.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/stuff/command_args.h"

#if defined G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#else
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#endif

using namespace std;

namespace g2o {
namespace bal {
using Vector9 = VectorN<9>;
}
}  // namespace g2o

/**
 * \brief camera vertex which stores the parameters for a pinhole camera
 *
 * The parameters of the camera are
 * - rx,ry,rz representing the rotation axis, whereas the angle is given by
 * ||(rx,ry,rz)||
 * - tx,ty,tz the translation of the camera
 * - f the focal length of the camera
 * - k1, k2 two radial distortion parameters
 */
class VertexCameraBAL : public g2o::BaseVertex<9, g2o::bal::Vector9> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexCameraBAL() {}

  virtual bool read(std::istream& /*is*/) {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual void setToOriginImpl() {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
  }

  virtual void oplusImpl(const double* update) {
    g2o::bal::Vector9::ConstMapType v(update, VertexCameraBAL::Dimension);
    _estimate += v;
  }
};

/**
 * \brief 3D world feature
 *
 * A 3D point feature in the world
 */
class VertexPointBAL : public g2o::BaseVertex<3, g2o::Vector3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexPointBAL() {}

  virtual bool read(std::istream& /*is*/) {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual void setToOriginImpl() {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
  }

  virtual void oplusImpl(const double* update) {
    g2o::Vector3::ConstMapType v(update);
    _estimate += v;
  }
};

/**
 * \brief edge representing the observation of a world feature by a camera
 *
 * see: http://grail.cs.washington.edu/projects/bal/
 * We use a pinhole camera model; the parameters we estimate for each camera
 * area rotation R, a translation t, a focal length f and two radial distortion
 * parameters k1 and k2. The formula for projecting a 3D point X into a camera
 * R,t,f,k1,k2 is:
 * P  =  R * X + t     (conversion from world to camera coordinates)
 * p  = -P / P.z       (perspective division)
 * p' =  f * r(p) * p  (conversion to pixel coordinates) where P.z is the third
 * (z) coordinate of P.
 *
 * In the last equation, r(p) is a function that computes a scaling factor to
 * undo the radial distortion: r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4.
 *
 * This gives a projection in pixels, where the origin of the image is the
 * center of the image, the positive x-axis points right, and the positive
 * y-axis points up (in addition, in the camera coordinate system, the positive
 * z-axis points backwards, so the camera is looking down the negative z-axis,
 * as in OpenGL).
 */
class EdgeObservationBAL
    : public g2o::BaseBinaryEdge<2, g2o::Vector2, VertexCameraBAL,
                                 VertexPointBAL> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  EdgeObservationBAL() {}
  virtual bool read(std::istream& /*is*/) {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }
  virtual bool write(std::ostream& /*os*/) const {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  /**
   * templatized function to compute the error as described in the comment above
   */
  template <typename T>
  bool operator()(const T* p_camera, const T* p_point, T* p_error) const {
    typename g2o::VectorN<9, T>::ConstMapType camera(p_camera);
    typename g2o::VectorN<3, T>::ConstMapType point(p_point);

    typename g2o::VectorN<3, T> p;

    // Rodrigues' formula for the rotation
    T theta = camera.template head<3>().norm();
    if (theta > T(0)) {
      g2o::VectorN<3, T> v = camera.template head<3>() / theta;
      T cth = cos(theta);
      T sth = sin(theta);

      g2o::VectorN<3, T> vXp = v.cross(point);
      T vDotp = v.dot(point);
      T oneMinusCth = T(1) - cth;

      p = point * cth + vXp * sth + v * vDotp * oneMinusCth;
    } else {
      // taylor expansion for theta close to zero
      p = point + camera.template head<3>().cross(point);
    }

    // translation of the camera
    p += camera.template segment<3>(3);

    // perspective division
    g2o::VectorN<2, T> projectedPoint = -p.template head<2>() / p(2);

    // conversion to pixel coordinates
    T radiusSqr = projectedPoint.squaredNorm();
    const T& f = camera(6);
    const T& k1 = camera(7);
    const T& k2 = camera(8);
    T r_p = T(1) + k1 * radiusSqr + k2 * radiusSqr * radiusSqr;
    g2o::VectorN<2, T> prediction = f * r_p * projectedPoint;

    // compute the error
    typename g2o::VectorN<2, T>::MapType error(p_error);
    error = prediction - measurement().cast<T>();
    (void)error;
    return true;
  }

  G2O_MAKE_AUTO_AD_FUNCTIONS
};

int main(int argc, char** argv) {
  int maxIterations;
  bool verbose;
  bool usePCG;
  string outputFilename;
  string inputFilename;
  string statsFilename;
  g2o::CommandArgs arg;
  arg.param("i", maxIterations, 5, "perform n iterations");
  arg.param("o", outputFilename, "", "write points into a vrml file");
  arg.param("pcg", usePCG, false, "use PCG instead of the Cholesky");
  arg.param("v", verbose, false, "verbose output of the optimization process");
  arg.param("stats", statsFilename, "", "specify a file for the statistics");
  arg.paramLeftOver("graph-input", inputFilename, "",
                    "file which will be processed");

  arg.parseArgs(argc, argv);

  typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BalBlockSolver;
#ifdef G2O_HAVE_CHOLMOD
  string choleskySolverName = "CHOLMOD";
  typedef g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>
      BalLinearSolver;
#else
  string choleskySolverName = "Eigen";
  typedef g2o::LinearSolverEigen<BalBlockSolver::PoseMatrixType>
      BalLinearSolver;
#endif
  typedef g2o::LinearSolverPCG<BalBlockSolver::PoseMatrixType>
      BalLinearSolverPCG;

  g2o::SparseOptimizer optimizer;
  std::unique_ptr<g2o::LinearSolver<BalBlockSolver::PoseMatrixType>>
      linearSolver;
  if (usePCG) {
    cout << "Using PCG" << endl;
    linearSolver = g2o::make_unique<BalLinearSolverPCG>();
  } else {
    cout << "Using Cholesky: " << choleskySolverName << endl;
    auto cholesky = g2o::make_unique<BalLinearSolver>();
    cholesky->setBlockOrdering(true);
    linearSolver = std::move(cholesky);
  }
  g2o::OptimizationAlgorithmLevenberg* solver =
      new g2o::OptimizationAlgorithmLevenberg(
          g2o::make_unique<BalBlockSolver>(std::move(linearSolver)));

  // solver->setUserLambdaInit(1);
  optimizer.setAlgorithm(solver);
  if (statsFilename.size() > 0) {
    optimizer.setComputeBatchStatistics(true);
  }

  vector<VertexPointBAL*> points;
  vector<VertexCameraBAL*> cameras;

  // parse BAL dataset
  cout << "Loading BAL dataset " << inputFilename << endl;
  {
    ifstream ifs(inputFilename.c_str());
    int numCameras, numPoints, numObservations;
    ifs >> numCameras >> numPoints >> numObservations;

    cerr << PVAR(numCameras) << " " << PVAR(numPoints) << " "
         << PVAR(numObservations) << endl;

    int id = 0;
    cameras.reserve(numCameras);
    for (int i = 0; i < numCameras; ++i, ++id) {
      VertexCameraBAL* cam = new VertexCameraBAL;
      cam->setId(id);
      optimizer.addVertex(cam);
      cameras.push_back(cam);
    }

    points.reserve(numPoints);
    for (int i = 0; i < numPoints; ++i, ++id) {
      VertexPointBAL* p = new VertexPointBAL;
      p->setId(id);
      p->setMarginalized(true);
      bool addedVertex = optimizer.addVertex(p);
      if (!addedVertex) {
        cerr << "failing adding vertex" << endl;
      }
      points.push_back(p);
    }

    // read in the observation
    for (int i = 0; i < numObservations; ++i) {
      int camIndex, pointIndex;
      double obsX, obsY;
      ifs >> camIndex >> pointIndex >> obsX >> obsY;

      assert(camIndex >= 0 && (size_t)camIndex < cameras.size() &&
             "Index out of bounds");
      VertexCameraBAL* cam = cameras[camIndex];
      assert(pointIndex >= 0 && (size_t)pointIndex < points.size() &&
             "Index out of bounds");
      VertexPointBAL* point = points[pointIndex];

      EdgeObservationBAL* e = new EdgeObservationBAL;
      e->setVertex(0, cam);
      e->setVertex(1, point);
      e->setInformation(g2o::Matrix2::Identity());
      e->setMeasurement(g2o::Vector2(obsX, obsY));
      bool addedEdge = optimizer.addEdge(e);
      if (!addedEdge) {
        cerr << "error adding edge" << endl;
      }
    }

    // read in the camera params
    for (int i = 0; i < numCameras; ++i) {
      g2o::bal::Vector9 cameraParameter;
      for (int j = 0; j < 9; ++j) ifs >> cameraParameter(j);
      VertexCameraBAL* cam = cameras[i];
      cam->setEstimate(cameraParameter);
    }

    // read in the points
    for (int i = 0; i < numPoints; ++i) {
      g2o::Vector3 p;
      ifs >> p(0) >> p(1) >> p(2);
      VertexPointBAL* point = points[i];
      point->setEstimate(p);
    }
  }
  cout << "done." << endl;

  cout << "Initializing ... " << flush;
  optimizer.initializeOptimization();
  cout << "done." << endl;
  optimizer.setVerbose(verbose);
  cout << "Start to optimize" << endl;
  optimizer.optimize(maxIterations);

  if (statsFilename != "") {
    cerr << "writing stats to file \"" << statsFilename << "\" ... ";
    ofstream fout(statsFilename.c_str());
    const g2o::BatchStatisticsContainer& bsc = optimizer.batchStatistics();
    for (size_t i = 0; i < bsc.size(); i++) fout << bsc[i] << endl;
    cerr << "done." << endl;
  }

  // dump the points
  if (outputFilename.size() > 0) {
    ofstream fout(outputFilename.c_str());  // loadable with meshlab
    fout << "#VRML V2.0 utf8\n"
         << "Shape {\n"
         << "  appearance Appearance {\n"
         << "    material Material {\n"
         << "      diffuseColor " << 1 << " " << 0 << " " << 0 << "\n"
         << "      ambientIntensity 0.2\n"
         << "      emissiveColor 0.0 0.0 0.0\n"
         << "      specularColor 0.0 0.0 0.0\n"
         << "      shininess 0.2\n"
         << "      transparency 0.0\n"
         << "    }\n"
         << "  }\n"
         << "  geometry PointSet {\n"
         << "    coord Coordinate {\n"
         << "      point [\n";
    for (vector<VertexPointBAL*>::const_iterator it = points.begin();
         it != points.end(); ++it) {
      fout << (*it)->estimate().transpose() << endl;
    }
    fout << "    ]\n"
         << "  }\n"
         << "}\n"
         << "  }\n";
  }

  return 0;
}


/* Copyright 2022 Zuru Tech HK Limited.
 *
 * 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 <filesystem>

#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <petscmat.h>
#include <petscvec.h>

#include <solvers/SparseSystem.hpp>

namespace {

namespace fs = std::filesystem;

TEST(SparseSystemTest, TestLoadNpz)
{
    const fs::path datasets_dir(DATASETS_DIR);
    const fs::path dataset_dir = datasets_dir / "stand_small_test";
    fs::path npz_path;
    for (const fs::path& entry : fs::directory_iterator(dataset_dir)) {
        if (entry.extension() == ".npz") {
            npz_path = entry;
            break;
        }
    }
    solvers::SparseSystem system = solvers::SparseSystem::loadNpz(npz_path);
    auto [A, b] = system.toEigenCSR();
    Eigen::VectorXd x = system.getEigenSolution();
    constexpr double tolerance = 1e-6;
    ASSERT_TRUE(b.isApprox(A * x, tolerance));
}

TEST(SparseSystemTest, TestSaveNpz)
{
    constexpr size_t n = 4;
    std::vector<Eigen::Triplet<double>> upper_A_entries = {
        Eigen::Triplet<double>(0, 0, 1.),  Eigen::Triplet<double>(0, 1, -2.),
        Eigen::Triplet<double>(0, 3, 1.5), Eigen::Triplet<double>(1, 1, 4.),
        Eigen::Triplet<double>(1, 2, -1.), Eigen::Triplet<double>(2, 3, -3.),
        Eigen::Triplet<double>(3, 3, 0.5),
    };
    Eigen::SparseMatrix<double, Eigen::RowMajor> upper_A(n, n);
    upper_A.setFromTriplets(upper_A_entries.begin(), upper_A_entries.end());
    upper_A.makeCompressed();
    Eigen::Vector4d x(-0.5, 1., 2., -2.);
    Eigen::SparseMatrix<double, Eigen::RowMajor> A(
        upper_A.selfadjointView<Eigen::Upper>());
    Eigen::Vector4d b = A * x;
    solvers::SparseSystem system = solvers::SparseSystem(upper_A, b, x);

    fs::path npz_path = fs::temp_directory_path() / "test.npz";
    system.saveNpz(npz_path);
    solvers::SparseSystem loaded_system =
        solvers::SparseSystem::loadNpz(npz_path);
    auto [loaded_upper_A, loaded_b] = system.toEigenUpperCSR();
    Eigen::VectorXd loaded_x = system.getEigenSolution();
    ASSERT_TRUE(b.isApprox(loaded_b));
    ASSERT_TRUE(x.isApprox(loaded_x));
    ASSERT_TRUE(upper_A.isApprox(loaded_upper_A));
}

TEST(SparseSystemTest, TestToPetsc)
{
    constexpr double tolerance = 1e-6;

    fs::path datasets_dir(DATASETS_DIR);
    fs::path dataset_dir = datasets_dir / "stand_small_test";
    fs::path npz_path;
    for (const std::filesystem::path& entry :
         std::filesystem::directory_iterator(dataset_dir)) {
        if (entry.extension() == ".npz") {
            npz_path = entry;
            break;
        }
    }
    solvers::SparseSystem system = solvers::SparseSystem::loadNpz(npz_path);

    auto n = static_cast<int>(system.dim());
    auto [A, b] = system.toPetscCSR();
    double b_norm = 1.;
    VecNorm(b, NormType::NORM_2, &b_norm);
    Vec x = system.getPetscSolution();
    Vec r;
    double r_norm = b_norm;
    VecCreateSeq(PETSC_COMM_SELF, n, &r);
    MatMult(A, x, r);
    VecAXPY(r, -1, b);
    VecNorm(r, NormType::NORM_2, &r_norm);
    ASSERT_TRUE(r_norm < tolerance * b_norm);
}

}    // namespace


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * 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.
*   * Neither the name of the Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Ioan Sucan */

#define BOOST_TEST_MODULE "State"
#include <boost/test/unit_test.hpp>
#include <thread>
#include <iostream>

#include "ompl/base/ScopedState.h"
#include "ompl/base/spaces/SE3StateSpace.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/Time.h"

using namespace ompl;

// define a convenience macro
#define BOOST_OMPL_EXPECT_NEAR(a, b, diff) BOOST_CHECK_SMALL((a) - (b), diff)

BOOST_AUTO_TEST_CASE(Scoped)
{
    auto mSE3(std::make_shared<base::SE3StateSpace>());

    base::RealVectorBounds b(3);
    b.setLow(0);
    b.setHigh(1);
    mSE3->setBounds(b);
    mSE3->setup();

    auto mC0(std::make_shared<base::CompoundStateSpace>());
    mC0->addSubspace(mSE3, 1.0);
    mC0->setup();

    auto mC1(std::make_shared<base::CompoundStateSpace>());
    mC1->addSubspace(mC0, 1.0);
    mC1->setup();

    auto mC2(std::make_shared<base::CompoundStateSpace>());
    mC2->addSubspace(mSE3->getSubspace(1), 1.0);
    mC2->addSubspace(mSE3->getSubspace(0), 1.0);
    mC2->setup();

    base::ScopedState<base::SE3StateSpace> sSE3(mSE3);
    base::ScopedState<base::RealVectorStateSpace> sSE3_R(mSE3->getSubspace(0));
    base::ScopedState<base::SO3StateSpace> sSE3_SO2(mSE3->getSubspace(1));
    base::ScopedState<base::CompoundStateSpace> sC0(mC0);
    base::ScopedState<> sC1(mC1);
    base::ScopedState<> sC2(mC2);

    sSE3.random();

    sSE3 >> sSE3_SO2;

    BOOST_CHECK_EQUAL(sSE3->rotation().x, sSE3_SO2->x);
    BOOST_CHECK_EQUAL(sSE3->rotation().y, sSE3_SO2->y);
    BOOST_CHECK_EQUAL(sSE3->rotation().z, sSE3_SO2->z);
    BOOST_CHECK_EQUAL(sSE3->rotation().w, sSE3_SO2->w);

    base::ScopedState<> sSE3_copy(mSE3);
    sSE3_copy << sSE3;
    BOOST_CHECK_EQUAL(sSE3_copy, sSE3);
    sSE3 >> sSE3_copy;
    BOOST_CHECK_EQUAL(sSE3_copy, sSE3);

    sSE3_R << sSE3_copy;

    BOOST_CHECK_EQUAL(sSE3->getX(), sSE3_R->values[0]);
    BOOST_CHECK_EQUAL(sSE3->getY(), sSE3_R->values[1]);
    BOOST_CHECK_EQUAL(sSE3->getZ(), sSE3_R->values[2]);

    sSE3_SO2 >> sC1;
    sC1 << sSE3_R;

    sC1 >> sC0;
    sSE3_copy = sC0->components[0];
    BOOST_CHECK_EQUAL(sSE3_copy, sSE3);

    sSE3.random();

    sSE3 >> sC2;
    sSE3_copy << sC2;
    BOOST_CHECK_EQUAL(sSE3_copy, sSE3);

    sSE3.random();
    sSE3 >> sSE3_SO2;
    sSE3 >> sSE3_R;

    (sSE3_R ^ sSE3_SO2) >> sSE3_copy;
    BOOST_CHECK_EQUAL(sSE3_copy, sSE3);
    BOOST_CHECK_EQUAL(sSE3_copy[mSE3 * sSE3_R.getSpace()], sSE3_R);
    BOOST_CHECK_EQUAL(sSE3_copy[sSE3_SO2.getSpace()], sSE3_SO2);

    sSE3->setY(1.0);
    BOOST_OMPL_EXPECT_NEAR(sSE3.reals()[1], 1.0, 1e-12);

    BOOST_OMPL_EXPECT_NEAR(sSE3[1], 1.0, 1e-12);
    sSE3[2] = 0.1;
    BOOST_OMPL_EXPECT_NEAR(sSE3.reals()[2], 0.1, 1e-12);

    sSE3.random();
    std::vector<double> r = sSE3.reals();
    BOOST_CHECK_EQUAL(r.size(), 7u);
    sSE3_copy = r;
    BOOST_CHECK_EQUAL(sSE3_copy, sSE3);
    BOOST_CHECK_EQUAL(sSE3[6], r[6]);
    BOOST_CHECK_EQUAL(sSE3[0], r[0]);
    BOOST_CHECK_EQUAL(sSE3.getSpace()->getValueAddressAtIndex(sSE3.get(), 7), (double*)nullptr);

    sSE3_R = 0.5;
    BOOST_CHECK_EQUAL(sSE3_R[0], 0.5);

    sSE3 << sSE3_R;
    mSE3->setName("test");
    BOOST_CHECK_EQUAL(sSE3["test"], 0.5);
    sSE3["test"] = 0.1;
    BOOST_CHECK_EQUAL(sSE3[0], 0.1);
}

BOOST_AUTO_TEST_CASE(ScopedRV)
{
    auto m(std::make_shared<base::RealVectorStateSpace>(2));

    base::ScopedState<base::RealVectorStateSpace> s1(m);
    s1->values[0] = 1.0;
    s1->values[1] = 2.0;

    base::ScopedState<base::RealVectorStateSpace> s2 = s1;
    BOOST_CHECK(s2->values[1] == s1->values[1]);

    base::ScopedState<> s3(m);
    s3 = s1;
    base::ScopedState<> s4 = s3;
    BOOST_CHECK(s4 == s3);
    BOOST_CHECK(s4 == s1);

    base::ScopedState<base::RealVectorStateSpace> s5 = s2;
    BOOST_CHECK(s5 == s1);

    s1->values[1] = 4.0;

    BOOST_CHECK(s5 != s1);

    base::ScopedState<base::RealVectorStateSpace> s6(s5);
    BOOST_CHECK(s6 != s1);
    s1 = s5;
    s5 = s1;
    BOOST_CHECK(s6 == s1);
}

BOOST_AUTO_TEST_CASE(Allocation)
{
    auto m(std::make_shared<base::SE3StateSpace>());
    base::RealVectorBounds b(3);
    b.setLow(0);
    b.setHigh(1);
    m->setBounds(b);
    base::SpaceInformation si(m);
    si.setup();

    const unsigned int N = 30000;
    const unsigned int M = 20;
    std::vector<base::State*> states(N, nullptr);

    ompl::time::point start = ompl::time::now();
    for (unsigned int j = 0 ; j < M ; ++j)
    {
        for (unsigned int i = 0 ; i < N ; ++i)
            states[i] = si.allocState();

        for (unsigned int i = 0 ; i < N ; ++i)
            si.freeState(states[i]);
    }
    double d = ompl::time::seconds(ompl::time::now() - start);
    std::cout << (double)N * (double)M / d << " state allocations then frees per second" << std::endl;

    start = ompl::time::now();
    for (unsigned int j = 0 ; j < M ; ++j)
    {
        for (unsigned int i = 0 ; i < N ; ++i)
        {
            base::State *s = si.allocState();
            si.freeState(s);
        }
    }
    d = ompl::time::seconds(ompl::time::now() - start);
    std::cout << (double)N * (double)M / d << " mixed state allocations & frees per second" << std::endl;

    start = ompl::time::now();
    for (unsigned int j = 0 ; j < M ; ++j)
    {
        for (unsigned int i = 0 ; i < N ; ++i)
        {
            base::State *s = si.allocState();
            si.freeState(s);
            states[i] = si.allocState();
        }
        for (unsigned int i = 0 ; i < N ; ++i)
            si.freeState(states[i]);
    }
    d = ompl::time::seconds(ompl::time::now() - start);
    std::cout << (double)N * (double)M / d << " allocations per second" << std::endl;
}

void randomizedAllocator(const base::SpaceInformation *si)
{
    RNG r;
    const unsigned int n = 500;

    std::vector<base::State*> states(n + 1, nullptr);
    for (unsigned int i = 0 ; i < n * 1000 ; ++i)
    {
        int j = r.uniformInt(0, n);
        if (states[j] == nullptr)
            states[j] = si->allocState();
        else
        {
            si->freeState(states[j]);
            states[j] = nullptr;
        }
    }
    for (auto & state : states)
        if (state != nullptr)
            si->freeState(state);
}

BOOST_AUTO_TEST_CASE(AllocationWithThreads)
{
    auto m(std::make_shared<base::SE3StateSpace>());
    base::RealVectorBounds b(3);
    b.setLow(0);
    b.setHigh(1);
    m->setBounds(b);
    base::SpaceInformation si(m);
    si.setup();
    const int NT = 10;
    ompl::time::point start = ompl::time::now();
    std::vector<std::thread*> threads;
    for (int i = 0 ; i < NT ; ++i)
        threads.push_back(new std::thread([&si] { randomizedAllocator(&si); }));
    for (int i = 0 ; i < NT ; ++i)
    {
        threads[i]->join();
        delete threads[i];
    }
    std::cout << "Time spent randomly allocating & freeing states: "
        << ompl::time::seconds(ompl::time::now() - start) << std::endl;
}

BOOST_AUTO_TEST_CASE(PartialCopy)
{
    auto m(std::make_shared<base::SE3StateSpace>());
    base::RealVectorBounds b(3);
    b.setLow(0);
    b.setHigh(1);
    m->setBounds(b);
    m->setup();
    base::StateSpacePtr r3 = m->getSubspace(0);
    base::StateSpacePtr q = m->getSubspace(1);
    base::StateSamplerPtr s1 = m->base::StateSpace::allocSubspaceStateSampler(r3);
    base::StateSamplerPtr s2 = m->base::StateSpace::allocSubspaceStateSampler(q);
    base::ScopedState<base::SE3StateSpace> state(m);
    base::ScopedState<base::SE3StateSpace> tmp(m);
    std::vector<std::string> subspaces;
    m->getCommonSubspaces(m, subspaces);
    BOOST_CHECK(subspaces.size() == 1);
    BOOST_CHECK(subspaces[0] == m->getName());
    q->getCommonSubspaces(r3, subspaces);
    BOOST_CHECK(subspaces.empty());
    m->getCommonSubspaces(q, subspaces);
    BOOST_CHECK(subspaces.size() == 1);
    base::ScopedState<> dummy(q);

    for (int i = 0 ; i < 100 ; ++i)
    {
        state.random();
        tmp = state;
        s1->sampleUniform(tmp.get());
        BOOST_CHECK(tmp[q] == state[q]);
        BOOST_CHECK(tmp[r3] != state[r3]);
        s2->sampleUniform(tmp.get());
        BOOST_CHECK(tmp[q] != state[q]);
        BOOST_CHECK(copyStateData(m, state.get(), q, tmp[q].get(), subspaces) == base::ALL_DATA_COPIED);
        BOOST_CHECK(tmp[q] == state[q]);
        BOOST_CHECK(copyStateData(m, state.get(), q, tmp[q].get()) == base::ALL_DATA_COPIED);
        BOOST_CHECK(copyStateData(q, dummy.get(), m, state.get()) == base::SOME_DATA_COPIED);
        BOOST_CHECK(copyStateData(q, dummy.get(), r3, state[r3].get()) == base::NO_DATA_COPIED);
    }
}