//
// regressiontest.cc - regression test suite for vecmat3.h (not exhaustive)
//
// Copyright (c) 2013  Ramses van Zon
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include <iostream>
#include <sstream>
#include <cassert>
#include "vecmat3.h"

#define BOOST_TEST_MODULE vecmat3_test

#ifndef DEBUG
  #include <boost/test/included/unit_test.hpp>
  #include <boost/test/floating_point_comparison.hpp>
  BOOST_AUTO_TEST_SUITE (vecmat3_test)
#else
  #define declare_assign_vector main2
  #define BOOST_AUTO_TEST_CASE(x) void x()
  #define BOOST_CHECK(x) assert(x)
  #define BOOST_CHECK_CLOSE_FRACTION(x,y,z)
  void main2();int main(){main2();return 0;}
#endif

BOOST_AUTO_TEST_CASE( declare_uninitialized_vector )
{
  Vector v;
  BOOST_CHECK( sizeof(v) >= sizeof(DOUBLE)*3 );
}

BOOST_AUTO_TEST_CASE( declare_uninitialized_matrix )
{
  Matrix m;
  BOOST_CHECK( sizeof(m) >= sizeof(DOUBLE)*3 );
}

BOOST_AUTO_TEST_CASE( declare_initialized_vector )
{
  Vector v(1,2,3);
  BOOST_CHECK( v.x == 1 );
  BOOST_CHECK( v.y == 2 );
  BOOST_CHECK( v.z == 3 );
}

BOOST_AUTO_TEST_CASE( declare_initialized_matrix )
{
  Matrix m(1,2,3,
           4,5,6,
           7,8,9);
  BOOST_CHECK( m.xx == 1 ); BOOST_CHECK( m.xy == 2 ); BOOST_CHECK( m.xz == 3 );
  BOOST_CHECK( m.yx == 4 ); BOOST_CHECK( m.yy == 5 ); BOOST_CHECK( m.yz == 6 );
  BOOST_CHECK( m.zx == 7 ); BOOST_CHECK( m.zy == 8 ); BOOST_CHECK( m.zz == 9 );
}

BOOST_AUTO_TEST_CASE( declare_zero_vector )
{
  Vector v(0);
  BOOST_CHECK( v.x == 0 );
  BOOST_CHECK( v.y == 0 );
  BOOST_CHECK( v.z == 0 );
}

BOOST_AUTO_TEST_CASE( declare_zero_matrix )
{
  Matrix m(0);
  BOOST_CHECK( m.xx == 0 ); BOOST_CHECK( m.xy == 0 ); BOOST_CHECK( m.xz == 0 );
  BOOST_CHECK( m.yx == 0 ); BOOST_CHECK( m.yy == 0 ); BOOST_CHECK( m.yz == 0 );
  BOOST_CHECK( m.zx == 0 ); BOOST_CHECK( m.zy == 0 ); BOOST_CHECK( m.zz == 0 );
}

BOOST_AUTO_TEST_CASE( declare_assign_vector )
{
  Vector v(1,2,3);
  Vector w = v;
  BOOST_CHECK( w.x == 1 );
  BOOST_CHECK( w.y == 2 );
  BOOST_CHECK( w.z == 3 );
}

BOOST_AUTO_TEST_CASE( declare_assign_matrix )
{
  Matrix m(1,2,3,
           4,5,6,
           7,8,9);
  Matrix n = m;
  BOOST_CHECK( n.xx == 1 ); BOOST_CHECK( n.xy == 2 ); BOOST_CHECK( n.xz == 3 );
  BOOST_CHECK( n.yx == 4 ); BOOST_CHECK( n.yy == 5 ); BOOST_CHECK( n.yz == 6 );
  BOOST_CHECK( n.zx == 7 ); BOOST_CHECK( n.zy == 8 ); BOOST_CHECK( n.zz == 9 );
}

BOOST_AUTO_TEST_CASE( comma_assign_vector )
{
  Vector w;
  w = 1, 2, 3;
  BOOST_CHECK( w.x == 1 );
  BOOST_CHECK( w.y == 2 );
  BOOST_CHECK( w.z == 3 );
}

BOOST_AUTO_TEST_CASE( comma_assign_matrix )
{
  Matrix n;
  n = 1,2,3,
      4,5,6,
      7,8,9;
  BOOST_CHECK( n.xx == 1 ); BOOST_CHECK( n.xy == 2 ); BOOST_CHECK( n.xz == 3 );
  BOOST_CHECK( n.yx == 4 ); BOOST_CHECK( n.yy == 5 ); BOOST_CHECK( n.yz == 6 );
  BOOST_CHECK( n.zx == 7 ); BOOST_CHECK( n.zy == 8 ); BOOST_CHECK( n.zz == 9 );
}

BOOST_AUTO_TEST_CASE( zero_vector )
{
  Vector v(1,2,3);
  v.zero();
  BOOST_CHECK( v.x == 0 );
  BOOST_CHECK( v.y == 0 );
  BOOST_CHECK( v.z == 0 );
}

BOOST_AUTO_TEST_CASE( zero_matrix )
{
  Matrix m(1,2,3,
           4,5,6,
           7,8,9);
  m.zero();
  BOOST_CHECK( m.xx == 0 ); BOOST_CHECK( m.xy == 0 ); BOOST_CHECK( m.xz == 0 );
  BOOST_CHECK( m.yx == 0 ); BOOST_CHECK( m.yy == 0 ); BOOST_CHECK( m.yz == 0 );
  BOOST_CHECK( m.zx == 0 ); BOOST_CHECK( m.zy == 0 ); BOOST_CHECK( m.zz == 0 );
}

BOOST_AUTO_TEST_CASE( stream_vector )
{
  Vector v(1,2,3);
  std::stringstream s;
  s << v;
  BOOST_CHECK( s.str() == "1 2 3" );
}

BOOST_AUTO_TEST_CASE( stream_matrix )
{
  Matrix m(1,2,3,
           4,5,6,
           7,8,9);
  std::stringstream s;
  s << m;
  BOOST_CHECK( s.str() == "\n1 2 3\n4 5 6\n7 8 9\n" );
}

BOOST_AUTO_TEST_CASE( multiply_matrix )
{
  Matrix X(1,2,3,
           4,5,6,
           7,8,9);
  Matrix Y(0,1,2,
           3,4,5,
           6,7,8);
  Matrix Z = X*Y;
  Matrix Zexpected( 24,  30,  36,
                    51,  66,  81,
                    78, 102, 126);
  BOOST_CHECK( Z.xx == Zexpected.xx );
  BOOST_CHECK( Z.xy == Zexpected.xy );
  BOOST_CHECK( Z.xz == Zexpected.xz );
  BOOST_CHECK( Z.yx == Zexpected.yx );
  BOOST_CHECK( Z.yy == Zexpected.yy );
  BOOST_CHECK( Z.yz == Zexpected.yz );
  BOOST_CHECK( Z.zx == Zexpected.zx );
  BOOST_CHECK( Z.zy == Zexpected.zy );
  BOOST_CHECK( Z.zz == Zexpected.zz );
}

BOOST_AUTO_TEST_CASE( in_situ_multiply_matrix )
{
  Matrix X(1,2,3,
           4,5,6,
           7,8,9);
  Matrix Y(0,1,2,
           3,4,5,
           6,7,8);
  X = X*Y;
  Matrix Xexpected( 24,  30,  36,
                    51,  66,  81,
                    78, 102, 126);
  BOOST_CHECK( X.xx == Xexpected.xx );
  BOOST_CHECK( X.xy == Xexpected.xy );
  BOOST_CHECK( X.xz == Xexpected.xz );
  BOOST_CHECK( X.yx == Xexpected.yx );
  BOOST_CHECK( X.yy == Xexpected.yy );
  BOOST_CHECK( X.yz == Xexpected.yz );
  BOOST_CHECK( X.zx == Xexpected.zx );
  BOOST_CHECK( X.zy == Xexpected.zy );
  BOOST_CHECK( X.zz == Xexpected.zz );
}

BOOST_AUTO_TEST_CASE( vector_nrm2 )
{
  Vector d(-1, 2.2, 3.1);
  DOUBLE expected = 15.45;
  DOUBLE tol = 1e-10;
  BOOST_CHECK_CLOSE_FRACTION( d.nrm2(), expected, tol);
}

BOOST_AUTO_TEST_CASE( vector_nrm )
{
  Vector d(-1, 2.2, 3.1);
  DOUBLE expected = sqrt(15.45);
  DOUBLE tol = 1e-10;
  BOOST_CHECK_CLOSE_FRACTION( d.nrm(), expected, tol);
}

BOOST_AUTO_TEST_CASE( matrix_nrm2 )
{
  Matrix d(  -1, 0, 0,
            2.2, 0, 1,
              0, 0, 3.1);
  DOUBLE expected = 16.45;
  DOUBLE tol = 1e-10;
  BOOST_CHECK_CLOSE_FRACTION( d.nrm2(), expected, tol);
}

BOOST_AUTO_TEST_CASE( matrix_nrm )
{
  Matrix d(  -1, 0, 0,
            2.2, 0, 1,
              0, 0, 3.1);
  DOUBLE expected = sqrt(16.45);
  DOUBLE tol = 1e-10;
  BOOST_CHECK_CLOSE_FRACTION( d.nrm(), expected, tol);
}

BOOST_AUTO_TEST_CASE( vector_square_brackets )
{
  Vector d(-1, 2.2, 3.1);
  BOOST_CHECK( d[0] == -1 );
  BOOST_CHECK( d[1] == 2.2 );
  BOOST_CHECK( d[2] == 3.1 );
}

BOOST_AUTO_TEST_CASE( vector_round_brackets )
{
  Vector d(-1, 2.2, 3.1);
  BOOST_CHECK( d(0) == -1 );
  BOOST_CHECK( d(1) == 2.2 );
  BOOST_CHECK( d(2) == 3.1 );
}

BOOST_AUTO_TEST_CASE( matrix_square_brackets )
{
  Matrix M(-1, 2.2, 3.1,
            4,   5,   6,
            7,   8,   9);
  BOOST_CHECK( M[0][0] == -1 );
  BOOST_CHECK( M[0][1] == 2.2 );
  BOOST_CHECK( M[0][2] == 3.1 );
  BOOST_CHECK( M[1][0] == 4 );
  BOOST_CHECK( M[1][1] == 5 );
  BOOST_CHECK( M[1][2] == 6 );
  BOOST_CHECK( M[2][0] == 7 );
  BOOST_CHECK( M[2][1] == 8 );
  BOOST_CHECK( M[2][2] == 9 );
}

BOOST_AUTO_TEST_CASE( matrix_round_brackets )
{
  Matrix M(-1, 2.2, 3.1,
            4,   5,   6,
            7,   8,   9);
  BOOST_CHECK( M(0,0) == -1 );
  BOOST_CHECK( M(0,1) == 2.2 );
  BOOST_CHECK( M(0,2) == 3.1 );
  BOOST_CHECK( M(1,0) == 4 );
  BOOST_CHECK( M(1,1) == 5 );
  BOOST_CHECK( M(1,2) == 6 );
  BOOST_CHECK( M(2,0) == 7 );
  BOOST_CHECK( M(2,1) == 8 );
  BOOST_CHECK( M(2,2) == 9 );
}

BOOST_AUTO_TEST_CASE( multiply_vector )
{
  DOUBLE tol = 1e-10;
  Vector c(-1, 2.2, 3.1);
  Vector d;
  d = 2.4*c;
  BOOST_CHECK_CLOSE_FRACTION( d.x, -2.4, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y, 5.28, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, 7.44, tol );
}

BOOST_AUTO_TEST_CASE( in_situ_multiply_vector )
{
  DOUBLE tol = 1e-10;
  Vector d(-1, 2.2, 3.1);
  d *= 2;
  BOOST_CHECK_CLOSE_FRACTION( d.x,  -2, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y, 4.4, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, 6.2, tol );
}

BOOST_AUTO_TEST_CASE( in_situ_divide_vector )
{
  DOUBLE tol = 1e-10;
  Vector d(-1, 2.2, 3.1);
  d /= 5;
  BOOST_CHECK_CLOSE_FRACTION( d.x, -0.2, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y, 0.44, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, 0.62, tol );
}

BOOST_AUTO_TEST_CASE( in_situ_add_vectors )
{
  DOUBLE tol = 1e-10;
  Vector d(-1, 2.2, 3.1);
  Vector c( 1, 1.7, -13.9);
  d += c;
  BOOST_CHECK_CLOSE_FRACTION( d.x,   0.0, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y,   3.9, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, -10.8, tol );
}

BOOST_AUTO_TEST_CASE( in_situ_subtract_vectors )
{
  DOUBLE tol = 1e-10;
  Vector d(-1, 2.2, 3.1);
  Vector c( 1, 1.7, -13.9);
  d -= c;
  BOOST_CHECK_CLOSE_FRACTION( d.x, -2.0, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y,  0.5, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, 17.0, tol );
}

BOOST_AUTO_TEST_CASE( add_vectors )
{
  DOUBLE tol = 1e-10;
  Vector b(-1, 2.2, 3.1);
  Vector c( 1, 1.7, -13.9);
  Vector d;
  d = b + c;
  BOOST_CHECK_CLOSE_FRACTION( d.x,   0.0, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y,   3.9, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, -10.8, tol );
}

BOOST_AUTO_TEST_CASE( subtract_vectors )
{
  DOUBLE tol = 1e-10;
  Vector b(-1, 2.2, 3.1);
  Vector c( 1, 1.7, -13.9);
  Vector d;
  d = b - c;
  BOOST_CHECK_CLOSE_FRACTION( d.x, -2.0, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y,  0.5, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, 17.0, tol );
}

BOOST_AUTO_TEST_CASE( cross_product )
{
  DOUBLE tol = 1e-10;
  Vector b( 3,  4,  5);
  Vector c(-7, -6, -5);
  Vector d = b^c;
  BOOST_CHECK_CLOSE_FRACTION( d.x, 10, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y,-20, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, 10, tol );
}

BOOST_AUTO_TEST_CASE( negative )
{
  DOUBLE tol = 1e-10;
  Vector b( 3,  4,  5);
  Vector d;
  d = -b;
  BOOST_CHECK_CLOSE_FRACTION( d.x, -3, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.y, -4, tol );
  BOOST_CHECK_CLOSE_FRACTION( d.z, -5, tol );
}

BOOST_AUTO_TEST_CASE( inner_product )
{
  DOUBLE tol = 1e-10;
  Vector b( 3,  4, 5);
  Vector c(-7, -6, 5);
  DOUBLE d = (b|c);
  DOUBLE dexpected = -20;
  BOOST_CHECK_CLOSE_FRACTION( d, dexpected, tol );
}

BOOST_AUTO_TEST_CASE( inner_product_variant )
{
  DOUBLE tol = 1e-10;
  Vector b( 3,  4, 5);
  Vector c(-7, -6, 5);
  DOUBLE d = b*c;
  DOUBLE dexpected = -20;
  BOOST_CHECK_CLOSE_FRACTION( d, dexpected, tol );
}

BOOST_AUTO_TEST_CASE( get_row )
{
  Matrix D( -1.0,  2.2, 3.1,
            -5.0, -3.0, 2.0,
             1.0, -2.0, 4.4);
  Vector w = D.row(1);
  BOOST_CHECK( w.x == -5.0 );
  BOOST_CHECK( w.y == -3.0 );
  BOOST_CHECK( w.z ==  2.0 );
}

BOOST_AUTO_TEST_CASE( set_row )
{
  Matrix D( -1.0,  2.2, 3.1,
            -5.0, -3.0, 2.0,
             1.0, -2.0, 4.4);
  Vector h(  1.0, 3.0 ,6.0);
  D.setRow(1,h);
  Vector w = D.row(1);
  BOOST_CHECK( w.x ==  1.0 );
  BOOST_CHECK( w.y ==  3.0 );
  BOOST_CHECK( w.z ==  6.0 );
}

BOOST_AUTO_TEST_CASE( get_column )
{
  Matrix D( -1.0,  2.2, 3.1,
            -5.0, -3.0, 2.0,
             1.0, -2.0, 4.4);
  Vector w = D.column(2);
  BOOST_CHECK( w.x == 3.1 );
  BOOST_CHECK( w.y == 2.0 );
  BOOST_CHECK( w.z == 4.4 );
}

BOOST_AUTO_TEST_CASE( set_column )
{
  Matrix D( -1.0,  2.2, 3.1,
            -5.0, -3.0, 2.0,
             1.0, -2.0, 4.4);
  Vector h(  1.0, 3.0 ,6.0);
  D.setColumn(1,h);
  Vector w = D.column(1);
  BOOST_CHECK( w.x ==  1.0 );
  BOOST_CHECK( w.y ==  3.0 );
  BOOST_CHECK( w.z ==  6.0 );
}

BOOST_AUTO_TEST_CASE( dyadic )
{
  Vector g( -1, 2, 3);
  Vector h(  1, 3, 6);
  Matrix F;
  F = Dyadic(g,h);
  Matrix Fexpected(-1,-3,-6,
                   2, 6, 12,
                   3, 9, 18);
  BOOST_CHECK( F.xx ==  Fexpected.xx );
  BOOST_CHECK( F.xy ==  Fexpected.xy );
  BOOST_CHECK( F.xz ==  Fexpected.xz );
  BOOST_CHECK( F.yx ==  Fexpected.yx );
  BOOST_CHECK( F.yy ==  Fexpected.yy );
  BOOST_CHECK( F.yz ==  Fexpected.yz );
  BOOST_CHECK( F.zx ==  Fexpected.zx );
  BOOST_CHECK( F.zy ==  Fexpected.zy );
  BOOST_CHECK( F.zz ==  Fexpected.zz );
}

BOOST_AUTO_TEST_CASE( scalar_multiply_matrix )
{
  DOUBLE tol = 1e-10;
  Matrix C(-1, 2, 3,
            7, 6, 5,
            4, 3, 1);
  Matrix D;
  D = 2.4*C;
  BOOST_CHECK_CLOSE_FRACTION( D.xx, -2.4, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xy,  4.8, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xz,  7.2, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yx, 16.8, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yy, 14.4, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yz, 12.0, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zx,  9.6, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zy,  7.2, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zz,  2.4, tol );
}

BOOST_AUTO_TEST_CASE( in_situ_scalar_multiply_matrix )
{
  DOUBLE tol = 1e-10;
  Matrix D(-1, 2, 3,
            7, 6, 5,
            4, 3, 1);
  D *= 2.4;
  BOOST_CHECK_CLOSE_FRACTION( D.xx, -2.4, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xy,  4.8, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xz,  7.2, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yx, 16.8, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yy, 14.4, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yz, 12.0, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zx,  9.6, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zy,  7.2, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zz,  2.4, tol );
}

BOOST_AUTO_TEST_CASE( scalar_in_situ_divide_matrix )
{
  DOUBLE tol = 1e-10;
  Matrix D(-1, 2, 3,
            7, 6, 5,
            4, 3, 1);
  D /= 4;
  BOOST_CHECK_CLOSE_FRACTION( D.xx, -0.25, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xy,  0.5, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xz,  0.75, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yx,  1.75, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yy,  1.5, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yz,  1.25, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zx,  1.0, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zy,  0.75, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zz,  0.25, tol );
}

BOOST_AUTO_TEST_CASE( in_situ_add_matrices )
{
  DOUBLE tol = 1e-10;
  Matrix D(-1, 2, 3,
            7, 6, 5,
            4, 3, 1);
  Matrix C(-2, 5, 4,
            4, 6, 2,
           -4, 3, 4);
  D += C;
  BOOST_CHECK_CLOSE_FRACTION( D.xx, -3, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xy,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yx, 11, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yy, 12, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zx,  0, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zy,  6, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zz,  5, tol );
}

BOOST_AUTO_TEST_CASE( in_situ_subtract_matrices )
{
  DOUBLE tol = 1e-10;
  Matrix D(-1, 2, 3,
            7, 6, 5,
            4, 3, 1);
  Matrix C( 2, -5, -4,
           -4, -6, -2,
            4, -3, -4);
  D -= C;
  BOOST_CHECK_CLOSE_FRACTION( D.xx, -3, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xy,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yx, 11, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yy, 12, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zx,  0, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zy,  6, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zz,  5, tol );
}

BOOST_AUTO_TEST_CASE( add_matrices )
{
  DOUBLE tol = 1e-10;
  Matrix A(-1, 2, 3,
            7, 6, 5,
            4, 3, 1);
  Matrix B(-2, 5, 4,
            4, 6, 2,
           -4, 3, 4);
  Matrix D;
  D = A + B;
  BOOST_CHECK_CLOSE_FRACTION( D.xx, -3, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xy,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yx, 11, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yy, 12, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zx,  0, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zy,  6, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zz,  5, tol );
}

BOOST_AUTO_TEST_CASE( subtract_matrices )
{
  DOUBLE tol = 1e-10;
  Matrix A(-1, 2, 3,
            7, 6, 5,
            4, 3, 1);
  Matrix B(2, -5, -4,
           -4, -6, -2,
           4, -3, -4);
  Matrix D;
  D = A - B;
  BOOST_CHECK_CLOSE_FRACTION( D.xx, -3, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xy,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.xz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yx, 11, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yy, 12, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.yz,  7, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zx,  0, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zy,  6, tol );
  BOOST_CHECK_CLOSE_FRACTION( D.zz,  5, tol );
}

BOOST_AUTO_TEST_CASE( intermediate_row )
{
  DOUBLE tol = 1e-10;
  Matrix A(-1,  2,  3,
            7,  6,  5,
            4,  3,  1);
  Matrix B( 2, -5, -4,
           -4, -6, -2,
            4, -3, -4);
  Vector a = (A*B).row(1);
  BOOST_CHECK_CLOSE_FRACTION( a.x, 10, tol );
  BOOST_CHECK_CLOSE_FRACTION( a.y, -86, tol );
  BOOST_CHECK_CLOSE_FRACTION( a.z, -60, tol );
}

BOOST_AUTO_TEST_CASE( rodrigues )
{
  DOUBLE tol = 1e-10;
  Vector d(1,1,1);
  Vector n = d/d.nrm();
  Matrix R = Rodrigues((M_PI/2.0)*n);
  BOOST_CHECK_CLOSE_FRACTION( R.xx, 1./3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.xy, (1.-sqrt(3.))/3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.xz, (1.+sqrt(3.))/3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.yx, (1.+sqrt(3.))/3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.yy, 1./3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.yz, (1.-sqrt(3.))/3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.zx, (1.-sqrt(3.))/3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.zy, (1.+sqrt(3.))/3., tol );
  BOOST_CHECK_CLOSE_FRACTION( R.zz, 1./3., tol );
}

BOOST_AUTO_TEST_CASE( inverse )
{
  DOUBLE tol = 1e-10;
  Matrix A(-1,  2,  3,
            3,  2,  3,
            1,  3,  1);
  Matrix B = Inverse(A);
  BOOST_CHECK_CLOSE_FRACTION( B.xx, -1./4., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.xy,  1./4., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.xz,     0., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.yx,     0., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.yy, -1./7., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.yz,  3./7., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.zx,  1./4., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.zy, 5./28., tol );
  BOOST_CHECK_CLOSE_FRACTION( B.zz, -2./7., tol );
}

#ifndef DEBUG
BOOST_AUTO_TEST_SUITE_END()
#endif


/*
 * Graph.hpp
 *
 *  Created on: May 18, 2012
 *      Author: david
 */

#ifndef DASP_GRAPH_HPP_
#define DASP_GRAPH_HPP_

#include "Point.hpp"
#include "as_range.hpp"
#include "graphseg.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <Eigen/Dense>

namespace dasp
{
	struct NeighbourhoodProperties
	{
		unsigned int num_border_pixels;
		std::vector<unsigned int> border_pixel_ids;
	};

	/** Undirected graph */
	typedef boost::adjacency_list<
		boost::vecS, boost::vecS, boost::undirectedS,
		boost::no_property,
		NeighbourhoodProperties
	> NeighbourhoodGraph;

	/** Undirected, weighted (type=float) graph */
	typedef graphseg::SpectralGraph UndirectedWeightedGraph;

	/** Weighted graph of superpoints */
	typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
		Point,
		float
	> DaspGraph;

}

#endif


#ifndef PARMCB_DETAIL_DIJKSTRA_HPP_
#define PARMCB_DETAIL_DIJKSTRA_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/scoped_array.hpp>
#include <boost/throw_exception.hpp>
#include <boost/functional/hash.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>

#include <parmcb/detail/util.hpp>

namespace parmcb {

    namespace detail {

        template<class Graph>
        struct IndexInHeapFunctor {
            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;

            std::vector<std::size_t> &index_in_heap;
            const VertexIndexMapType &index_map;

            IndexInHeapFunctor(std::vector<std::size_t> &index_in_heap, const VertexIndexMapType &index_map) :
                    index_in_heap(index_in_heap), index_map(index_map) {
            }

            std::size_t& operator()(const Vertex &v) const {
                return index_in_heap.at(index_map[v]);
            }
        };

    } // detail

    template<class Graph, class WeightMap, class DistanceMap, class PredecessorMap>
    void dijkstra(const Graph &g, const WeightMap &weight_map,
            const typename boost::graph_traits<Graph>::vertex_descriptor &s, DistanceMap &dist_map,
            PredecessorMap &pred_map) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;
        typedef typename boost::property_traits<DistanceMap>::value_type DistanceType;
        typedef boost::d_ary_heap_indirect<Vertex, 4,
                boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, std::size_t>, Vertex,
                        std::size_t&>, DistanceMap, std::less<DistanceType>> VertexQueue;

        std::less<DistanceType> compare;
        parmcb::detail::closed_plus<DistanceType> combine = parmcb::detail::closed_plus<DistanceType>();

        const VertexIndexMapType &index_map = boost::get(boost::vertex_index, g);
        std::vector<std::size_t> index_in_heap(boost::num_vertices(g));
        boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, std::size_t>, Vertex, std::size_t&> index_in_heap_map(
                parmcb::detail::VertexIndexFunctor<Graph, std::size_t>(index_in_heap, index_map));

        VertexQueue queue(dist_map, index_in_heap_map, compare);

        boost::put(dist_map, s, DistanceType());
        boost::put(pred_map, s, std::make_tuple(false, Edge()));
        queue.push(s);

        while (!queue.empty()) {
            Vertex u = queue.top();
            queue.pop();
            DistanceType d_u = boost::get(dist_map, u);

            auto eiRange = boost::out_edges(u, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto e = *ei;

                auto w = boost::target(e, g);
                if (w == u) {
                    w = boost::source(e, g);
                }
                if (w == u) {
                    // self-loop
                    continue;
                }
                if (w == s) {
                    continue;
                }

                const WeightType c = combine(d_u, get(weight_map, e));
                bool visited_w = std::get<0>(boost::get(pred_map, w));
                if (!visited_w) {
                    // first time found
                    boost::put(dist_map, w, c);
                    boost::put(pred_map, w, std::make_tuple(true, e));
                    queue.push(w);
                } else if (compare(c, boost::get(dist_map, w))) {
                    // already reached
                    boost::put(dist_map, w, c);
                    boost::put(pred_map, w, std::make_tuple(true, e));
                    queue.update(w);
                }
            }
        }
    }

} // parmcb

#endif


#include <benchmark/benchmark.h>

#include <tnt/core/core.hpp>
#include <tnt/math/math.hpp>

#include <opencv2/core.hpp>
#include <benchmark/opencv_utils.hpp>

#include <Eigen/Dense>

template <typename DataType>
static void multiply_TNT(benchmark::State& state, int size)
{
    while (state.KeepRunning()) {
        state.PauseTiming(); // We don't count tensor creation
        tnt::Shape shape{size, size};

        tnt::Tensor<DataType> left(shape, 1.f);
        tnt::Tensor<DataType> right(shape, 2.f);

        state.ResumeTiming();
        tnt::multiply(left, right);
    }
}

template <typename DataType>
static void multiply_OCV(benchmark::State& state, int size)
{
    while (state.KeepRunning()) {
        state.PauseTiming();

        cv::Mat left  = tnt::create_cv_mat<DataType>(size, size, cv::Scalar(1));
        cv::Mat right = tnt::create_cv_mat<DataType>(size, size, cv::Scalar(2));

        cv::Mat dst;

        state.ResumeTiming();
        cv::multiply(left, right, dst);
    }
}

template <typename DataType>
static void multiply_EIG(benchmark::State& state, int size)
{
    using MatType = Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>;

    while (state.KeepRunning()) {
        state.PauseTiming();

        MatType left = MatType::Constant(size, size, 1);
        MatType right = MatType::Constant(size, size, 2);

        state.ResumeTiming();
        MatType dst = left.cwiseProduct(right);
    }
}

template <typename T>
class RegisterMultiplyBenchmark
{
public:
    RegisterMultiplyBenchmark(const std::string& type)
    {
        std::vector<int> sizes{4, 16, 64, 512, 2048};
        for (int size : sizes) {
            std::string suffix = type + ">[" + std::to_string(size) + "x" + std::to_string(size) + "]";
            benchmark::RegisterBenchmark(("Multiply:TNT<" + suffix).c_str(), multiply_TNT<T>, size);
            benchmark::RegisterBenchmark(("Multiply:OCV<" + suffix).c_str(), multiply_OCV<T>, size);
            benchmark::RegisterBenchmark(("Multiply:EIG<" + suffix).c_str(), multiply_EIG<T>, size);
        }
    }
};

static RegisterMultiplyBenchmark<int>    multiply_benchmark_int("int");
static RegisterMultiplyBenchmark<float>  multiply_benchmark_float("float");
static RegisterMultiplyBenchmark<double> multiply_benchmark_double("double");


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_FMOD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_FMOD_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/function/definition/rem.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    using fmod_ = rem_;
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::fmod_,fmod);
} }

#endif


/*
SplitLas, 2015

Megan Kress <kressmeg@msu.edu>

This program takes the input of a LAS
file and splits it into 195mx195m LAS files.

*/

#include <stdio.h>
#include <vector>
#include <iostream>
#include <string>
#include <fstream>  // std::ifstream
#include <liblas/liblas.hpp>
#include <liblas/filter.hpp>
#include <sstream>
#include <algorithm>
#include <iterator>
#include <stdlib.h>
#include <boost/filesystem.hpp>
#include <boost/thread.hpp>

using namespace std;

void thread(string segment, string file, vector<liblas::Bounds<double> > filters, int j)
{

    unsigned int k = 0;
    for(unsigned int i = j; i < filters.size() + j; ++i)
    {

        liblas::Bounds<double> bounds = filters.at(k++);

        int ptCount = 0;

        ifstream ifs;
        ifs.open( file.c_str(), ios::in | ios::binary );

        liblas::Reader reader = liblas::Reader(ifs);

        liblas::Header header = reader.GetHeader();

        std::vector<liblas::FilterPtr> myfilter;

        liblas::FilterPtr b_filter = liblas::FilterPtr(new liblas::BoundsFilter(bounds));

        myfilter.push_back(b_filter);

        vector<double>::iterator  xmin, xmax, ymin, ymax, zmin, zmax;
        std::vector<double> x_v, y_v, z_v;

        reader.SetFilters(myfilter);

//        cout << "read points\n";

        if(reader.ReadNextPoint())
        {
            std::ofstream ofs;

            stringstream outFile;
            outFile << segment << "_" << i << ".las";
            string outFiles = outFile.str();

            ofs.open(outFiles.c_str(), ios::out | ios::binary);

            liblas::Writer writer	(ofs, header);

            liblas::Point p = reader.GetPoint();

            ++ptCount;

            writer.WritePoint(p);

            x_v.push_back(p.GetX());
            y_v.push_back(p.GetY());
            z_v.push_back(p.GetZ());

            while (reader.ReadNextPoint())
            {

                liblas::Point p = reader.GetPoint();

                ++ptCount;

                writer.WritePoint(p);

                x_v.push_back(p.GetX());
                y_v.push_back(p.GetY());
                z_v.push_back(p.GetZ());

            }

            xmin = min_element(x_v.begin(), x_v.end());
            xmax = max_element(x_v.begin(), x_v.end());
            ymin = min_element(y_v.begin(), y_v.end());
            ymax = max_element(y_v.begin(), y_v.end());
            zmin = min_element(z_v.begin(), z_v.end());
            zmax = max_element(z_v.begin(), z_v.end());

            liblas::Bounds<double> extents(*xmin,*ymin,*zmin,*xmax,*ymax,*zmax);

            static liblas::Header header1 ( header );

            header1.SetExtent(extents);

            writer.SetHeader(header1);

            writer.WriteHeader();

            ofs.close();

    }
    }
}

int main(int argc, char *argv[])
{

    string file = argv[1];

    double width = 195;

    if(argc > 2)
    {
        width = strtod(argv[2],NULL);
    }

//    cout << "\n" << width << "\n";

    string delimiter = "/";
    string delimiter1 = ".";

    string segment = file.substr(0,file.find(delimiter1));

    cout << segment << "\n";

    ifstream ifs;
    ifs.open( file.c_str(), ios::in | ios::binary );

    liblas::Reader reader = liblas::Reader(ifs);

    liblas::Header header = reader.GetHeader();

    double minx = header.GetMinX();
    double maxx = header.GetMaxX();
    double miny = header.GetMinY();
    double maxy = header.GetMaxY();
    double minz = header.GetMinZ();
    double maxz = header.GetMaxZ();

    double x = minx;
    double y = miny;

    std::vector<liblas::Bounds<double> > filters;

    while(x < maxx)
    {
        while(y < maxy)
        {
            liblas::Bounds<double> bound(x,y,minz,x+width,y+width,maxz);

            filters.push_back(bound);

            y += width;
        }

        y = miny;
        x += width;
    }

     liblas::Reader reader0 = liblas::Reader(ifs);
     liblas::Reader reader1 = liblas::Reader(ifs);
     liblas::Reader reader2 = liblas::Reader(ifs);
     liblas::Reader reader3 = liblas::Reader(ifs);
     liblas::Reader reader4 = liblas::Reader(ifs);
     liblas::Reader reader5 = liblas::Reader(ifs);

    std::vector<liblas::Bounds<double> > filter0, filter1, filter2, filter3, filter4, filter5;

    int j1 = filters.size()/6;
    int j2 = j1 + j1;
    int j3 = j2 + j1;
    int j4 = j3 + j1;
    int j5 = j4 + j1;

    filter0.reserve(j1 + 5);
    filter1.reserve(j1 + 5);
    filter2.reserve(j1 + 5);
    filter3.reserve(j1 + 5);
    filter4.reserve(j1 + 5);
    filter5.reserve(j1 + 5);

//    cout << j1 << '\n';
//    cout << filters.size();

    filter0.insert(filter0.begin(), filters.begin(), filters.begin() + j1 - 1);
    filter1.insert(filter1.begin(), filters.begin() + j1, filters.begin() + j2 - 1);
    filter2.insert(filter2.begin(), filters.begin() + j2, filters.begin() + j3 - 1);
    filter3.insert(filter3.begin(), filters.begin() + j3, filters.begin() + j4 - 1);
    filter4.insert(filter4.begin(), filters.begin() + j4, filters.begin() + j5 - 1);
    filter5.insert(filter5.begin(), filters.begin() + j5, filters.end());

    boost::thread t0{thread, segment, file, filter0, 0};
    boost::thread t1{thread, segment, file, filter1, j1};
    boost::thread t2{thread, segment, file, filter2, j2};
    boost::thread t3{thread, segment, file, filter3, j3};
    boost::thread t4{thread, segment, file, filter4, j4};
    boost::thread t5{thread, segment, file, filter5, j5};

    t0.join();
    t1.join();
    t2.join();
    t3.join();
    t4.join();
    t5.join();

//    boost::filesystem::path full_path( boost::filesystem::current_path() );
////    cout << full_path << file;
//    file = full_path.string() + "/" + file;
//    remove(file.c_str());

    return 0;

}


#include <iostream>                    // std::cout
#include <fstream>                     // std::ifstream, std::ofstream
#include <filesystem>                  // std::filesystem
#include <string>                      // std::string
#include <tuple>                       // std::tuple
#include <chrono>                      // std::chrono
#include <utility>                     // std::pair
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "networks.hpp"                // GAN_Encoder, GAN_Generator, GAN_Discriminator
#include "transforms.hpp"              // transforms_Compose
#include "datasets.hpp"                // datasets::ImageFolderWithPaths
#include "dataloader.hpp"              // DataLoader::ImageFolderWithPaths
#include "visualizer.hpp"              // visualizer

// Define Namespace
namespace fs = std::filesystem;
namespace po = boost::program_options;

// Function Prototype
std::tuple<torch::Tensor, torch::Tensor, torch::Tensor> AnomalyScore(torch::Tensor image, torch::Tensor fake_image, torch::Tensor z, GAN_Discriminator &dis, const float Lambda=0.1);

// ---------------
// Test Function
// ---------------
void test(po::variables_map &vm, torch::Device &device, GAN_Encoder &enc, GAN_Generator &gen, GAN_Discriminator &dis, std::vector<transforms_Compose> &transform){

    constexpr std::pair<float, float> output_range = {-1.0, 1.0};  // range of the value in output images

    // (0) Initialization and Declaration
    float ave_anomaly_score, ave_res_loss, ave_dis_loss;
    double seconds, ave_time;
    std::string path, result_dir, fname;
    std::string dataroot;
    std::ofstream ofs, ofs_score;
    std::chrono::system_clock::time_point start, end;
    std::tuple<torch::Tensor, std::vector<std::string>> data;
    std::tuple<torch::Tensor, torch::Tensor, torch::Tensor> anomaly_score_with_alpha;
    torch::Tensor image, z, output;
    torch::Tensor anomaly_score, res_loss, dis_loss;
    datasets::ImageFolderWithPaths dataset;
    DataLoader::ImageFolderWithPaths dataloader;

    // (1) Get Test Dataset
    dataroot = "datasets/" + vm["dataset"].as<std::string>() + '/' + vm["test_dir"].as<std::string>();
    dataset = datasets::ImageFolderWithPaths(dataroot, transform);
    dataloader = DataLoader::ImageFolderWithPaths(dataset, /*batch_size_=*/1, /*shuffle_=*/false, /*num_workers_=*/0);
    std::cout << "total test images : " << dataset.size() << std::endl << std::endl;

    // (2) Get Model
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + "_enc.pth"; torch::load(enc, path, device);
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + "_gen.pth"; torch::load(gen, path, device);
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + "_dis.pth"; torch::load(dis, path, device);

    // (3) Initialization of Value
    ave_anomaly_score = 0.0;
    ave_res_loss = 0.0;
    ave_dis_loss = 0.0;
    ave_time = 0.0;

    // (4) Tensor Forward
    torch::NoGradGuard no_grad;
    enc->eval();
    gen->eval();
    dis->eval();
    result_dir = vm["test_result_dir"].as<std::string>();  fs::create_directories(result_dir);
    ofs.open(result_dir + "/loss.txt", std::ios::out);
    ofs_score.open(result_dir + "/anomaly_score.txt", std::ios::out);
    while (dataloader(data)){

        image = std::get<0>(data).to(device);

        if (!device.is_cpu()) torch::cuda::synchronize();
        start = std::chrono::system_clock::now();

        z = enc->forward(image);
        output = gen->forward(z);
        anomaly_score_with_alpha = AnomalyScore(image, output, z, dis, vm["test_Lambda"].as<float>());
        anomaly_score = std::get<0>(anomaly_score_with_alpha);
        res_loss = std::get<1>(anomaly_score_with_alpha);
        dis_loss = std::get<2>(anomaly_score_with_alpha);

        if (!device.is_cpu()) torch::cuda::synchronize();
        end = std::chrono::system_clock::now();
        seconds = (double)std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() * 0.001 * 0.001;

        ave_anomaly_score += anomaly_score.item<float>();
        ave_res_loss += res_loss.item<float>();
        ave_dis_loss += dis_loss.item<float>();
        ave_time += seconds;

        std::cout << '<' << std::get<1>(data).at(0) << "> anomaly_score:" << anomaly_score.item<float>() << " res:" << res_loss.item<float>() << " dis:" << dis_loss.item<float>() << std::endl;
        ofs << '<' << std::get<1>(data).at(0) << "> anomaly_score:" << anomaly_score.item<float>() << " res:" << res_loss.item<float>() << " dis:" << dis_loss.item<float>() << std::endl;
        ofs_score << anomaly_score.item<float>() << std::endl;

        fname = result_dir + '/' + std::get<1>(data).at(0);
        visualizer::save_image(output.detach(), fname, /*range=*/output_range, /*cols=*/1, /*padding=*/0);

    }

    // (5) Calculate Average
    ave_anomaly_score = ave_anomaly_score / (double)dataset.size();
    ave_res_loss = ave_res_loss / (double)dataset.size();
    ave_dis_loss = ave_dis_loss / (double)dataset.size();
    ave_time = ave_time / (double)dataset.size();

    // (6) Average Output
    std::cout << "<All> anomaly_score:" << ave_anomaly_score << " res:" << ave_res_loss << " dis:" << ave_dis_loss << " (time:" << ave_time << ')' << std::endl;
    ofs << "<All> anomaly_score:" << ave_anomaly_score << " res:" << ave_res_loss << " dis:" << ave_dis_loss << " (time:" << ave_time << ')' << std::endl;

    // Post Processing
    ofs.close();
    ofs_score.close();

    // End Processing
    return;

}

// ---------------------------------------------
// Function to Calculate Anomaly Score
// ---------------------------------------------
std::tuple<torch::Tensor, torch::Tensor, torch::Tensor> AnomalyScore(torch::Tensor image, torch::Tensor fake_image, torch::Tensor z, GAN_Discriminator &dis, const float Lambda){
    torch::Tensor res_loss = torch::abs(image - fake_image).sum();
    torch::Tensor image_feature = dis->forward(image, z).second;
    torch::Tensor fake_image_feature = dis->forward(fake_image, z).second;
    torch::Tensor dis_loss = torch::abs(image_feature - fake_image_feature).sum();
    torch::Tensor anomaly_score = (1.0 - Lambda) * res_loss + Lambda * dis_loss;
    return {anomaly_score, res_loss, dis_loss};
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Clement Jamin
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Spatial_searching - test Kd_tree_search
#include <boost/test/unit_test.hpp>

#include <gudhi/Kd_tree_search.h>

#include <CGAL/Epick_d.h>
#include <CGAL/Random.h>

#include <vector>

BOOST_AUTO_TEST_CASE(test_Kd_tree_search) {
  typedef CGAL::Epick_d<CGAL::Dimension_tag<4> > K;
  typedef K::FT FT;
  typedef K::Point_d Point;
  typedef std::vector<Point> Points;

  typedef Gudhi::spatial_searching::Kd_tree_search<
      K, Points> Points_ds;

  CGAL::Random rd;

  Points points;
  for (int i = 0; i < 500; ++i)
    points.push_back(Point(rd.get_double(-1., 1), rd.get_double(-1., 1), rd.get_double(-1., 1), rd.get_double(-1., 1)));

  Points_ds points_ds(points);

  // Test k_nearest_neighbors
  std::size_t closest_pt_index =
      points_ds.k_nearest_neighbors(points[10], 1, false).begin()->first;
  BOOST_CHECK(closest_pt_index == 10);

  auto kns_range = points_ds.k_nearest_neighbors(points[20], 10, true);

  std::vector<std::size_t> knn_result;
  FT last_dist = -1.;
  for (auto const& nghb : kns_range) {
    BOOST_CHECK(nghb.second > last_dist);
    knn_result.push_back(nghb.second);
    last_dist = nghb.second;
  }

  // Test incremental_nearest_neighbors
  closest_pt_index =
      points_ds.incremental_nearest_neighbors(points[10]).begin()->first;
  BOOST_CHECK(closest_pt_index == 10);

  auto inn_range = points_ds.incremental_nearest_neighbors(points[20]);

  std::vector<std::size_t> inn_result;
  last_dist = -1.;
  auto inn_it = inn_range.begin();
  for (int i = 0; i < 10; ++inn_it, ++i) {
    auto const& nghb = *inn_it;
    BOOST_CHECK(nghb.second > last_dist);
    inn_result.push_back(nghb.second);
    last_dist = nghb.second;
  }

  // Same result for KNN and INN?
  BOOST_CHECK(knn_result == inn_result);

  // Test k_furthest_neighbors
  auto kfn_range = points_ds.k_furthest_neighbors(points[20], 10, true);

  std::vector<std::size_t> kfn_result;
  last_dist = kfn_range.begin()->second;
  for (auto const& nghb : kfn_range) {
    BOOST_CHECK(nghb.second <= last_dist);
    kfn_result.push_back(nghb.second);
    last_dist = nghb.second;
  }

  // Test k_furthest_neighbors
  auto ifn_range = points_ds.incremental_furthest_neighbors(points[20]);

  std::vector<std::size_t> ifn_result;
  last_dist = ifn_range.begin()->second;
  auto ifn_it = ifn_range.begin();
  for (int i = 0; i < 10; ++ifn_it, ++i) {
    auto const& nghb = *ifn_it;
    BOOST_CHECK(nghb.second <= last_dist);
    ifn_result.push_back(nghb.second);
    last_dist = nghb.second;
  }

  // Same result for KFN and IFN?
  BOOST_CHECK(kfn_result == ifn_result);

  // Test all_near_neighbors
  Point rs_q(rd.get_double(-1., 1), rd.get_double(-1., 1), rd.get_double(-1., 1), rd.get_double(-1., 1));
  std::vector<std::size_t> rs_result;
  points_ds.all_near_neighbors(rs_q, 0.5, std::back_inserter(rs_result));
  K k;
  for (auto const& p_idx : rs_result)
    BOOST_CHECK(k.squared_distance_d_object()(points[p_idx], rs_q) <= 0.5);
}


#pragma once

#include <utility>
#include <vector>

#include <Eigen/Dense>

#include "GNAObject.hh"

class GaussLegendre: public GNAObject,
                     public TransformationBind<GaussLegendre> {
  friend class GaussLegendreHist;
public:
  GaussLegendre(std::vector<double> edges,
                std::vector<int> orders)
  : m_edges(std::move(edges)), m_orders(std::move(orders))
  {
    init();
  }

  GaussLegendre(const std::vector<double> &edges,
                size_t orders)
  : m_edges(edges), m_orders(edges.size()-1, static_cast<int>(orders))
  {
    init();
  }

  GaussLegendre(const double *edges, const size_t *orders, size_t bins)
    : m_edges(bins+1), m_orders(bins)
  {
    std::copy(edges, edges+bins+1, m_edges.begin());
    std::copy(orders, orders+bins, m_orders.begin());
    init();
  }

  GaussLegendre(const double *edges, size_t order, size_t bins)
    : m_edges(bins+1), m_orders(bins, order)
  {
    std::copy(edges, edges+bins+1, m_edges.begin());
    init();
  }
protected:
  void init();

  std::vector<double> m_edges;
  std::vector<int> m_orders;
  Eigen::ArrayXd m_points;
  Eigen::ArrayXd m_weights;
};

class GaussLegendreHist: public GNASingleObject,
                         public TransformationBind<GaussLegendreHist> {
public:
  GaussLegendreHist(const GaussLegendre *base);
protected:

  const GaussLegendre *m_base;
};


#include <cstring>
#include <fstream>
#include <unistd.h>

#include <NTL/ZZX.h>
#include <NTL/vector.h>

#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

/**
 * 후보자 번호(벡터)를 암호화된 텍스트(Ctxt) 객체를 내보내는 모듈
 *
 * 공개키가 필요하며 공개키는 owner를 보고 찾아냄
 * 파라미터로는 o와 t가 있음
 * o는 owner의 주소
 * t는 후보자의 총 숫자
 *
 * 내보내는 파일명은 후보자 번호와 owner를 포함함
 *
 */
int main(int argc, char *argv[]) {
    ArgMapping amap;

    string owner = "owner";
    string voter = "voter";
    string dir = "data";
    long total = 4;

    amap.arg("o", owner, "owner's address");
    amap.arg("v", voter, "voter's address");
    amap.arg("t", total, "candidate's total number");
    amap.arg("dir", dir, "save directory");
    amap.parse(argc, argv);

    // file names
    const string publicKeyBinaryFileName = dir + "/publicKey/" + owner + ".bin";
    cout << publicKeyBinaryFileName << flush;

    // get publicKeyFile
    ifstream publicBinFile(publicKeyBinaryFileName.c_str(), ios::binary);
    assert(publicBinFile.is_open());

    // get CandidateFiles
    fstream candidateFile[total];
    for(long i = 0; i < total; i++) {
        string candidatesFileName = dir + "/candidate/" + owner + "-" + to_string(i) + "-" + voter + ".txt";
        candidateFile[i] = fstream(candidatesFileName.c_str(), fstream::out | fstream::trunc);
        assert(candidateFile[i].is_open());
    }

    // Read in context,
    std::unique_ptr<FHEcontext> context = buildContextFromBinary(publicBinFile);
    readContextBinary(publicBinFile, *context);

    // Read in SecKey and PubKey.
    // Got to insert pubKey into seckey obj first.
    std::unique_ptr<FHESecKey> secKey(new FHESecKey(*context));
    FHEPubKey *pubKey = (FHEPubKey *) secKey.get();

    // read publicKey
    readPubKeyBinary(publicBinFile, *pubKey);

    cout << "read publicKey successful.\n" << flush;

    publicBinFile.close();

    // make encryption texts
    for(long i = 0; i < total; i++) {
        // ready to encryption
        Ctxt encryptionText(*pubKey);
        Vec <ZZ> resultPoly;
        resultPoly.SetLength(total);

        // set poly
        for (long j = 0; j < total; j++) {
            if (j == i) resultPoly[j] = 1;
            else resultPoly[j] = 0;
        }

        // encrypt poly
        pubKey->Encrypt(encryptionText, to_ZZX(resultPoly));
        candidateFile[i] << encryptionText << endl;

//        cout << "array : " << resultPoly << endl;
    }

    for(long i = 0; i < total; i++) {
        candidateFile[i].close();
    }

    cout << "encrypt candidates successful.\n\n";
}


#pragma once
#ifndef DSFMTAVXDC_HPP
#define DSFMTAVXDC_HPP
/**
 * @file dSFMTAVXdc.hpp
 */

#include "devavxprng.h"
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include <MTToolBox/AlgorithmCalculateParity.hpp>
#include <MTToolBox/AlgorithmEquidistribution.hpp>
#include <MTToolBox/MersenneTwister64.hpp>
#include <NTL/GF2X.h>
#include "AlgorithmDSFMTEquidistribution.hpp"
#include "Annihilate.hpp"
#include "AlgorithmCalcFixPoint.hpp"
#include "DCOptions.hpp"

namespace MTToolBox {
    /**
     * search parameters using all_in_one function in the file search_all.hpp
     * @param opt command line options
     * @param count number of parameters user requested
     * @return 0 if this ends normally
     */
    template<typename U, typename G, int bitWidth>
    int dsfmtavx_search(DCOptions& opt, int count) {
        using namespace std;
        using namespace NTL;

        MersenneTwister64 mt(opt.seed);
        G g(opt.mexp);

        cout << "#seed = " << dec << opt.seed << endl;
        if (opt.verbose) {
            time_t t = time(NULL);
            cout << "search start at " << ctime(&t);
        }
        if (opt.fixedL) {
            g.setFixedSL1(opt.fixedSL1);
        }
        if (opt.fixedP) {
            g.setFixedPerm(opt.fixedPerm);
        }
        AlgorithmReducibleRecursionSearch<U> ars(g, mt);
        int i = 0;
        AlgorithmCalculateParity<U, G> cp;
        Annihilate<G, U> annihilate;
        cout << "# " << g.getHeaderString() << ", delta52"
             << endl;
        while (i < count) {
            if (ars.start(opt.mexp * 1000)) {
                GF2X irreducible = ars.getIrreducibleFactor();
                GF2X characteristic = ars.getCharacteristicPolynomial();
                if (deg(irreducible) != opt.mexp) {
                    cout << "error mexp = " << dec << opt.mexp << " deg = "
                         << dec << deg(irreducible) << endl;
                    return -1;
                }
                annihilate.getLCMPoly(characteristic, g);
                GF2X quotient = characteristic / irreducible;
                U fixpoint
                    = calc_fixpoint<G, U>(g, irreducible, quotient);
                g.setFixPoint(fixpoint);
                cp.searchParity(g, irreducible);
                U seed;
                setBitOfPos(&seed, 0, 1);
                g.seed(seed);
                if (!annihilate.anni(g)) {
                    return -1;
                }
                int veq52[52];
                DSFMTInfo info;
                info.bitSize = bitWidth;
                info.elementNo = bitWidth / 64;
                int delta52
                    = calc_dSFMT_equidistribution<U, G>
                    (g, veq52, 52, info, opt.mexp);
                cout << g.getParamString();
                cout << dec << delta52;
                cout << "," << veq52[51] << endl;
                //cout << endl;
                i++;
            } else {
                cout << "search failed" << endl;
                break;
            }
        }
        if (opt.verbose) {
            time_t t = time(NULL);
            cout << "search end at " << ctime(&t) << endl;
        }
        return 0;
    }
}

#endif // DSFMTAVX512FDC_HPP


#include <benchmark/benchmark.h>
#include <Eigen/Core>
#include "wave/odometry/geometry/line.hpp"
#include "wave/odometry/geometry/plane.hpp"

static void BM_LINE_ERROR_ONLY(benchmark::State &state) {
    wave::Vec3 pt;
    wave::VecE<wave::MatX> jacsw1, jacsw2;
    wave::VecE<const wave::MatX*> jw1, jw2;
    jacsw1.resize(4);
    jacsw2.resize(4);
    for (uint32_t i = 0; i < jacsw1.size(); ++i) {
        jacsw1.at(i).resize(6,6);
        jacsw2.at(i).resize(6,6);
        jacsw1.at(i).setRandom();
        jacsw2.at(i).setRandom();
        jw1.emplace_back(&(jacsw1.at(i)));
        jw2.emplace_back(&(jacsw2.at(i)));
    }

    double w1, w2;
    w1 = 0.2;
    w2 = 0.8;

    wave::LineResidual<double, 6, 12, 6, 12> residual(pt.data(), jw1, jw2, w1, w2);

    wave::Vec6 line;
    line.setRandom();
    line.block<3,1>(0,0).normalize();

    double residuals[3];
    double *parameters[1];
    parameters[0] = line.data();

    for (auto i : state) {
        residual.Evaluate(parameters, residuals, nullptr);
    }
}
BENCHMARK(BM_LINE_ERROR_ONLY);

static void BM_LINE_WITH_JACOBIAN(benchmark::State &state) {
    wave::Vec3 pt;
    wave::VecE<wave::MatX> jacsw1, jacsw2;
    wave::VecE<const wave::MatX*> jw1, jw2;
    jacsw1.resize(4);
    jacsw2.resize(4);
    for (uint32_t i = 0; i < jacsw1.size(); ++i) {
        jacsw1.at(i).resize(6,6);
        jacsw2.at(i).resize(6,6);
        jacsw1.at(i).setRandom();
        jacsw2.at(i).setRandom();
        jw1.emplace_back(&(jacsw1.at(i)));
        jw2.emplace_back(&(jacsw2.at(i)));
    }

    double w1, w2;
    w1 = 0.2;
    w2 = 0.8;

    wave::LineResidual<double, 6, 12, 6, 12> residual(pt.data(), jw1, jw2, w1, w2);

    wave::Vec6 line;
    line.setRandom();
    line.block<3,1>(0,0).normalize();

    double residuals[3];
    double *parameters[1];
    parameters[0] = line.data();

    Eigen::Matrix<double, 3, 6, Eigen::RowMajor> J_line;
    Eigen::Matrix<double, 3, 12, Eigen::RowMajor> J_T1;
    Eigen::Matrix<double, 3, 6, Eigen::RowMajor> J_V1;
    Eigen::Matrix<double, 3, 12, Eigen::RowMajor> J_T2;
    Eigen::Matrix<double, 3, 6, Eigen::RowMajor> J_V2;

    double *jacobians[5];
    jacobians[0] = J_line.data();
    jacobians[1] = J_T1.data();
    jacobians[2] = J_V1.data();
    jacobians[3] = J_T2.data();
    jacobians[4] = J_V2.data();

    for (auto i : state) {
        residual.Evaluate(parameters, residuals, jacobians);
    }
}
BENCHMARK(BM_LINE_WITH_JACOBIAN);

static void BM_PLANE_ERROR_ONLY(benchmark::State &state) {
    wave::Vec3 pt;
    wave::VecE<wave::MatX> jacsw1, jacsw2;
    wave::VecE<const wave::MatX*> jw1, jw2;
    jacsw1.resize(4);
    jacsw2.resize(4);
    for (uint32_t i = 0; i < jacsw1.size(); ++i) {
        jacsw1.at(i).resize(6,6);
        jacsw2.at(i).resize(6,6);
        jacsw1.at(i).setRandom();
        jacsw2.at(i).setRandom();
        jw1.emplace_back(&(jacsw1.at(i)));
        jw2.emplace_back(&(jacsw2.at(i)));
    }

    double w1, w2;
    w1 = 0.2;
    w2 = 0.8;

    wave::PlaneResidual<double, 6, 12, 6, 12> residual(pt.data(), jw1, jw2, w1, w2);

    wave::Vec6 plane;
    plane.setRandom();
    plane.block<3,1>(0,0).normalize();

    double residuals[1];
    double *parameters[1];
    parameters[0] = plane.data();

    for (auto i : state) {
        residual.Evaluate(parameters, residuals, nullptr);
    }
}
BENCHMARK(BM_PLANE_ERROR_ONLY);

static void BM_PLANE_WITH_JACOBIAN(benchmark::State &state) {
    wave::Vec3 pt;
    wave::VecE<wave::MatX> jacsw1, jacsw2;
    wave::VecE<const wave::MatX*> jw1, jw2;
    jacsw1.resize(4);
    jacsw2.resize(4);
    for (uint32_t i = 0; i < jacsw1.size(); ++i) {
        jacsw1.at(i).resize(6,6);
        jacsw2.at(i).resize(6,6);
        jacsw1.at(i).setRandom();
        jacsw2.at(i).setRandom();
        jw1.emplace_back(&(jacsw1.at(i)));
        jw2.emplace_back(&(jacsw2.at(i)));
    }

    double w1, w2;
    w1 = 0.2;
    w2 = 0.8;

    wave::PlaneResidual<double, 6, 12, 6, 12> residual(pt.data(), jw1, jw2, w1, w2);

    wave::Vec6 plane;
    plane.setRandom();
    plane.block<3,1>(0,0).normalize();

    double residuals[1];
    double *parameters[1];
    parameters[0] = plane.data();

    Eigen::Matrix<double, 1, 6, Eigen::RowMajor> J_line;
    Eigen::Matrix<double, 1, 12, Eigen::RowMajor> J_T1;
    Eigen::Matrix<double, 1, 6, Eigen::RowMajor> J_V1;
    Eigen::Matrix<double, 1, 12, Eigen::RowMajor> J_T2;
    Eigen::Matrix<double, 1, 6, Eigen::RowMajor> J_V2;

    double *jacobians[5];
    jacobians[0] = J_line.data();
    jacobians[1] = J_T1.data();
    jacobians[2] = J_V1.data();
    jacobians[3] = J_T2.data();
    jacobians[4] = J_V2.data();

    for (auto i : state) {
        residual.Evaluate(parameters, residuals, jacobians);
    }
}
BENCHMARK(BM_PLANE_WITH_JACOBIAN);

// Ensure that StateIterator provides all the necessary typedefs required to
// instantiate std::iterator_traits.
static_assert(std::is_same<typename std::iterator_traits<benchmark::State::StateIterator>::value_type,
                           typename benchmark::State::StateIterator::value_type>::value,
              "");

BENCHMARK_MAIN();


#include "CALPHADFunctions.h"
#include "InterpolationType.h"
#include "functions.h"
#include "xlogx.h"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <iostream>
#include <string>

#include <omp.h>

namespace pt = boost::property_tree;

using namespace Thermo4PFM;

int main(int argc, char* argv[])
{
#ifdef _OPENMP
    std::cout << "Run test with " << omp_get_max_threads() << " threads"
              << std::endl;
#endif

    const double cmin     = 0.1;
    const double cmax     = 0.9;
    const int nCintervals = 10;
    const double deltaC   = (cmax - cmin) / (double)nCintervals;

    std::vector<double> cl(nCintervals + 1);
    std::vector<double> cs(nCintervals + 1);

    {
        // serial loop
        for (int i = 0; i < nCintervals + 1; i++)
        {
            const double c = cmin + i * deltaC;

            // compute concentrations in each phase
            double val = xlogx(c);

            std::cout << "Concentration = " << c << ", xlogx = " << val
                      << std::endl;
        }
    }

// parallel loop
#pragma omp target
#pragma omp parallel for
    for (int i = 0; i < nCintervals + 1; i++)
    {
        const double c = cmin + i * deltaC;

        // compute concentrations in each phase
        double val = xlogx(c);

        double val2 = interp_func(ConcInterpolationType::PBG, 0.5);

        double val3 = CALPHADcomputeFMixBinary(1.2, 1.5, 1.6, 1.7, 0.5);

        //        REQUIRE(conc[0] == Approx(cl[i]).margin(1.e-1));
        //        REQUIRE(conc[1] == Approx(cs[i]).margin(1.e-1));
    }
}


/*
 *  Copyright (C) 2018 Naomasa Matsubayashi
 *  Licensed under MIT license, see file LICENSE in this source tree.
 */
#ifndef UWG_INITIATOR_HPP
#define UWG_INITIATOR_HPP
#include <iterator>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/qi.hpp>
#include <uwg/hash.hpp>
#include <uwg/mac.hpp>
#include <uwg/kdf.hpp>
#include <uwg/dh.hpp>
#include <uwg/aead.hpp>
#include <uwg/timestamp.hpp>
#include <uwg/initial_key.hpp>
#include <uwg/clear_key.hpp>
#include <uwg/types.hpp>
#include <uwg/dump.hpp>
namespace uwg {
  template< typename Out, typename CookieType >
  void initiator_hello_phase1( kx_state &stat, Out &out, const wg_key_type &self_static_private, const wg_key_type &self_static_public, const wg_key_type &remote_static_public, uint32_t self_spi, const CookieType &cookie ) {
    wg_key_type kdf_key;
    wg_key_type aead_key;
    wg_key_type mac_key;
    wg_tai64n_type self_timestamp;
    dh_generate( stat.self_ephemeral_private, stat.self_ephemeral_public );
    get_initial_chain_key( stat.chain_key );
    get_initial_hash_key( stat.hash_key, stat.chain_key );
    kdf( stat.chain_key ).update( stat.self_ephemeral_public ).get( stat.chain_key );
    hash().update( stat.hash_key, remote_static_public ).get( stat.hash_key );
    hash().update( stat.hash_key, stat.self_ephemeral_public ).get( stat.hash_key );
    dh( kdf_key, stat.self_ephemeral_private, remote_static_public );
    kdf( stat.chain_key ).update( kdf_key ).get( stat.chain_key, aead_key );
    out.resize( wg_kx1_len, 0u );
    auto encrypted_static = make_svv( out.data(), wg_kx1_encrypted_static_offset, wg_encrypted_static_len );
    aead_enc( encrypted_static, aead_key, 0ull, self_static_public, stat.hash_key );
    hash().update( stat.hash_key, encrypted_static ).get( stat.hash_key );
    dh( kdf_key, self_static_private, remote_static_public );
    kdf( stat.chain_key ).update( kdf_key ).get( stat.chain_key, aead_key );
    timestamp( std::back_inserter( self_timestamp ) );
    auto encrypted_timestamp = make_svv( out.data(), wg_kx1_encrypted_tai64n_offset, wg_encrypted_tai64n_len );
    aead_enc( encrypted_timestamp, aead_key, 0ull, self_timestamp, stat.hash_key );
    hash().update( stat.hash_key, encrypted_timestamp ).get( stat.hash_key );
    out[ 0 ] = 0x01;
    namespace karma = boost::spirit::karma;
    karma::generate( std::next( out.begin(), wg_kx1_self_spi_offset ), karma::little_dword, self_spi );
    std::copy( stat.self_ephemeral_public.begin(), stat.self_ephemeral_public.end(), std::next( out.begin(), wg_kx1_ephemeral_offset ) );
    constexpr static char label_mac1[] = "mac1----";
    auto mac1_message = make_svv( out.data(), 0, wg_kx1_mac1_message_len );
    auto mac1 = make_svv( out.data(), wg_kx1_mac1_offset, wg_mac_len );
    hash().update( label_mac1, label_mac1 + strlen( label_mac1 ) ).update( remote_static_public ).get( mac_key );
    mac( mac_key ).update( mac1_message ).get( mac1 );
    stat.mac1.resize( mac1.size() );
    std::copy( mac1.begin(), mac1.end(), stat.mac1.begin() );
    if( !cookie.empty() ) {
      auto mac2_message = make_svv( out.data(), 0, wg_kx1_mac2_message_len );
      auto mac2 = make_svv( out.data(), wg_kx1_mac2_offset, wg_mac_len );
      mac( cookie ).update( mac2_message ).get( mac2 );
    }
    clear_key( kdf_key );
    clear_key( aead_key );
    clear_key( mac_key );
  }

  template< typename In >
  void initiator_hello_phase2( key_state &key, const wg_key_type &self_static_private, const wg_key_type &self_static_public, const wg_key_type &/*remote_static_public*/, uint32_t /*self_spi*/, kx_state &stat, const In &in ) {
    if( in.size() != wg_kx2_len ) throw invalid_packet();
    if( in[ 0 ] != 0x02 ) throw invalid_packet();
    namespace qi = boost::spirit::qi;
    uint32_t remote_spi;
    qi::parse( std::next( in.begin(), wg_kx1_self_spi_offset ), std::next( in.begin(), wg_kx1_self_spi_offset + wg_spi_len ), qi::little_dword, remote_spi );
    auto encrypted_empty = make_svv( in.data(), wg_kx2_encrypted_empty_offset, wg_encrypted_empty_len );
    auto remote_ephemeral_public = make_svv( in.data(), wg_kx2_ephemeral_offset, wg_ephemeral_len );
    wg_key_type kdf_key;
    wg_key_type aead_key;
    wg_key_type q( wg_key_len, 0u );
    wg_key_type t;
    wg_key_type empty;
    wg_key_type calculated_kx2_mac1;
    wg_key_type mac_key;
    auto kx2_mac1_message = make_svv( in.data(), 0, wg_kx2_mac1_message_len );
    auto incoming_kx2_mac1 = make_svv( in.data(), wg_kx2_mac1_offset, wg_mac_len );
    constexpr static char label_mac1[] = "mac1----";
    hash().update( label_mac1, label_mac1 + strlen( label_mac1 ) ).update( self_static_public ).get( mac_key );
    mac( mac_key ).update( kx2_mac1_message ).get( calculated_kx2_mac1 );
    if( !std::equal( calculated_kx2_mac1.begin(), calculated_kx2_mac1.end(), incoming_kx2_mac1.begin(), incoming_kx2_mac1.end() ) )
      throw invalid_packet();
    kdf( stat.chain_key ).update( remote_ephemeral_public ).get( stat.chain_key );
    hash().update( stat.hash_key, remote_ephemeral_public ).get( stat.hash_key );
    dh( kdf_key, stat.self_ephemeral_private, remote_ephemeral_public );
    kdf( stat.chain_key ).update( kdf_key ).get( stat.chain_key );
    dh( kdf_key, self_static_private, remote_ephemeral_public );
    kdf( stat.chain_key ).update( kdf_key ).get( stat.chain_key );
    kdf( stat.chain_key ).update( q ).get( stat.chain_key, t, aead_key );
    hash().update( stat.hash_key, t ).get( stat.hash_key );
    if( !aead_dec( empty, aead_key, 0ull, encrypted_empty, stat.hash_key ) )
      throw invalid_packet();
    hash().update( stat.hash_key, encrypted_empty ).get( stat.hash_key );
    kdf( stat.chain_key ).update( empty ).get( key.send_key, key.receive_key );
    clear_key( kdf_key );
    clear_key( aead_key );
    clear_key( mac_key );
    clear_key( q );
    clear_key( t );
    clear_key( stat.self_ephemeral_private );
    clear_key( stat.self_ephemeral_public );
    clear_key( stat.chain_key );
    clear_key( stat.hash_key );
  }
}

#endif


/*
 * Copyright (c) Koji Terada
 * 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 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 Wim Meeussen */

// To get M_PI, especially on Windows.

#ifdef _MSC_VER
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#endif

#include <cmath>
#include <memory>

#include <Eigen/Geometry>  // NOLINT

#include "gtest/gtest.h"

#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/clock.hpp"

#include "tf2/convert.h"
#include "tf2/transform_datatypes.h"
#include "tf2_eigen/tf2_eigen.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

TEST(TfEigen, ConvertVector3dStamped)
{
  const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1, 2, 3), tf2::TimePoint(
      std::chrono::seconds(5)), "test");

  tf2::Stamped<Eigen::Vector3d> v1;
  geometry_msgs::msg::PointStamped p1;
  tf2::convert(v, p1);
  tf2::convert(p1, v1);

  EXPECT_EQ(v, v1);
}

// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented
// TEST(TfEigen, ConvertVector3d)
// {
//   const Eigen::Vector3d v(1,2,3);

//   Eigen::Vector3d v1;
//   geometry_msgs::msg::Point p1;
//   tf2::convert(v, p1);
//   tf2::convert(p1, v1);

//   EXPECT_EQ(v, v1);
// }

TEST(TfEigen, ConvertAffine3dStamped)
{
  const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis<double>(
      1, Eigen::Vector3d::UnitX()));
  const tf2::Stamped<Eigen::Affine3d> v(v_nonstamped, tf2::TimePoint(
      std::chrono::seconds(42)), "test_frame");

  tf2::Stamped<Eigen::Affine3d> v1;
  geometry_msgs::msg::PoseStamped p1;
  tf2::convert(v, p1);
  tf2::convert(p1, v1);

  EXPECT_EQ(v.translation(), v1.translation());
  EXPECT_EQ(v.rotation(), v1.rotation());
  EXPECT_EQ(v.frame_id_, v1.frame_id_);
  EXPECT_EQ(v.stamp_, v1.stamp_);
}

// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented
// TEST(TfEigen, ConvertAffine3d)
// {
//   const Eigen::Affine3d v(
//     Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));

//   Eigen::Affine3d v1;
//   geometry_msgs::msg::Pose p1;
//   tf2::convert(v, p1);
//   tf2::convert(p1, v1);

//   EXPECT_EQ(v.translation(), v1.translation());
//   EXPECT_EQ(v.rotation(), v1.rotation());
// }

TEST(TfEigen, ConvertTransform)
{
  Eigen::Matrix4d tm;

  double alpha = M_PI / 4.0;
  double theta = M_PI / 6.0;
  double gamma = M_PI / 12.0;

  tm << cos(theta) * cos(gamma), -cos(theta) * sin(gamma), sin(theta), 1,
    cos(alpha) * sin(gamma) + sin(alpha) * sin(theta) * cos(gamma),
    cos(alpha) * cos(gamma) - sin(alpha) * sin(theta) * sin(gamma), -sin(alpha) * cos(theta), 2,
    sin(alpha) * sin(gamma) - cos(alpha) * sin(theta) * cos(gamma),
    cos(alpha) * sin(theta) * sin(gamma) + sin(alpha) * cos(gamma), cos(alpha) * cos(theta), 3,
    0, 0, 0, 1;

  Eigen::Affine3d T(tm);

  geometry_msgs::msg::TransformStamped msg = tf2::eigenToTransform(T);
  Eigen::Affine3d Tback = tf2::transformToEigen(msg);

  EXPECT_TRUE(T.isApprox(Tback));
  EXPECT_TRUE(tm.isApprox(Tback.matrix()));

  // same for Isometry
  Eigen::Isometry3d I(tm);

  msg = tf2::eigenToTransform(T);
  Eigen::Isometry3d Iback = tf2::transformToEigen(msg);

  EXPECT_TRUE(I.isApprox(Iback));
  EXPECT_TRUE(tm.isApprox(Iback.matrix()));
}

struct EigenBufferTransform : public ::testing::Test
{
  static void SetUpTestSuite()
  {
    rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
    tf_buffer = std::make_unique<tf2_ros::Buffer>(clock);
    tf_buffer->setUsingDedicatedThread(true);

    // populate buffer
    geometry_msgs::msg::TransformStamped t;
    t.transform.translation.x = 10;
    t.transform.translation.y = 20;
    t.transform.translation.z = 30;
    t.transform.rotation.w = 0;
    t.transform.rotation.x = 1;
    t.transform.rotation.y = 0;
    t.transform.rotation.z = 0;
    t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2));
    t.header.frame_id = "A";
    t.child_frame_id = "B";
    tf_buffer->setTransform(t, "test");
  }

  template<int mode>
  void testEigenTransform();

  ::testing::AssertionResult doTestEigenQuaternion(
    const Eigen::Quaterniond & parameter, const Eigen::Quaterniond & expected);

  static std::unique_ptr<tf2_ros::Buffer> tf_buffer;
  static constexpr double EPS = 1e-3;
};

std::unique_ptr<tf2_ros::Buffer> EigenBufferTransform::tf_buffer;

template<int mode>
void EigenBufferTransform::testEigenTransform()
{
  using T = Eigen::Transform<double, 3, mode>;
  using stampedT = tf2::Stamped<T>;

  const stampedT i1{
    T{Eigen::Translation3d{1, 2, 3} *Eigen::Quaterniond{0, 1, 0, 0}}, tf2::timeFromSec(2), "A"};

  // simple api
  const stampedT i_simple = tf_buffer->transform(i1, "B", tf2::durationFromSec(2.0));
  EXPECT_NEAR(i_simple.translation().x(), -9, EPS);
  EXPECT_NEAR(i_simple.translation().y(), 18, EPS);
  EXPECT_NEAR(i_simple.translation().z(), 27, EPS);
  const auto q1 = Eigen::Quaterniond(i_simple.linear());
  EXPECT_NEAR(q1.x(), 0.0, EPS);
  EXPECT_NEAR(q1.y(), 0.0, EPS);
  EXPECT_NEAR(q1.z(), 0.0, EPS);
  EXPECT_NEAR(q1.w(), 1.0, EPS);

  // advanced api
  const stampedT v_advanced =
    tf_buffer->transform(i1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0));
  EXPECT_NEAR(v_advanced.translation().x(), -9, EPS);
  EXPECT_NEAR(v_advanced.translation().y(), 18, EPS);
  EXPECT_NEAR(v_advanced.translation().z(), 27, EPS);
  const auto q2 = Eigen::Quaterniond(v_advanced.linear());
  EXPECT_NEAR(q2.x(), 0.0, EPS);
  EXPECT_NEAR(q2.y(), 0.0, EPS);
  EXPECT_NEAR(q2.z(), 0.0, EPS);
  EXPECT_NEAR(q2.w(), 1.0, EPS);
}

TEST_F(EigenBufferTransform, Affine3d) {
  testEigenTransform<Eigen::Affine>();
}

TEST_F(EigenBufferTransform, Isometry3d) {
  testEigenTransform<Eigen::Isometry>();
}

TEST_F(EigenBufferTransform, Vector)
{
  const tf2::Stamped<Eigen::Vector3d> v1{{1, 2, 3}, tf2::timeFromSec(2), "A"};

  // simple api
  const tf2::Stamped<Eigen::Vector3d> v_simple =
    tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0));
  EXPECT_NEAR(v_simple.x(), -9, EPS);
  EXPECT_NEAR(v_simple.y(), 18, EPS);
  EXPECT_NEAR(v_simple.z(), 27, EPS);

  // advanced api
  const tf2::Stamped<Eigen::Vector3d> v_advanced =
    tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0));
  EXPECT_NEAR(v_advanced.x(), -9, EPS);
  EXPECT_NEAR(v_advanced.y(), 18, EPS);
  EXPECT_NEAR(v_advanced.z(), 27, EPS);
}

// Test transformation of a 6-long wrench or twist
TEST_F(EigenBufferTransform, WrenchTransform)
{
  // Transform the wrench (due to gravity) of a point mass to a different frame

  double mass = 1.0;
  double gravity = -9.81;
  // Negative y force, no moment
  Eigen::VectorXd wrench_in(6);
  wrench_in << 0., mass * gravity, 0., 0., 0., 0.;

  // The new frame is not rotated at all but it is offset along x-axis
  double x_offset = -0.1;
  geometry_msgs::msg::TransformStamped tf_to_new_frame;
  tf_to_new_frame.transform.translation.x = x_offset;
  tf_to_new_frame.transform.rotation.w = 1.0;

  Eigen::VectorXd wrench_out(6);
  tf2::doTransform(wrench_in, wrench_out, tf_to_new_frame);

  EXPECT_NEAR(wrench_out(0), 0., EPS);
  EXPECT_NEAR(wrench_out(1), mass * gravity, EPS);
  EXPECT_NEAR(wrench_out(2), 0., EPS);
  EXPECT_NEAR(wrench_out(3), 0., EPS);
  EXPECT_NEAR(wrench_out(4), 0., EPS);
  EXPECT_NEAR(wrench_out(5), mass * gravity * x_offset, EPS);
}

// helper method for Quaternion tests
::testing::AssertionResult EigenBufferTransform::doTestEigenQuaternion(
  const Eigen::Quaterniond & parameter, const Eigen::Quaterniond & expected)
{
  const tf2::Stamped<Eigen::Quaterniond> q1{parameter, tf2::timeFromSec(2), "A"};
  // avoid linking error
  const double eps = EPS;

  // simple api
  const tf2::Stamped<Eigen::Quaterniond> q_simple =
    tf_buffer->transform(q1, "B", tf2::durationFromSec(2.0));
  // compare rotation matrices, as the quaternions can be ambigous
  EXPECT_TRUE(q_simple.toRotationMatrix().isApprox(expected.toRotationMatrix(), eps));

  // advanced api
  const tf2::Stamped<Eigen::Quaterniond> q_advanced =
    tf_buffer->transform(q1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0));
  EXPECT_TRUE(q_advanced.toRotationMatrix().isApprox(expected.toRotationMatrix(), eps));
  return ::testing::AssertionSuccess();
}

TEST_F(EigenBufferTransform, QuaternionRotY)
{
  // rotated by -90° around y
  // 0, 0, -1
  // 0, 1, 0,
  // 1, 0, 0
  const Eigen::Quaterniond param{Eigen::AngleAxisd(-1 * M_PI_2, Eigen::Vector3d::UnitY())};
  const Eigen::Quaterniond expected{0, M_SQRT1_2, 0, -1 * M_SQRT1_2};
  EXPECT_TRUE(doTestEigenQuaternion(param, expected));
}

TEST_F(EigenBufferTransform, QuaternionRotX)
{
  // rotated by 90° around y
  const Eigen::Quaterniond param{Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX())};
  const Eigen::Quaterniond expected{Eigen::AngleAxisd(-1 * M_PI_2, Eigen::Vector3d::UnitX())};
  EXPECT_TRUE(doTestEigenQuaternion(param, expected));
}

TEST_F(EigenBufferTransform, QuaternionRotZ)
{
  // rotated by 180° around z
  const Eigen::Quaterniond param{Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())};
  const Eigen::Quaterniond expected{Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY())};
  EXPECT_TRUE(doTestEigenQuaternion(param, expected));
}

int main(int argc, char ** argv)
{
  testing::InitGoogleTest(&argc, argv);

  return RUN_ALL_TESTS();
}


/*
 * Copyright 2009 Annan Harley
 *
 * 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.
 */

#ifndef _YAFFT_HPP_
#define _YAFFT_HPP_

#include <complex>

#include <boost/shared_ptr.hpp>

#include "yafft_simd.hpp"
#include "yafft_twiddles.hpp"
#include "yafft_unswizzle.hpp"

namespace yafft {

  template <typename FP_T>
  class iyafft_windowfn {
  public:
    virtual ~iyafft_windowfn() {}
    virtual FP_T * const get_window(const size_t windowsz) = 0;
  };

  enum window_t {
    HAMMING,
    HANN,
    GAUSS,
    BLACKMAN
  };

  template <typename FP_T>
  boost::shared_ptr<iyafft_windowfn<FP_T> > get_windowfn(const window_t wt);

  typedef iyafft_windowfn<float> iyafft_float_windowfn_t;
  typedef boost::shared_ptr<iyafft_float_windowfn_t> iyafft_float_windowfn_ptr;

  typedef iyafft_windowfn<double> iyafft_double_windowfn_t;
  typedef boost::shared_ptr<iyafft_double_windowfn_t> iyafft_double_windowfn_ptr;

  template <typename FP_T>
  class iyafft {
  public:
    virtual ~iyafft() {}

    virtual bool init() = 0;

    virtual void run_forward(std::complex<FP_T> * const,
			     std::complex<FP_T> * const) = 0;

    virtual void run_forward(FP_T * const,
			     std::complex<FP_T> * const) = 0;

    virtual void run_reverse(std::complex<FP_T> * const,
			     std::complex<FP_T> * const) = 0;

    virtual void run_reverse(std::complex<FP_T> * const,
			     FP_T * const) = 0;
  };

  template <typename FP_T>
  boost::shared_ptr<iyafft<FP_T> > get_fft(const size_t N);

  typedef boost::shared_ptr<iyafft<float> > iyafft_float_ptr;
  typedef boost::shared_ptr<iyafft<double> > iyafft_double_ptr;

  template <typename FP_T>
  class iyafft_pow2 : public iyafft<FP_T> {
  public:

    virtual ~iyafft_pow2() {}

    virtual void run_forward_nounswizzle(std::complex<FP_T> * const) = 0;

    virtual void run_forward_mul_noout(std::complex<FP_T> * const,
				       FP_T * const) = 0;

    virtual void run_reverse_mul_noout(std::complex<FP_T> * const,
				       FP_T * const) = 0;

    virtual void run_reverse_mul_noin(FP_T * const,
				      FP_T * const,
				      std::complex<FP_T> * const) = 0;

    virtual void run_forward_mul_noin(FP_T * const,
				      FP_T * const,
				      std::complex<FP_T> * const) = 0;

    virtual FP_T * const get_work() = 0;
  };
}

#endif //  _YAFFT_HPP_


// MIT License

// Copyright (c) 2012 Yu Takahashi

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

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

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

/**
@file   PolyhedralSphericalHarmo.hpp
@Author Benjamin Bercovici (bebe0705@colorado.edu) (from the original code by
Yu Takahashi and Siamak Hesar)
@date   October, 2017
@brief  Declaration of the methods enabling the
computation of exterior gravity spherical harmonics
coefficients about a polyhedral shape model.
*/

#ifndef HEADER_SHARMLIB
#define HEADER_SHARMLIB

#include <armadillo>

#define NM_DIM_MAX 70
#define DIM_MAX (NM_DIM_MAX+1)*(NM_DIM_MAX+2)/2

/* macro to access trinomial coefficients */
/* coded as a macro so the compiler can optimize loop invariants */

#define Index(i,j,k,n) ( ((n)-(i)) * ((n)-(i)+1) / 2 + (k) )

namespace SHARMLib {

/**
structure to store trinomial coefficients
**/
    typedef struct {
        int degree;
    double data [DIM_MAX]; /* worst-case dimension */
    } Trinomial;

    void    CalculateBasicTables (int n_degree,
      double (&mixingFactors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
      int (&trinomialCoefficientCount)[NM_DIM_MAX + 1]);
    void    CalculateFullyNormalizedTables (int n_degree,
        double (&diagonalFactors) [NM_DIM_MAX + 1],
        double (&subdiagonalFactors) [NM_DIM_MAX + 1],
        double (&vertical1Factors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
        double (&vertical2Factors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1]);
    void    CalculateUnnormalizedTables (int n_degree,
       double (&diagonalFactors) [NM_DIM_MAX + 1],
       double (&subdiagonalFactors) [NM_DIM_MAX + 1],
       double (&vertical1Factors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
       double (&vertical2Factors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1]);

    double  IntegrateOneSimplex (const Trinomial *tri,
       double (&mixingFactors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1] [NM_DIM_MAX + 1]);
    void    TriAdd  (Trinomial * const result,
       const Trinomial * const left,
       const Trinomial * const right,
       int (&trinomialCoefficientCount)[NM_DIM_MAX + 1]);
    void    TriCopy (Trinomial * const target, const Trinomial * const source,
       int (&trinomialCoefficientCount) [NM_DIM_MAX + 1]);

    void    TriMult (Trinomial * const result, const Trinomial * const left,
       const Trinomial * const right,
       int (&trinomialCoefficientCount) [NM_DIM_MAX + 1]);

    void    TriMultScalar (Trinomial * const result, const double scalar,
     int (&trinomialCoefficientCount) [NM_DIM_MAX + 1]);
    void    TriPrint (const Trinomial * const tri, const char * words);
    void    TriSub  (Trinomial * const result, const Trinomial * const left,
       const Trinomial * const right,
       int (&trinomialCoefficientCount) [NM_DIM_MAX + 1]);

/*
Accumulate contribution of one simplex to potential coefficients Cnm and Snm.
@param n_degree degree of the expansion to build
@param Cnm array of Cnm to be incremented
@param Snm array of Snm to be incremented
@param x0 coordinates x component of the first vertex
@param y0 coordinates y component of the first vertex
@param z0 coordinates z component of the first vertex
@param x1 coordinates x component of the second vertex
@param y1 coordinates y component of the second vertex
@param z1 coordinates z component of the second vertex
@param x2 coordinates x component of the third vertex
@param y2 coordinates y component of the third vertex
@param z2 coordinates y component of the third vertex
@param trinomialCoefficientCount
@param diagonalFactors
@param subdiagonalFactors
@param vertical1Factors
@param vertical2Factors
@param mixingFactors
@param ref_radius reference radius in the expansion
*/
    void AccumulateOneSimplex (
        int n_degree,
        double Cnm [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
        double Snm [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
        double x0, double y0, double z0,
        double x1, double y1, double z1,
        double x2, double y2, double z2,
        int (&trinomialCoefficientCount) [NM_DIM_MAX + 1],
        double (&diagonalFactors) [NM_DIM_MAX + 1],
        double (&subdiagonalFactors) [NM_DIM_MAX + 1],
        double (&vertical1Factors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
        double (&vertical2Factors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
        double (&mixingFactors) [NM_DIM_MAX + 1] [NM_DIM_MAX + 1] [NM_DIM_MAX + 1],
        double ref_radius
        );

/*
Accumulate contribution of one polyhedral element
@param Cnm2f contribution to the total Cnm for this element
@param Snm2f contribution to the total Snm for this element
@param n_degree degree of the expansion to build
@param ref_radius reference radius in the expansion
@param r0 pointer to coordinates of first vertex
@param r1 pointer to coordinates of second vertex
@param r2 pointer to coordinates of third vertex
@param normalized true if the normalized coefficients should be computed
*/
    void ComputePolyhedralCS(
        arma::mat & Cnm2f,
        arma::mat & Snm2f,
        int n_degree,
        double ref_radius,
        double * r0,
        double * r1,
        double * r2,
        bool normalized) ;

    void GetBnmNormalizedExterior(int n_degree,
        arma::mat & b_bar_real,
        arma::mat & b_bar_imag,
        const arma::vec & pos,
        double ref_radius);

}

#endif


#include <boost/simd/ieee/include/functions/fast_ldexp.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/four.hpp>
#include <boost/simd/include/constants/mfour.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/quarter.hpp>

NT2_TEST_CASE_TPL ( fast_ldexp_real,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::fast_ldexp;
  using boost::simd::tag::fast_ldexp_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<fast_ldexp_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  NT2_TEST_EQUAL(fast_ldexp(boost::simd::Mone<T>(), 2), boost::simd::Mfour<r_t>());
  NT2_TEST_EQUAL(fast_ldexp(boost::simd::One<T>(),  2), boost::simd::Four<r_t>());
  NT2_TEST_EQUAL(fast_ldexp(boost::simd::One<T>(), -2), boost::simd::Quarter<r_t>());
}

NT2_TEST_CASE_TPL ( fast_ldexp_si,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::fast_ldexp;
  using boost::simd::tag::fast_ldexp_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<fast_ldexp_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  NT2_TEST_EQUAL(fast_ldexp(boost::simd::Mone<T>(), 2), boost::simd::Mfour<r_t>());
  NT2_TEST_EQUAL(fast_ldexp(boost::simd::One<T>(),  2), boost::simd::Four<r_t>());
  NT2_TEST_EQUAL(fast_ldexp(boost::simd::Zero<T>(), 2), boost::simd::Zero<r_t>());
}

NT2_TEST_CASE_TPL ( fast_ldexp_ui,  BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::fast_ldexp;
  using boost::simd::tag::fast_ldexp_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<fast_ldexp_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  NT2_TEST_EQUAL(fast_ldexp(boost::simd::One<T>(),  2), boost::simd::Four<r_t>());
  NT2_TEST_EQUAL(fast_ldexp(boost::simd::Zero<T>(), 2), boost::simd::Zero<r_t>());
}


#include <map>

using std::map;

#include <boost/log/trivial.hpp>

#include "eigenIncluder.hpp"
#include "algebraTrace.hpp"
#include "rtsSmoothing.hpp"
#include "writeClock.hpp"
#include "acsConfig.hpp"
#include "constants.hpp"
#include "algebra.hpp"
#include "mongo.hpp"
#include "ppp.hpp"

bool isPositiveSemiDefinite(MatrixXd& mat)
{
	for (int i = 0; i < mat.rows(); i++)
	for (int j = 0; j < i; j++)
	{
		double a	= mat(i, i);
		double ab	= mat(i, j);
		double b	= mat(j, j);

		if (ab * ab > a * b)
		{
// 			std::cout << "large off diagonals " << std::endl;
// 			return false;
			if (ab > 0) ab = +sqrt(0.99 * a * b);
			else		ab = -sqrt(0.99 * a * b);
			mat(i, j) = ab;
			mat(j, i) = ab;
		}
	}
	return true;
}

void postRTSActions(
	bool		final,				///< This is a final answer, not intermediate - output to files
	KFState&	kfState,			///< State to get filter traces from
	string		clockFilename,		///< Filename for smoothed clock output
	StationMap*	stationMap_ptr)		///< Pointer to map of stations
{
	std::ofstream ofs(kfState.rts_filename, std::ofstream::out | std::ofstream::app);

	if	(   final
		&&  clockFilename.empty() == false
		&&  acsConfig.output_clocks
		&&( acsConfig.clocks_receiver_source	== +E_Ephemeris::KALMAN
		  ||acsConfig.clocks_satellite_source	== +E_Ephemeris::KALMAN))
	{
		tryPrepareFilterPointers(kfState, stationMap_ptr);
		outputClocks(clockFilename, acsConfig.clocks_receiver_source, acsConfig.clocks_satellite_source, kfState.time, kfState, stationMap_ptr);
	}

	if (final)
	{
		kfState.outputStates(ofs);
	}

#	ifdef ENABLE_MONGODB
	if	(   acsConfig.output_mongo_states
		&&( final
		  ||acsConfig.output_intermediate_rts_mongo_states))
	{
		mongoStates(kfState, acsConfig.mongo_rts_suffix);
	}
#	endif

// 	pppoutstat(ofs, archiveKF, true);
}

/** Output filter states from a reversed binary trace file
*/
void RTS_Output(
	KFState&	kfState,			///< State to get filter traces from
	StationMap*	stationMap_ptr,		///< Pointer to map of stations
	string		clockFilename)		///< Filename to output clocks to once smoothed
{
	string reversedStatesFilename = kfState.rts_filename + BACKWARD_SUFFIX;

	long int startPos = -1;
	while (1)
	{
		E_SerialObject type = getFilterTypeFromFile(startPos, reversedStatesFilename);

		switch (type)
		{
			default:
			{
				std::cout << "UNEXPECTED RTS OUTPUT TYPE";
				return;
			}

			case E_SerialObject::FILTER_PLUS:
			{
				KFState archiveKF;
				bool pass = getFilterObjectFromFile(type, archiveKF, startPos, reversedStatesFilename);

				if (pass == false)
				{
					std::cout << "BAD RTS OUTPUT READ";
					return;
				}

				archiveKF.rts_filename = kfState.rts_filename;
				postRTSActions(true, archiveKF, clockFilename, stationMap_ptr);

				break;
			}
		}

		if (startPos == 0)
		{
			return;
		}
	}
}

KFState RTS_Process(
	KFState&	kfState,
	bool		write,
	StationMap*	stationMap_ptr,
	string		clockFilename)
{
	if (kfState.rts_lag == 0)
	{
		return KFState();
	}

	MatrixXd transistionMatrix;

	KFState kalmanMinus;
	KFState smoothedKF;

	bool smoothedXready = false;
	bool smoothedPready = false;

	string inputFile	= kfState.rts_forward_filename;
	string outputFile	= kfState.rts_filename + BACKWARD_SUFFIX;

	if (write)
	{
		std::ofstream ofs(outputFile,	std::ofstream::out | std::ofstream::trunc);
	}

	long int startPos = -1;
	int lag = 0;
	while (lag != kfState.rts_lag)
	{
		E_SerialObject type = getFilterTypeFromFile(startPos, inputFile);

		if (type == +E_SerialObject::NONE)
		{
			break;
		}

		switch (type)
		{
			default:
			{
				break;
			}
			case E_SerialObject::TRANSITION_MATRIX:
			{
				TransitionMatrixObject transistionMatrixObject;
				bool pass = getFilterObjectFromFile(type, transistionMatrixObject, startPos, inputFile);
				if (pass == false)
				{
					return KFState();
				}

//				std::cout << "Setting transition matrix " << transistionMatrixObject.rows << std::endl;

				transistionMatrix = MatrixXd::Zero(transistionMatrixObject.rows, transistionMatrixObject.cols);

				for (auto& [keyPair, value] : transistionMatrixObject.forwardTransitionMap)
				{
					transistionMatrix(keyPair.first, keyPair.second) = value;
				}

				break;
			}
			case E_SerialObject::FILTER_MINUS:
			{
				bool pass = getFilterObjectFromFile(type, kalmanMinus, startPos, inputFile);
				if (pass == false)
				{
					return KFState();
				}

				if (smoothedXready == false)
				{
					smoothedXready = true;
				}

				break;
			}
			case E_SerialObject::FILTER_PLUS:
			{
				lag++;

				if (write)
				{
					BOOST_LOG_TRIVIAL(info)
					<< "Lag: " << lag;
				}

				KFState kalmanPlus;
				bool pass = getFilterObjectFromFile(type, kalmanPlus, startPos, inputFile);
				if (pass == false)
				{
					return KFState();
				}

				if (smoothedPready == false)
				{
					smoothedPready	= true;
					smoothedKF		= kalmanPlus;

					if (write)
					{
						spitFilterToFile(smoothedKF, E_SerialObject::FILTER_PLUS, outputFile);
					}

					break;
				}

				if (smoothedXready == false)
				{
					break;
				}

				smoothedKF.time = kalmanPlus.time;

				smoothedKF. P	= (smoothedKF.	P	+ smoothedKF.	P.transpose()).eval() / 2;
				kalmanMinus.P	= (kalmanMinus.	P	+ kalmanMinus.	P.transpose()).eval() / 2;
				kalmanPlus. P	= (kalmanPlus.	P	+ kalmanPlus.	P.transpose()).eval() / 2;

				//get process noise and dynamics
				auto& F = transistionMatrix;

				kalmanMinus.P(0,0) = 1;			//todo aaron, undo this later?

				MatrixXd Pinv = kalmanMinus.P.inverse();

				Pinv = (Pinv + Pinv.transpose()).eval()	/ 2;

				MatrixXd Ck = kalmanPlus.P * F.transpose() * Pinv;

				smoothedKF.x = ( kalmanPlus.x + Ck * (smoothedKF.x - kalmanMinus.x)						).eval();
				smoothedKF.P = ( kalmanPlus.P + Ck * (smoothedKF.P - kalmanMinus.P) * Ck.transpose()	).eval();

				smoothedKF.kfIndexMap = kalmanPlus.kfIndexMap;

				if (write)
				{
					spitFilterToFile(smoothedKF, E_SerialObject::FILTER_PLUS, outputFile);
				}
				else
				{
					bool final = false;
					if (lag == kfState.rts_lag)
					{
						final = true;
					}

					smoothedKF.rts_filename = kfState.rts_filename;
					postRTSActions(final, smoothedKF, clockFilename, stationMap_ptr);
				}

				break;
			}
		}

		if (startPos == 0)
		{
			break;
		}
	}

	if (write)
	{
		RTS_Output(kfState, stationMap_ptr, clockFilename);
	}

	if (lag == kfState.rts_lag)
	{
		//delete the beginning of the history file
		string tempFile	= kfState.rts_forward_filename + "_temp";
		{
			std::ofstream	tempStream(tempFile,	std::ifstream::binary | std::ofstream::out | std::ofstream::trunc);
			std::fstream	inputStream(inputFile,	std::ifstream::binary | std::ifstream::in);

			inputStream.seekg(0,	inputStream.end);
			long int lengthPos = inputStream.tellg();

			vector<char>	fileContents(lengthPos - startPos);

			inputStream.seekg(startPos,	inputStream.beg);

			inputStream.read(&fileContents[0], lengthPos - startPos);
			tempStream.write(&fileContents[0], lengthPos - startPos);
		}

		std::remove(inputFile.c_str());
		std::rename(tempFile.c_str(), inputFile.c_str());
	}

	if (lag == kfState.rts_lag)
	{
		return smoothedKF;
	}
	else
	{
		return KFState();
	}
}


#include <string>
#include <vector>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <dlib/string.h>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <dlib/opencv.h>
#include <dlib/image_processing.h>
#include <dlib/image_processing/frontal_face_detector.h>

#include "dlib_resnet_face_recognition.h"

namespace dlib {

// ----------------------------------------------------------------------------------------
// The next bit of code defines a ResNet network. It's basically copied
// and pasted from the dnn_imagenet_ex.cpp example, except we replaced the loss
// layer with loss_metric and made the network somewhat smaller.  Go read the introductory
// dlib DNN examples to learn what all this stuff means.
//
template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
    alevel0<
    alevel1<
    alevel2<
    alevel3<
    alevel4<
    max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
    input_rgb_image_sized<150>
    >>>>>>>>>>>>;

}

void resnet_load (dlib::anet_type &net)
{
    std::string imageRecognitionModelPath("dlib_face_recognition_resnet_model_v1.dat");
    dlib::deserialize(imageRecognitionModelPath) >> net;
}

void resnet_predict (dlib::anet_type &net, cv::Mat &im, std::vector<float> &descriptor)
{
    cv::Mat imRGB;
    cv::cvtColor(im, imRGB, cv::COLOR_GRAY2RGB);
    dlib::matrix<dlib::rgb_pixel> imDlib(dlib::mat(dlib::cv_image<dlib::rgb_pixel>(imRGB)));
    dlib::matrix<float,0,1> imageDescriptor = net(imDlib);
    std::vector<float> imageDescriptorVec(imageDescriptor.begin(), imageDescriptor.end());
    descriptor.swap(imageDescriptorVec);
}

void resnet_descriptor (dlib::anet_type &net, cv::Mat &image, std::vector<float> &descriptor)
{
    cv::Mat input;
    cv::resize(image,input,cv::Size(150,150));
    resnet_predict(net,input,descriptor);
}

static dlib::anet_type net;

void resnet_descriptor_init ()
{
	resnet_load(net);
}

std::vector<float> resnet_descriptor (cv::Mat &image)
{
	std::vector<float> descriptor;
	resnet_descriptor(net,image,descriptor);
	return descriptor;
}

void write_resnet_descriptor (std::string path, int label, std::vector<float> &descriptor)
{
    std::ofstream out;
    out.open(path);
    out << descriptor.size() << " " << label << std::endl;
    for (size_t k=0; k<descriptor.size(); k++) out << descriptor[k] << " ";
    out << std::endl;
    out.close();
}

void read_resnet_descriptor (std::string path, int &label, std::vector<float> &descriptor)
{
    std::ifstream in;
    in.open(path);
    int n=0;
    if (in >> n) {
        if (in >> label) {
            for (int k=0; k<n; k++) {
                float d;
                in >> d;
                descriptor.push_back(d);
            }
        }
    }
    in.close();
}