#include <spatialops/Nebo.h>
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/structured/MatVecFields.h>
#include <spatialops/structured/MatVecOps.h>
#include <spatialops/structured/FieldComparisons.h>
#include <spatialops/structured/FieldHelper.h>
#include <test/TestHelper.h>
#include <spatialops/util/TimeLogger.h>
#include <spatialops/structured/test/MatrixRandomizers.h>

#include <boost/program_options.hpp>
#include <iostream>
#include <numeric>
#include <stdlib.h>
#include <string>
#include <utility>

namespace po = boost::program_options;

using namespace SpatialOps;

template<typename T, size_t S>
size_t array_size(T(&)[S]) { return S; };

//==============================================================================

template<typename FieldT>
void generate_matrix_with_eigen( const typename FieldT::value_type * const eigenValues,
                                 const size_t eigenSize,
                                 FieldMatrix<FieldT>& M,
                                 FieldMatrix<FieldT>& scratchM,
                                 FieldVector<FieldT>& scratchV,
                                 SpatFldPtr<FieldT>&  scratch,
                                 const short int deviceIndex )
{
  assert( !scratch.isnull() );
  assert( eigenSize == M.elements() );
  assert( M.elements() == scratchM.elements() && scratchM.elements() == scratchV.elements() );

  random_matrix( scratchM, MatrixTypes::LINEARLY_INDEPENDENT, deviceIndex, scratch );

  const size_t elements = M.elements();

  //Multiply by transpose scaled by eigenValues
  size_t m = elements;
  size_t k = elements;
  size_t n = elements;
  size_t kn = k*n;
  size_t mk = m*k;

  while( n-- ){
    k = elements;
    while( k-- ){
      scratchV.at(k) <<= eigenValues[k] * scratchM.at(--kn);
    }
    m  = elements;
    mk = elements*elements;
    while( m-- ){
      FieldT& res = M.at(m*elements + n);
      res <<= 0;
      k = elements;
      while( k-- ){
        res <<= res + scratchM.at(--mk) * scratchV.at(k);
      }
    }
  }
}

template<typename FieldT>
void generate_matrix_with_complex_eigen( const typename FieldT::value_type * const eigenValues,
                                         const size_t eigenSize,
                                         FieldMatrix<FieldT>& M,
                                         FieldMatrix<FieldT>& scratchM,
                                         FieldVector<FieldT>& scratchV,
                                         SpatFldPtr<FieldT>&  scratch,
                                         const short int deviceIndex )
{
  assert( !scratch.isnull() );
  assert( M.elements() % 2 == 0 );  //Can only generate matrices of 2n x 2n
  assert( eigenSize == (M.elements()/2) );  //Complex comes in +- pairs
  assert( M.elements() == scratchM.elements() && scratchM.elements() == scratchV.elements() );

  const size_t elements = M.elements();

  size_t m = elements;
  size_t twiceN = elements;

  //Assign eigen values on block diagonal top right and bottom left (block size 2)
  m = elements;
  twiceN = elements;
  while( m-- ){
    twiceN = elements;
    while( twiceN-- ){
      FieldT& res = M.at(m*elements + twiceN);

      //Set top right block value to positive eigenvalue
      if( m % 2 == 0 && twiceN == m + 1 )
      {
        res <<= eigenValues[(m/2)];
      }
      else if( m % 2 == 1 && twiceN == m - 1 )
      {
        res <<= -eigenValues[(m/2)];
      }
      else
      {
        res <<= 0;
      }
    }
  }
}

//==============================================================================

template<typename FieldT>
bool matrix_eigen_tests( FieldMatrix<FieldT>& A,
                         FieldMatrix<FieldT>& B,
                         FieldVector<FieldT>& x,
                         FieldVector<FieldT>& b,
                         FieldT& compare,
                         SpatFldPtr<FieldT>& scratch,
                         const bool noisyOn,
                         const size_t length,
                         const MemoryWindow w,
                         const short int deviceIndex )
{
    TestHelper noisy( noisyOn );
    {
      std::pair<std::pair<MatrixTypes_e, bool>, char const *> validMatrices[] = {
      std::make_pair(std::make_pair(MatrixTypes::RANDOM   , true), "Random"),
      std::make_pair(std::make_pair(MatrixTypes::DIAGONAL , false),"Diagonal"),
      std::make_pair(std::make_pair(MatrixTypes::ZERO     , false),"Zero"),
      std::make_pair(std::make_pair(MatrixTypes::NILPOTENT, false),"Nilpotent"),
    };
    const size_t typeSize = array_size(validMatrices);
    for( size_t typeI = 0; typeI < typeSize; typeI++ ){
      TestHelper noisyType( noisyOn );
      MatrixTypes_e matrixType    = validMatrices[typeI].first.first;
      bool          generateEigen = validMatrices[typeI].first.second;
      char const *  description   = validMatrices[typeI].second;

      srand(10);
      const size_t matricesToTest = 10;
      for( size_t testMatrixIndex = 0; testMatrixIndex < matricesToTest; testMatrixIndex++ ){
        TestHelper noisyDeep( noisyOn );

        //Generate Matrix A and Eigen Values in b
        if( generateEigen ){
          typename FieldT::value_type eigenVals[length];
          for( size_t eIndex = 0; eIndex < length; eIndex++){
            eigenVals[eIndex] = (static_cast<typename FieldT::value_type>(rand())/RAND_MAX)*MatrixRandomizer<FieldT>::get_magnitude();
#           ifndef NDEBUG
            for( size_t checkIndex = 0; checkIndex < eIndex; checkIndex++ ){
              assert(eigenVals[eIndex] != eigenVals[checkIndex]);
            }
#           endif /* NDEBUG */
          }

          generate_matrix_with_eigen(&eigenVals[0], length, A, B, b, scratch, deviceIndex);
          for( size_t eIndex = 0; eIndex < length; eIndex++ ){
            b.at(eIndex) <<= eigenVals[eIndex];
          }
        }
        else{
          random_matrix(A, matrixType, deviceIndex, scratch);
          for( size_t eIndex = 0; eIndex < length; eIndex++ ){
            b.at(eIndex) = A.at(eIndex*length+eIndex);
          }
        }

        x = A.real_eigenvalues();

        //sort result
        typename FieldT::value_type buffer[length];
        x.set_device_as_active(CPU_INDEX);
        for( size_t i = 0; i < w.glob_npts(); i++ ){
          for( size_t j = 0; j < length; j++ ){
            buffer[j] = x.at(j)[i];
          }
          std::sort(buffer, buffer+length);
          for( size_t j = 0; j < length; j++){
            x.at(j)[i] = buffer[j];
          }
        }
        x.set_device_as_active(deviceIndex);
        //sort eigen compare
        b.set_device_as_active(CPU_INDEX);
        for( size_t i = 0; i < w.glob_npts(); i++ ){
          for( size_t j = 0; j < length; j++ ){
            buffer[j] = b.at(j)[i];
          }
          std::sort(buffer, buffer+length);
          for( size_t j = 0; j < length; j++ ){
            b.at(j)[i] = buffer[j];
          }
        }
        b.set_device_as_active(deviceIndex);

        //verify result
        for( size_t i = 0; i < length; i++ ){
          std::ostringstream msg;
          msg << "correct sorted eigen val #" << i;
          noisyDeep(field_equal(x(i), b(i), 1e-4), msg.str());
        }

        std::ostringstream msg;
        msg << "eigen values of matrix #" << testMatrixIndex;
        noisyType( noisyDeep.ok(), msg.str() );
      }

      std::ostringstream msg;
      msg << "Eigen matrix of type " << description;
      noisy( noisyType.ok(), msg.str() );
    }
  }

  return noisy.ok();
}

template<typename FieldT>
bool matrix_complex_eigen_tests( FieldMatrix<FieldT>& A,
                                 FieldMatrix<FieldT>& B,
                                 FieldVector<FieldT>& x,
                                 FieldVector<FieldT>& b,
                                 FieldVector<FieldT>& c,
                                 FieldT& compare,
                                 SpatFldPtr<FieldT>& scratch,
                                 const bool noisyOn,
                                 const size_t length,
                                 const MemoryWindow w,
                                 const short int deviceIndex )
{
    TestHelper noisy( noisyOn );
    {
      std::pair<std::pair<MatrixTypes_e, bool>, char const *> validMatrices[] = {
        std::make_pair(std::make_pair(MatrixTypes::RANDOM   , true), "Random"),
      };
      const size_t typeSize = array_size(validMatrices);
      for( size_t typeI = 0; typeI < typeSize; typeI++ ){
        TestHelper noisyType( noisyOn );
        MatrixTypes_e matrixType    = validMatrices[typeI].first.first;
        bool          generateEigen = validMatrices[typeI].first.second;
        char const *  description   = validMatrices[typeI].second;

        srand(10);
        const size_t matricesToTest = 10;
        for( size_t testMatrixIndex = 0; testMatrixIndex < matricesToTest; testMatrixIndex++ ){
          TestHelper noisyDeep( noisyOn );

          //Generate Matrix A and Eigen Values in b
          if( generateEigen ){
            const size_t eigenSize = length/2;
            typename FieldT::value_type eigenVals[eigenSize];
            for( size_t eIndex = 0; eIndex < eigenSize; eIndex++){
              eigenVals[eIndex] = (static_cast<typename FieldT::value_type>(rand())/RAND_MAX)*MatrixRandomizer<FieldT>::get_magnitude();
#             ifndef NDEBUG
              for( size_t checkIndex = 0; checkIndex < eIndex; checkIndex++ ){
                assert(eigenVals[eIndex] != eigenVals[checkIndex]);
              }
#             endif /* NDEBUG */
            }

            generate_matrix_with_complex_eigen(&eigenVals[0], eigenSize, A, B, b, scratch, deviceIndex);
            for( size_t eIndex = 0; eIndex < eigenSize; eIndex++ ){
              b.at(eIndex) <<= eigenVals[eIndex];
            }
            for( size_t eIndex = 0; eIndex < eigenSize; eIndex++ ){
              b.at(eigenSize + eIndex) <<= -eigenVals[eIndex];
            }
          }
          else{
            //DEPRECIATED: only a diagonal matrix of complex values would have
            //complex eigen values on the diagonal, which we cannot do using
            //only a real matrix
            random_matrix(A, matrixType, deviceIndex, scratch);
            for( size_t eIndex = 0; eIndex < length; eIndex++ ){
              b.at(eIndex) = A.at(eIndex*length+eIndex);
            }
          }

          A.eigenvalues(x, c);

          //sort real result
          typename FieldT::value_type buffer[length];
          x.set_device_as_active(CPU_INDEX);
          for( size_t i = 0; i < w.glob_npts(); i++ ){
            for( size_t j = 0; j < length; j++ ){
              buffer[j] = x.at(j)[i];
            }
            std::sort(buffer, buffer+length);
            for( size_t j = 0; j < length; j++){
              x.at(j)[i] = buffer[j];
            }
          }
          x.set_device_as_active(deviceIndex);
          //sort complex result
          c.set_device_as_active(CPU_INDEX);
          for( size_t i = 0; i < w.glob_npts(); i++ ){
            for( size_t j = 0; j < length; j++ ){
              buffer[j] = c.at(j)[i];
            }
            std::sort(buffer, buffer+length);
            for( size_t j = 0; j < length; j++){
              c.at(j)[i] = buffer[j];
            }
          }
          c.set_device_as_active(deviceIndex);
          //sort eigen compare
          b.set_device_as_active(CPU_INDEX);
          for( size_t i = 0; i < w.glob_npts(); i++ ){
            for( size_t j = 0; j < length; j++ ){
              buffer[j] = b.at(j)[i];
            }
            std::sort(buffer, buffer+length);
            for( size_t j = 0; j < length; j++ ){
              b.at(j)[i] = buffer[j];
            }
          }
          b.set_device_as_active(deviceIndex);

          //verify result
          for( size_t i = 0; i < length; i++ ){
            std::ostringstream msg;
            msg << "correct sorted eigen val #" << i;
            noisyDeep(field_equal(c(i), b(i), 1e-4), msg.str());
          }
          for( size_t i = 0; i < length; i++ ){
            std::ostringstream msg;
            msg << "correct real eigen val #" << i;
            noisyDeep(field_equal(0, x(i), 1e-4), msg.str());
          }

          std::ostringstream msg;
          msg << "eigen values of matrix #" << testMatrixIndex;
          noisyType( noisyDeep.ok(), msg.str() );
        }

        std::ostringstream msg;
        msg << "Eigen matrix of type " << description;
        noisy( noisyType.ok(), msg.str() );
      }
  }

  return noisy.ok();
}

//==============================================================================

template<typename FieldT>
bool matrix_solve_test_helper( MatrixTypes_e type,
                               FieldMatrix<FieldT>& A,
                               FieldVector<FieldT>& x,
                               FieldVector<FieldT>& b,
                               FieldT& compare,
                               SpatFldPtr<FieldT>& scratch,
                               const bool noisyOn,
                               const size_t length,
                               const short int deviceIndex,
                               const bool synthesizeb )
{
  TestHelper noisyType( noisyOn );
  std::vector<double> xvals(length); // make up a solution and compare later
  int j=length;
  while( j-- ){
    xvals[j] = j + 1.0 ;
    x(j) <<= xvals[j];
  }
  srand(10);
  random_matrix(A, type, deviceIndex, scratch);
  if( synthesizeb ){
    b = A * x; // synthesize "b"
  }
  x = 0.0; // "forget" our solution
  j=length;
  while( j-- ){
    assert( field_equal(0.0, x(j), 1e-4) );
  }
  x = A.solve(b);
  j=length;
  while( j-- ){
    compare <<= xvals[j];
    noisyType(field_equal(x(j), compare, 1e-4), "correct x value");
  }

  return noisyType.ok();
}

//==============================================================================

template<typename FieldT>
bool matrix_solve_tests( FieldMatrix<FieldT>& A,
                         FieldVector<FieldT>& x,
                         FieldVector<FieldT>& b,
                         FieldT& compare,
                         SpatFldPtr<FieldT>& scratch,
                         const bool noisyOn,
                         const size_t length,
                         const short int deviceIndex )
{
  TestHelper noisy( noisyOn );
  {
    MatrixTypes_e validMatrices[] = {
      MatrixTypes::RANDOM,
      MatrixTypes::SYMMETRIC,
      MatrixTypes::POSITIVE_DEFINITE,
      MatrixTypes::DIAGONAL,
    };
    const size_t typeSize = array_size(validMatrices);
    for( size_t typeI = 0; typeI < typeSize; typeI++ ){
      const bool ret = matrix_solve_test_helper( validMatrices[typeI],
                                                 A,
                                                 x,
                                                 b,
                                                 compare,
                                                 scratch,
                                                 noisyOn,
                                                 length,
                                                 deviceIndex,
                                                 true );
      {
        std::ostringstream msg;
        msg << "Random matrix of type "
            << MatrixTypes::matrix_type_to_string(validMatrices[typeI]);
        noisy( ret, msg.str() );
      }
    }
  }
# ifndef NDEBUG
  //Technically the solver doesn't support these kinds of matrices but we test
  //them anyway. If these stop working it is okay to remove them.
  {
    MatrixTypes_e multipleSolutionMatrices[] = {
      MatrixTypes::ZERO,
      MatrixTypes::ZERO_LAST_ROW,
    };
    const size_t typeSize = array_size(multipleSolutionMatrices);
    for( size_t typeI = 0; typeI < typeSize; typeI++ ){
      bool pass = false;
      try{
        matrix_solve_test_helper( multipleSolutionMatrices[typeI],
                                  A,
                                  x,
                                  b,
                                  compare,
                                  scratch,
                                  noisyOn,
                                  length,
                                  deviceIndex,
                                  true );
      }
      catch(...){
        pass = true;
      }

      {
        std::ostringstream msg;
        msg << "Expected failure of random matrix of type "
            << MatrixTypes::matrix_type_to_string(multipleSolutionMatrices[typeI]);
        noisy( pass, msg.str() );
      }
    }
  }
  //Technically the solver doesn't support these kinds of matrices but we test
  //them anyway. If these stop working it is okay to remove them.
  {
    MatrixTypes_e noSolutionMatrices[] = {
      MatrixTypes::ZERO,
      MatrixTypes::ZERO_LAST_ROW,
    };
    const size_t typeSize = array_size(noSolutionMatrices);
    for( size_t typeI = 0; typeI < typeSize; typeI++ ){
      //Instantiate b such that a row of 0's cannot equal it
      int i = length;
      while( i-- ){
        b(i) <<= 1+(rand()%100);  // put pseudo-random numbers in b
      }
      bool pass = false;
      try{
        matrix_solve_test_helper( noSolutionMatrices[typeI],
                                  A,
                                  x,
                                  b,
                                  compare,
                                  scratch,
                                  noisyOn,
                                  length,
                                  deviceIndex,
                                  false );
      }
      catch(...){
        pass = true;
      }

      {
        std::ostringstream msg;
        msg << "Expected failure of random matrix of type "
            << MatrixTypes::matrix_type_to_string(noSolutionMatrices[typeI])
            << " with no possible solution";
        noisy( pass, msg.str() );
      }
    }
  }
# endif // NDEBUG

  return noisy.ok();
}

//==============================================================================

bool drive_test( const IntVec dim, const int length, const bool verbose )
{
  typedef SVolField FieldT;
  TestHelper quiet( true );
  const bool noisyOn = verbose;
  const GhostData ghost(1);
  // Boundary present if direction is active
  const IntVec hasBC(dim[0]>1,dim[1]>1,dim[2]>1);
  const BoundaryCellInfo bc = BoundaryCellInfo::build<FieldT>(hasBC,hasBC);
  const MemoryWindow w( get_window_with_ghost(dim,ghost,bc) );

  // Get number of device types to check
  const size_t deviceTypeCount = 1  //CPU count
#                              ifdef __CUDACC__
                               + 1  //GPU count
#                              endif /* __CUDACC__ */
    ;

  std::pair<int, const char *> deviceTypes[deviceTypeCount];
  {
    size_t deviceCount = 0;
    deviceTypes[deviceCount]   = std::make_pair(CPU_INDEX, "CPU");
#   ifdef __CUDACC__
    deviceTypes[++deviceCount] = std::make_pair(GPU_INDEX, "GPU");
#   endif /* __CUDACC__ */
  }

  short int deviceIndex;
  for( size_t i = 0; i < deviceTypeCount; i++ ){
    TestHelper quietDevice( true );
    deviceIndex = deviceTypes[i].first;
    SpatFldPtr<FieldT> scratch( new FieldT(w,bc,ghost, NULL, InternalStorage, CPU_INDEX) );
    FieldMatrix<FieldT> A(length,w,bc,ghost, deviceIndex ),
                        B(length,w,bc,ghost, deviceIndex ),
                        C(length,w,bc,ghost, deviceIndex );
    FieldVector<FieldT> b(length,w,bc,ghost, deviceIndex ),
                        x(length,w,bc,ghost, deviceIndex ),
                        y(length,w,bc,ghost, deviceIndex );
    FieldT offset ( w, bc, ghost, NULL, InternalStorage, deviceIndex  ),
           compare( w, bc, ghost, NULL, InternalStorage, deviceIndex  );
    int dx = 1;
    offset <<= sqrt(dx);

    scratch->add_device(deviceIndex);

    A.add_device(CPU_INDEX);
    B.add_device(CPU_INDEX);
    C.add_device(CPU_INDEX);
    b.add_device(CPU_INDEX);
    x.add_device(CPU_INDEX);
    y.add_device(CPU_INDEX);
    offset.add_device(CPU_INDEX);
    compare.add_device(CPU_INDEX);

    // test assigning to an element directly
    {
      b(0) <<= 5.0;
      compare <<= 5.0;
      quietDevice(field_equal(b(0), compare, 1e-4), "vector(0) <<= scalar assignment"); // vector
      A(0) <<= 1.0;
      compare <<= 1.0;
      quietDevice(field_equal(A(0), compare, 1e-4), "matrix(0) <<= scalar assignment"); // matrix
    }

    // test the vector = scalar operator
    const double c = 2.1;
    {
      TestHelper noisy(noisyOn);
      b = c; // this assigns double c to all fields in vector b
      int j = length;
      while( j-- ){
        compare <<= c;
        noisy( field_equal(b(j), compare, 1e-4), "vector = scalar assignment" );
      }
      quietDevice( noisy.ok(), "vector = scalar assignment" );
    }

    // iterate through vector and assign elements separately
    {
      TestHelper noisy(noisyOn);
      int j = length;
      while( j-- ){
        x(j) <<= c;
        compare <<= c;
        noisy( field_equal(x(j), compare, 1e-4), "iterative vector assignment" );
      }
      quietDevice( noisy.ok(), "iterative vector assignment" );
    }

    // iterate through matrix
    {
      TestHelper noisy(noisyOn);
      int ij = length * length;
      while( ij-- ){
        A(ij) <<= c + 1;
        compare <<= c + 1;
        noisy( field_equal(A(ij), compare, 1e-4), "iterative matrix assignment" );
      }
      quietDevice( noisy.ok(), "iterative matrix assignment" );
    }

    // test dot product of b `dot` x
    {
      dot_product_proto(b,x).eval(&A(0)); // store in A(0) for convenience
      int j=length;
      compare <<= 0;
      while( j-- ){
        compare <<= compare + b(j) * x(j);
      }
      quietDevice( field_equal(A(0), compare, 1e-4), "dot product" );
    }

    // test the ability to multiply a vector by a scalar
    {
      TestHelper noisy( noisyOn );
      srand(10);
      const double c = 2.1;
      int i = length;
      while( i-- ){
        x(i) <<= offset*(rand()%100);   // put pseudo-random numbers in x
      }
      b = c*x; // use b as storage for scalar mat multiply as convenience
      i = length;
      while( i-- ){
        compare <<= c*x(i);
        noisy(field_equal(b(i), compare, 1e-4), "correct b value");
      }
      quietDevice( noisy.ok(), "vector scalar multiply" );
    }

    // test the ability to multiply a matrix by a scalar
    {
      TestHelper noisy( noisyOn );
      srand(10);
      const double c = 2.1;
      int ij = length*length;
      while( ij-- ){
        A(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in A
      }
      B = c * A; // use B as storage for mat scalar multiply as convenience
      ij = length*length;
      while( ij-- ){
        compare <<= c*A(ij);
        noisy(field_equal(B(ij), compare, 1e-4), "correct B value");
      }
      quietDevice( noisy.ok(), "matrix scalar multiply" );
    }

    // test the ability to add a vector to a vector
    {
      TestHelper noisy( noisyOn );
      srand(10);
      int i = length;
      while( i-- ){
        x(i) <<= offset*(rand()%100);   // put pseudo-random numbers in x
      }
      i = length;
      while( i-- ){
        y(i) <<= offset*(rand()%100);   // put pseudo-random numbers in y
      }
      b = x + y; // use b as storage for vec vec addition as convenience
      i = length;
      while( i-- ){
        compare <<= x(i)+y(i);
        noisy(field_equal(b(i), compare, 1e-4), "correct b value");
      }
      quietDevice( noisy.ok(), "vector vector addition" );
    }

    // test the ability to add a matrix to a matrix
    {
      TestHelper noisy( noisyOn );
      srand(10);
      int ij = length*length;
      while( ij-- ){
        A(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in A
      }
      ij=length*length;
      while( ij-- ){
        B(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in B
      }
      C = A + B; // use C as storage for mat mat addition as convenience
      ij = length*length;
      while( ij-- ){
        compare <<= A(ij)+B(ij);
        noisy(field_equal(C(ij), compare, 1e-4), "correct C value");
      }
      quietDevice( noisy.ok(), "matrix matrix addition" );
    }

    // test the ability to subtract a vector to a vector
    {
      TestHelper noisy( noisyOn );
      srand(10);
      int i = length;
      while( i-- ){
        x(i) <<= offset*(rand()%100);   // put pseudo-random numbers in x
      }
      i = length;
      while( i-- ){
        y(i) <<= offset*(rand()%100);   // put pseudo-random numbers in y
      }
      b = x - y; // use b as storage for vec vec subtraction as convenience
      i = length;
      while( i-- ){
        compare <<= x(i)-y(i);
        noisy(field_equal(b(i), compare, 1e-4), "correct b value");
      }
      quietDevice( noisy.ok(), "vector vector subtraction" );
    }

    // test the ability to subtract a matrix to a matrix
    {
      TestHelper noisy( noisyOn );
      srand(10);
      int ij = length*length;
      while( ij-- ){
        A(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in A
      }
      ij=length*length;
      while( ij-- ){
        B(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in B
      }
      C = A - B; // use C as storage for mat mat subtraction as convenience
      ij = length*length;
      while( ij-- ){
        compare <<= A(ij)-B(ij);
        noisy(field_equal(C(ij), compare, 1e-4), "correct C value");
      }
      quietDevice( noisy.ok(), "matrix matrix subtraction" );
    }

    // test the ability to multiply a matrix by a vector
    {
      TestHelper noisy( noisyOn );
      srand(10);
      int ij = length*length;
      while( ij-- ){
        A(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in A
      }
      int j=length;
      while( j-- ){
        x(j) <<= offset*(rand()%100);   // put pseudo-random numbers in x
      }
      b = A * x; // use b as storage for mat vec multiply as convenience
      ij = length*length;
      while( ij ){
        j=length;
        compare <<= 0;
        while( j ){
          compare <<= compare+A(--ij)*x(--j);
        }
        noisy(field_equal(b(ij/length), compare, 1e-4), "correct b value");
      }
      quietDevice( noisy.ok(), "matrix vector multiply" );
    }

    //// test the ability to multiply a matrix by a matrix
    {
      TestHelper noisy( noisyOn );
      srand(10);
      int ij = length*length;
      while( ij-- ){
        A(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in A
      }
      ij = length*length;
      while( ij-- ){
        B(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in B
      }
      C = A * B; // use C as storage for mat mat multiply as convenience

      for(size_t columns = 0; columns < length; columns++)
      {
        size_t ij = 0;
        for(size_t CRow = 0; CRow < length; CRow++)
        {
          compare <<= 0;
          for(size_t BRow = 0; BRow < length; BRow++)
          {
            compare <<= compare+A(ij++)*B(columns + BRow*length);
          }
          noisy(field_equal(C(columns + CRow*length), compare, 1e-4), "correct C value");
        }
      }
      quietDevice( noisy.ok(), "matrix matrix multiply" );
    }

    // test the ability to offset a matrix's diagonal by c
    {
      TestHelper noisy( noisyOn );
      srand(10);
      const double c = 2.1;
      int ij = length*length;
      while( ij-- ){
        A(ij) <<= offset*(rand()%100);   // put pseudo-random numbers in A
        B(ij) <<= A(ij);                 // create a copy of A
      }
      B.add_to_diagonal(c); // use B as storage for diagonal offset as convenience
      ij = length*length;
      while( ij-- ){
        if(ij/length == ij%length)
          compare <<= c + A(ij);
        else
          compare <<= A(ij);
        noisy(field_equal(B(ij), compare, 1e-4), "correct B value");
      }
      quietDevice( noisy.ok(), "matrix scalar diagonal offset" );
    }

    // test the ability to solve x = inv( A ) * b
    {
      const bool result = matrix_solve_tests( A,
                                              x,
                                              b,
                                              compare,
                                              scratch,
                                              noisyOn,
                                              length,
                                              deviceIndex );
      quietDevice( result, "pointwise matrix solve" );
    }

    // test only real eigenvalues randomly
    {
      const bool result = matrix_eigen_tests( A,
                                              B,
                                              x,
                                              b,
                                              compare,
                                              scratch,
                                              noisyOn,
                                              length,
                                              w,
                                              deviceIndex );
      quietDevice( result, "only real eigen values of matrix"  );
    }

    // test only complex eigenvalues randomly
    if( length % 2 == 0 ) {
      const bool result = matrix_complex_eigen_tests( A,
                                                      B,
                                                      x,
                                                      b,
                                                      y,
                                                      compare,
                                                      scratch,
                                                      noisyOn,
                                                      length,
                                                      w,
                                                      deviceIndex );
      quietDevice( result, "only complex eigen values of matrix"  );
    }

    //print results of device run
    quiet(quietDevice.ok(), deviceTypes[i].second);
  }

  return quiet.ok();
}

//==============================================================================

double drive_perf( const IntVec listSize,
                   const size_t matrixDim,
                   const size_t iterationsToAverage,
                   const size_t burnInIterations,
                   const short int deviceIndex )
{
  typedef SVolField FieldT;
  TestHelper quiet( true );
  const GhostData ghost(1);
  // Boundary present if direction is active
  const IntVec hasBC(listSize[0]>1,listSize[1]>1,listSize[2]>1);
  const BoundaryCellInfo bc = BoundaryCellInfo::build<FieldT>(hasBC,hasBC);
  const MemoryWindow w( get_window_with_ghost(listSize,ghost,bc) );

  SpatFldPtr<FieldT> scratch( new FieldT(w,bc,ghost, NULL, InternalStorage, CPU_INDEX) );
  FieldMatrix<FieldT> A( matrixDim,w,bc,ghost, deviceIndex ),
                      B( matrixDim,w,bc,ghost, deviceIndex );
  FieldVector<FieldT> b( matrixDim,w,bc,ghost, deviceIndex ),
                      x( matrixDim,w,bc,ghost, deviceIndex );

  scratch->add_device(deviceIndex);

  double totalTime = 0.0;
  size_t currentIteration;
  double tmpIterationTime;
  std::cout << "Performance Test of matrices of dimension "
            << matrixDim << "x" << matrixDim
            << " and list size " << listSize
            << std::endl;

  // test dot product of b `dot` x
  currentIteration = iterationsToAverage + burnInIterations;
  tmpIterationTime = 0.0;
  while( currentIteration-- ){
    int j=matrixDim;
    while( j-- ){
      b(j) <<= (rand()%100);  // put pseudo-random numbers in b
    }
    j=matrixDim;
    while( j-- ){
      x(j) <<= (rand()%100);  // put pseudo-random numbers in x
    }

    Timer timer;
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    timer.reset();
    dot_product_proto(b,x).eval(&A(0)); // store in A(0) for convenience
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    const double time = timer.stop();
    tmpIterationTime += currentIteration < iterationsToAverage ? time : 0;
  }
  totalTime += tmpIterationTime/iterationsToAverage;
  std::cout << '\t' << "Dot Product average time: " << tmpIterationTime/iterationsToAverage << std::endl;

  // test the ability to multiply a matrix by a vector
  currentIteration = iterationsToAverage + burnInIterations;
  tmpIterationTime = 0.0;
  while( currentIteration-- ){
    srand(10);
    random_matrix( A, MatrixTypes::RANDOM, deviceIndex, scratch );
    int j=matrixDim;
    while( j-- ){
      x(j) <<= (rand()%100);  // put pseudo-random numbers in x
    }

    Timer timer;
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    timer.reset();
    b = A * x; // use b as storage for mat vec multiply as convenience
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    const double time = timer.stop();
    tmpIterationTime += currentIteration < iterationsToAverage ? time : 0;
  }
  totalTime += tmpIterationTime/iterationsToAverage;
  std::cout << '\t' << "Matrix Multiply By Vector average time: " << tmpIterationTime/iterationsToAverage << std::endl;

  // test the ability to solve x = inv( A ) * b
  srand(10);
  currentIteration = iterationsToAverage + burnInIterations;
  tmpIterationTime = 0.0;
  while(currentIteration--)
  {
    int j=matrixDim;
    while( j-- ){
      x(j) <<= (rand()%100);   // put pseudo-random numbers in x
    }
    random_matrix( A, MatrixTypes::RANDOM, deviceIndex, scratch );
    b = A * x; // synthesize "b"
    x = 0.0; // "forget" our solution

    Timer timer;
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    timer.reset();
    x = A.solve(b);
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    const double time = timer.stop();
    tmpIterationTime += currentIteration < iterationsToAverage ? time : 0;
  }
  totalTime += tmpIterationTime/iterationsToAverage;
  std::cout << '\t' << "Linear Solve average time: " << tmpIterationTime/iterationsToAverage << std::endl;

  // test eigenvalues randomly
  srand(10);
  currentIteration = iterationsToAverage + burnInIterations;
  tmpIterationTime = 0.0;
  while( currentIteration-- ){
    typename FieldT::value_type eigenVals[matrixDim];
    for( size_t eIndex = 0; eIndex < matrixDim; eIndex++ ){
      eigenVals[eIndex] = (static_cast<typename FieldT::value_type>(rand())/RAND_MAX)*MatrixRandomizer<FieldT>::get_magnitude();
#     ifndef NDEBUG
      for(size_t checkIndex = 0; checkIndex < eIndex; checkIndex++){
        assert(eigenVals[eIndex] != eigenVals[checkIndex]);
      }
#     endif /* NDEBUG */
    }

    generate_matrix_with_eigen( &eigenVals[0], matrixDim, A, B, b, scratch, deviceIndex );
    Timer timer;
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    timer.reset();
    x = A.real_eigenvalues();
#ifdef __CUDACC__
    cudaDeviceSynchronize();
#endif
    const double time = timer.stop();
    tmpIterationTime += currentIteration < iterationsToAverage ? time : 0;
  }
  totalTime += tmpIterationTime/iterationsToAverage;
  std::cout << '\t' << "Eigen Decomposition average time: " << tmpIterationTime/iterationsToAverage << std::endl;

  return totalTime;
}

//==============================================================================

int main(int argc, const char *argv[])
{
  po::options_description desc("Supported Options");
  desc.add_options()
    ( "help",    "print help message\n" )
    ( "timing",  "run timing tests instead of correctness tests" )
    ( "verbose", "enable verbose output" );

  po::variables_map args;
  po::store( po::parse_command_line(argc,argv,desc), args );
  po::notify(args);
  if( args.count("help") ){
    std::cout << desc << std::endl
      << "Examples:" << std::endl
      << " matrix_field --verbose" << std::endl
      << " matrix_field --timing" << std::endl
      << std::endl;
    return -1;
  }

  const bool toTime  = ( args.count("timing") > 0 );
  const bool verbose = ( args.count("verbose") > 0 );

  if( !toTime ){
    TestHelper status( true );

    const int lengths[] = { 1, 2, 3, 5, 10, 13 };

    for( int lengthIndex = 0; lengthIndex < array_size(lengths); lengthIndex++ ){
      std::ostringstream msg;
      const int length = lengths[lengthIndex];
      msg << length;
      const std::string l = msg.str();

      status( drive_test( IntVec(2,1,1),   length, verbose ), "Dimension: ( 2, 1, 1) tests. Length: " +l );
      status( drive_test( IntVec(1,2,1),   length, verbose ), "Dimension: ( 1, 2, 1) tests. Length: " +l );
      status( drive_test( IntVec(1,1,2),   length, verbose ), "Dimension: ( 1, 1, 2) tests. Length: " +l );
      status( drive_test( IntVec(2,2,2),   length, verbose ), "Dimension: ( 2, 2, 2) tests. Length: " +l );
      status( drive_test( IntVec(10,10,1), length, verbose ), "Dimension: (10,10, 1) tests. Length: " +l );
    }

    if( status.ok() ){
      return 0;
    }
    return -1;
  }
  else{
    const size_t dimsLow[] = { 8, 16, 32 };
    const size_t dimsLowSize = array_size(dimsLow);
    const size_t listSizeRootsHigh[] = { 16, 32, 64 };
    const size_t listSizeRootsHighSize = array_size(listSizeRootsHigh);

    const size_t dimsHigh[] = { 50, 100, 125 };
    const size_t dimsHighSize = array_size(dimsHigh);
    const size_t listSizeRootsLow[] = { 16 };
    const size_t listSizeRootsLowSize = array_size(listSizeRootsLow);

    const std::pair<std::pair<const size_t*, const size_t>, std::pair<const size_t*, const size_t> > testRunInfo[] = {
      std::make_pair(std::make_pair(dimsLow,  dimsLowSize),  std::make_pair(listSizeRootsHigh, listSizeRootsHighSize)),
      std::make_pair(std::make_pair(dimsHigh, dimsHighSize), std::make_pair(listSizeRootsLow,  listSizeRootsLowSize)),
    };

    const size_t iterationsToAverage = 50;

    const size_t burnIn = 2;

    //Get number of device types to check
    const size_t deviceTypeCount = 1  //CPU count
#                                ifdef __CUDACC__
                                 + 1  //GPU count
#                                endif /* __CUDACC__ */
    ;

    std::pair<int, const char *> deviceTypes[deviceTypeCount];
    {
      size_t deviceCount = 0;
      deviceTypes[deviceCount]   = std::make_pair(CPU_INDEX, "CPU");
#     ifdef __CUDACC__
      deviceTypes[++deviceCount] = std::make_pair(GPU_INDEX, "GPU");
#     endif /* __CUDACC__ */
    }

    for( size_t testIndex = 0; testIndex < array_size(testRunInfo); testIndex++ ){
      for( size_t dimIndex = 0; dimIndex < testRunInfo[testIndex].first.second; dimIndex++ ){
        for( size_t listIndex = 0; listIndex < testRunInfo[testIndex].second.second; listIndex++ ){
          for( size_t deviceIndex = 0; deviceIndex < deviceTypeCount; deviceIndex++ ){
            const size_t dim = testRunInfo[testIndex].first.first[dimIndex];
            const size_t listSizeRoot = testRunInfo[testIndex].second.first[listIndex];
            const IntVec listSize(listSizeRoot,listSizeRoot,listSizeRoot);

            //Explicitly ignore this combination due to memory constraints
            if( !(dim == 32 && listSizeRoot == 64) ){
              std::cout << deviceTypes[deviceIndex].second << ": ";

              std::cout << "List Dimension: " << listSize << " tests."
                        << " Dim: " << dim << "x" << dim << "."
                        << " " << deviceTypes[deviceIndex].second << "."
                        << " Total Time on Average: " << drive_perf( listSize,
                                                                      dim,
                                                                      iterationsToAverage,
                                                                      burnIn,
                                                                      deviceTypes[deviceIndex].first )
                        << std::endl;
            }
          }
        }
      }
    }
  }
}


#define BOOST_TEST_MODULE FastFourierTransform
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/fft.hpp>
#include <vexcl/random.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/reductor.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(transform_expression)
{
    const size_t N = 1024;
    std::vector<vex::backend::command_queue> queue(1, ctx.queue(0));

    vex::vector<cl_float> data(queue, N);
    vex::FFT<cl_float> fft(queue, N);

    // should compile
    data += fft(data * data) * 5;
}

BOOST_AUTO_TEST_CASE(check_correctness)
{
#if defined(VEXCL_BACKEND_OPENCL) || defined(VEXCL_BACKEND_COMPUTE)
    // Apple fails this test on CPUs
#   if defined(__APPLE__)
    if (vex::Filter::CPU(ctx.device(0)))
        return;
#   endif
#endif

    const size_t N = 1024;
    std::vector<vex::backend::command_queue> queue(1, ctx.queue(0));

    vex::vector<cl_double>  in  (queue, N);
    vex::vector<cl_double2> out (queue, N);
    vex::vector<cl_double>  back(queue, N);

    vex::Random<cl_double> rnd;

    in = rnd(vex::element_index(), std::rand());

    vex::FFT<cl_double,  cl_double2> fft (queue, N);
    vex::FFT<cl_double2, cl_double > ifft(queue, N, vex::fft::inverse);

    out  = fft (in );
    back = ifft(out);

    vex::Reductor<cl_double, vex::SUM> sum(queue);

    BOOST_CHECK(std::sqrt(sum(pow(in - back, 2.0)) / N) < 1e-3);
}

void test(const vex::Context &ctx, std::vector<size_t> ns, size_t batch) {
    std::cout << "FFT(C2C) size=" << ns[0];
    for(size_t i = 1; i < ns.size(); i++) std::cout << 'x' << ns[i];
    std::cout << " batch=" << batch << std::endl;

    std::vector<vex::backend::command_queue> queue(1, ctx.queue(0));

    size_t n1 = std::accumulate(ns.begin(), ns.end(), static_cast<size_t>(1), std::multiplies<size_t>());
    size_t n = n1 * batch;

    // random data.
    std::vector<cl_double2> inp_h = random_vector<cl_double2>(n);

    // test
    vex::vector<cl_double2> inp (queue, inp_h);
    vex::vector<cl_double2> out (queue, n);
    vex::vector<cl_double2> back(queue, n);

    std::vector<size_t> ns_(ns.begin(), ns.end());
    std::vector<vex::fft::direction> dirs (ns.size(), vex::fft::forward);
    std::vector<vex::fft::direction> idirs(ns.size(), vex::fft::inverse);
    if(batch != 1) {
        ns_.insert(ns_.begin(), batch);
        dirs.insert(dirs.begin(), vex::fft::none);
        idirs.insert(idirs.begin(), vex::fft::none);
    }
    vex::FFT<cl_double2> fft (queue, ns_, dirs);
    vex::FFT<cl_double2> ifft(queue, ns_, idirs);

    out  = fft (inp);
    back = ifft(out);

    auto rms = [&](const vex::vector<cl_double2> &a, const vex::vector<cl_double2> &b) -> double {
        // Required for CUDA compatibility:
        VEX_FUNCTION(cl_double2, minus, (cl_double2, a)(cl_double2, b),
                double2 r = {a.x - b.x, a.y - b.y};
                return r;
                );

        VEX_FUNCTION(double, dot2, (cl_double2, a)(cl_double2, b),
                return a.x * b.x + a.y * b.y;
                );

        static vex::Reductor<double, vex::SUM> sum(queue);
        return std::sqrt(sum(dot2(minus(a, b), minus(a, b)))) /
            std::sqrt(sum(dot2(b, b)));
    };

    BOOST_CHECK_SMALL(rms(back, inp), 1e-8);
}

// random dimension, mostly 1.
size_t random_dim(double p, double s) {
    static std::default_random_engine rng( std::rand() );
    static std::uniform_real_distribution<double> rnd(0.0, 1.0);

    return 1 + static_cast<size_t>( s * std::pow(rnd(rng), p) );
}

BOOST_AUTO_TEST_CASE(test_dimensions)
{
#if defined(VEXCL_BACKEND_OPENCL) || defined(VEXCL_BACKEND_COMPUTE)
    // TODO: POCL fails this test.
    if (vex::Filter::Platform("Portable Computing Language")(ctx.device(0)))
        return;

    // Apple fails this test on CPUs
#if defined(__APPLE__)
    if (vex::Filter::CPU(ctx.device(0)))
        return;
#endif
#endif

    const size_t max = 1 << 12;

    vex::fft::planner p;

    for(size_t i = 0; i < 100; ++i) {
        // random number of dimensions, mostly 1.
        size_t dims = random_dim(3, 5);
        size_t batch = random_dim(5, 100);

        // random size.
        std::vector<size_t> n;
        size_t d_max = static_cast<size_t>(std::pow(static_cast<double>(max), 1.0 / dims));
        size_t total = batch;
        for(size_t d = 0 ; d < dims ; d++) {
            size_t sz = random_dim(dims == 1 ? 3 : 1, static_cast<double>(d_max));

            if(rand() % 3 != 0)
                sz = p.best_size(sz);

            n.push_back(sz);
            total *= sz;
        }

        // run
        if(total <= max) test(ctx, n, batch);
    }
}

BOOST_AUTO_TEST_SUITE_END()


// SLAM-Basic-Maze.cpp : Defines the entry point for the console application.
//
//export CPLUS_INCLUDE_PATH="$CPLUS_INCLUDE_PATH:/usr/include/python2.7/"
//g++ initial.cpp -o testgl -lGL -lglut -lglfw -lGLEW -lGLU -std=c++11 -lboost_python -lpython2.7 -lboost_system -lboost_thread -lpthread
//export DISPLAY=localhost:0.0
//
#ifdef __APPLE_CC__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif
#include <algorithm>
#include <omp.h>
#include <string>
#include <iomanip>
#include <iostream>
#include <vector>
#include <atomic>
#include <mutex>
#include <math.h>
#include <sstream>
#include <chrono>
#include <boost/python.hpp>

using namespace std;
using namespace boost::python;

int flag_tennis = 0;
int flag_obs    = 0;
bool auto_path  = false;

#define earthRadiusKm 6371.0
std::vector<double> _lat_incoming; //Incoming lat GPS coordinates
std::vector<double> _lon_incoming; //Incoming lon GPS coordinates
size_t incoming_index = 0;

std::vector<double> _distance_vector;

std::vector<double> _distance_x;
std::vector<double> _distance_z;

vector<double> _lat_outgoing;//Outgoing GPS coordinates
vector<double> _lon_outgoing;//Outgoing GPS coordinates
size_t outgoing_index = 0;

//CSUF test coordinates
double lat_points[4] = {33.88239, 33.882513, 33.882434, 33.88245};
double lon_points[4] = {-117.883568, -117.88607751, -117.8835028, -117.883660};

double _angle_; // Global Calculations
double _distance_; // Global Calculations

/* @param temp_lat
 * 		the latitudinal points coming in from deepstream
 */
void set_lat(double temp_lat)
{
	_lat_incoming.push_back(temp_lat);
}

/* @param temp_lon
 * 		the longitudinal points coming from deepstream
 */
void set_lon(double temp_lon)
{
	_lon_incoming.push_back(temp_lon);
}

// This function converts decimal degrees to radians
double deg2rad(double deg)
{
  return (deg * M_PI / 180);
}

//  This function converts radians to decimal degrees
double rad2deg(double rad)
{
  return (rad * 180 / M_PI);
}

/**
 * Returns the distance between two points on the Earth.
 * Direct translation from http://en.wikipedia.org/wiki/Haversine_formula
 * @param lat1d Latitude of the first point in degrees
 * @param lon1d Longitude of the first point in degrees
 * @param lat2d Latitude of the second point in degrees
 * @param lon2d Longitude of the second point in degrees
 * @return The distance between the two points in kilometers
 */
double distanceEarth(double lat1d, double lon1d, double lat2d, double lon2d)
{
	double lat1r, lon1r, lat2r, lon2r, u, v;
	lat1r = deg2rad(lat1d);
	lon1r = deg2rad(lon1d);
	lat2r = deg2rad(lat2d);
	lon2r = deg2rad(lon2d);
	u = sin((lat2r - lat1r)/2);
	v = sin((lon2r - lon1r)/2);
	return 2.0 * earthRadiusKm * asin(sqrt(u * u + cos(lat1r) * cos(lat2r) * v * v));
}

//As latitude and longitude are usually expressed in degrees, do not forget to convert them to radians before using this function
double angleFromCoordinate(double lat1, double long1, double lat2,
        double long2)
{

    double dLon = (long2 - long1);

    double y = sin(dLon) * cos(lat2);
    double x = cos(lat1) * sin(lat2) - sin(lat1)
             * cos(lat2) * cos(dLon);

    double brng = atan2(y, x);
    brng = rad2deg(brng);
    brng = fmod((brng + 360), 360);
    brng = 360 - brng; // count degrees counter-clockwise - remove to make clockwise

    return brng;
}

struct _openGL_CAMERA_VALUES_
{
	// angle of rotation for the camera direction
	GLfloat angle = 0.0f;
	// actual vector representing the camera's direction
	GLfloat lx = 0.0f, lz = -1.0f, ly = 0.0f;
	// XZ position of the camera
	GLfloat x = 0.0f, z = 5.0f, y = 1.75f;
}_openGLCV_;

struct _openGL_KEY_STATES_
{
	// the key states. These variables will be zero
	//when no key is being presses
	GLfloat deltaAngle = 0.0f;
	GLfloat deltaMove = 0;
	GLint xOrigin = -1;
}_openGLKS_;

struct _openGL_MAINWINDOW_VALUES_
{
	// width and height of the window
	GLint height;
	GLint width;
	// variables to hold window identifiers
	GLint mainWindow;
	GLint TopDownWindow;
	GLint TopPanelWindow;
	//border between subwindows
	GLint border = 6;
}_openGLMV_;

struct _openGL_ROVER_TRACKING_
{
	std::vector<double> past_x;
	std::vector<double> past_z;

	std::vector<double> past_track_x;
	std::vector<double> past_track_z;

}_openGLRT_;

struct _openGL_ROVER_LINE_TRACKING_
{
	std::vector<double> line_x;
	std::vector<double> line_z;
}_openGLRLT_;

std::vector<GLfloat> past_x;
std::vector<GLfloat> past_y;
std::vector<GLfloat> past_z;

void setProjection(GLint w1, GLint h1)
{
	GLfloat ratio;
	// Prevent a divide by zero, when window is too short
	// (you cant make a window of zero width).
	ratio = (1.0f * w1 / h1);
	// Reset the coordinate system before modifying
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	// Set the viewport to be the entire window
	glViewport(0, 0, w1, h1);

	// Set the clipping volume
	gluPerspective(45, ratio, 0.1, 1000);
	glMatrixMode(GL_MODELVIEW);

}

void changeSize(GLint w1, GLint h1)
{
	if (h1 == 0) { h1 = 1; }

	_openGLMV_.width  = w1;
	_openGLMV_.height = h1;

	// set topdownWindow as the active window
	glutSetWindow(_openGLMV_.TopDownWindow);
	// resize and reposition the sub window
	glutPositionWindow(_openGLMV_.border, (_openGLMV_.height + _openGLMV_.border) / 40);
	glutReshapeWindow(_openGLMV_.width - 20, _openGLMV_.height - 20);
	setProjection(_openGLMV_.width / 2 - _openGLMV_.border * 3 / 2, _openGLMV_.height / 2 - _openGLMV_.border * 3 / 2);
}

#pragma endregion Static_Render

// --------------------------------------------------------------------------------
//       STATIC RENDERING OBJECTS
// --------------------------------------------------------------------------------
//Static Drawcube object for rendering.
#pragma region Static_Objects

//Parallelize
void drawRadCircle()
{
	GLfloat pi_short  = 3.14159f;
	GLfloat rad_short = 1000.0;

	glBegin(GL_POINTS);

		for (int i = 0; i < (int)rad_short; ++i)
		{
			glVertex3f(cos(pi_short*i / rad_short), 0.0, sin(pi_short*i / rad_short));
		}

	glEnd();
}

void drawCircle()
{
	glColor3f(0.0, 1.0, 0.0); // <R,G,B>

	//Circle
	glTranslatef(0.0f, 0.75f, 0.0f);
	glutSolidSphere(0.10f, 10, 10);
}

void drawTennisBall()
{
	//Circle
	glTranslatef(1.6f, 0.0f, 2.6f);
	glutSolidSphere(0.10f, 5, 5);
}

//Static Drawcube object for rendering.
void drawCubeObject()
{
	//Generic cube size
	GLdouble cube_size;

	cube_size = 0.20;
	// Red side - TOP
	glBegin(GL_POLYGON);
		glVertex3f(cube_size, cube_size, cube_size);
		glVertex3f(cube_size, cube_size, -cube_size);
		glVertex3f(-cube_size, cube_size, -cube_size);
		glVertex3f(-cube_size, cube_size, cube_size);
	glEnd();
}

void drawPoint()
{
	//Handle the next gps cordination.
	//Generic cube size
	GLdouble cube_size;

	cube_size = 0.20;
	// Red side - TOP
	glColor3f(0.0, 0.0, 1.0); // <R,G,B>
	glBegin(GL_POLYGON);
		glVertex3f(cube_size, cube_size, cube_size);
		glVertex3f(cube_size, cube_size, -cube_size);
		glVertex3f(-cube_size, cube_size, -cube_size);
		glVertex3f(-cube_size, cube_size, cube_size);
	glEnd();
}

#pragma endregion Static_Objects

//END OF STATIC OBJECTS
//----------------------------------------------------------------------------------

//Preliminary description for the error
// x range assumptions for the data

//---------------------------------------------------------------------------------
void renderBitmapString(GLfloat x, GLfloat y, GLfloat z, void *font, char *string)
{
	char *c;
	glRasterPos2i(x, y);
	for (c = string; *c != '\0'; c++)
	{
		glutBitmapCharacter(font, *c);
	}
}

void restorePerspectiveProjection()
{
	glMatrixMode(GL_PROJECTION);
	// restore previous projection matrix
	glPopMatrix();

	// get back to modelview mode
	glMatrixMode(GL_MODELVIEW);
}

void setOrthographicProjection()
{
	// switch to projection mode
	glMatrixMode(GL_PROJECTION);

	// save previous matrix which contains the
	//settings for the perspective projection
	glPushMatrix();

	// reset matrix
	glLoadIdentity();

	// set a 2D orthographic projection
	gluOrtho2D(0, _openGLMV_.width, _openGLMV_.height, 0);

	// switch back to modelview mode
	glMatrixMode(GL_MODELVIEW);
}

// Common Render Items for all subwindows
void renderSubWindowScene()
{
	GLdouble plain_size;
	GLdouble ground_level;
	GLdouble object_pos;

	plain_size   = 200.0f;
	ground_level = 0.0f;
	object_pos   = 10.0f;

	// Draw ground
	GLuint tex;
	glGenTextures(1, &tex);

	glColor3f(0.184, 0.310, 0.310); //<R,G,B>
	glBegin(GL_QUADS);
		glVertex3f(-plain_size, ground_level, -plain_size);
		glVertex3f(-plain_size, ground_level, plain_size);
		glVertex3f(plain_size, ground_level, plain_size);
		glVertex3f(plain_size, ground_level, -plain_size);
	glEnd();

	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glEnable(GL_BLEND);
	glBlendFunc(GL_ONE, GL_ONE);
	glColor3f(0.545, 0.000, 0.545); //<R,G,B>
	glutSolidSphere(10.0f, 20, 10);

	glColor3f(0.294, 0.000, 0.510);
	glRotatef(100 - (_openGLCV_.angle + _openGLKS_.deltaAngle)*180.0 / 3.14, 0.0, 1.0, 0.0);
	glutSolidSphere(10.0f, 20, 5);

	glEnd();
}

#pragma region Position_Computation

void computePos(GLfloat deltaMove)
{
	_openGLCV_.x += deltaMove * _openGLCV_.lx * 0.1f;
	_openGLCV_.z += deltaMove * _openGLCV_.lz * 0.1f;

	_openGLRT_.past_x.push_back(_openGLCV_.x);
	_openGLRT_.past_z.push_back(_openGLCV_.z);

	std::cout << "x pos = " << _openGLCV_.x << std::endl;
	std::cout << "z pos = " << _openGLCV_.z << std::endl;
}

void computeDir(GLfloat deltaAngle)
{
	_openGLCV_.angle += deltaAngle;
	//Angle calculation is required to move forward.

	_openGLCV_.lx = sin(_openGLCV_.angle);
	_openGLCV_.lz = -cos(_openGLCV_.angle);

	std::cout << "angle = " << _openGLCV_.angle << std::endl;
	std::cout << "lx = " << _openGLCV_.lx << std::endl;
	std::cout << "lz = " << _openGLCV_.lz << std::endl;
}
#pragma endregion Position_Computation

#pragma region Scene_Render
// Display function for main window.
void renderScene()
{
	glutSetWindow(_openGLMV_.mainWindow);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glutSwapBuffers();
}

// Display function for top-down view
void renderTopDownScene()
{
	glutSetWindow(_openGLMV_.TopDownWindow);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();

	std::atomic<int> index;
	index.store(0,std::memory_order_relaxed);

	//Main Camera Position, starting position for top down view.
	gluLookAt(_openGLCV_.x, _openGLCV_.y + 15, _openGLCV_.z, _openGLCV_.x,
		_openGLCV_.y - 1, _openGLCV_.z, _openGLCV_.lx, 0, _openGLCV_.lz);

			// Main object, main camera.
			glPushMatrix();
				glColor3f(0.0, 0.0, 1.0); //<R,G,B>
				glTranslatef(_openGLCV_.x, _openGLCV_.y, _openGLCV_.z);
				glRotatef(180 - (_openGLCV_.angle + _openGLKS_.deltaAngle)*180.0 / 3.14, 0.0, 1.0, 0.0);
				glutSolidCone(0.2, 0.5f, 2, 2);
				drawRadCircle();
			glPopMatrix();

			// TEST
			for(int i = 0; i <_distance_vector.size();i++)
			{
				glPushMatrix();
					glColor3f(0.0, 0.0, 1.0); //<R,G,B>
					glTranslatef(_distance_x[i] + _distance_vector[i], _openGLCV_.y, _distance_z[i] + _distance_vector[i]);
					glRotatef(180 - (_openGLCV_.angle + _openGLKS_.deltaAngle)*180.0 / 3.14, 0.0, 1.0, 0.0);
					drawCubeObject();
				glPopMatrix();
			}

	//Only register points in 15 intervals
	for (int i = 0; i < _openGLRT_.past_x.size(); i++)
	{
			if (i % 15 == 0)
			{
				_openGLRLT_.line_x.push_back(_openGLRT_.past_x[i]);
				_openGLRLT_.line_z.push_back(_openGLRT_.past_z[i]);

				_openGLRT_.past_track_x.push_back(_openGLRT_.past_x[i]);
				_openGLRT_.past_track_z.push_back(_openGLRT_.past_z[i]);

			}

		glPushMatrix();
			glColor3f(1.0, 1.0, 1.0); //<R,G,B>
			glPolygonMode(GL_FRONT_AND_BACK, GL_POINT);
			glTranslatef(_openGLRT_.past_x[i], 0.0f, _openGLRT_.past_z[i]);
			glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		glPopMatrix();
	}

	//Draws the tracer red-line from the first point of view.
	//Needs a better rework of the y direction calculation between matrices.
	// Draw the interpolated points second.
	glColor3f(0.0f, 0.0f, 0.0f); // Draw points in black
	glPointSize(5);
	glBegin(GL_POINTS);
		for (int i = 1; i < _openGLRLT_.line_x.size(); i++)
		{
			glVertex3f(_openGLRLT_.line_x[i], 0.0f, _openGLRLT_.line_z[i]);
		}
	glEnd();

	glPointSize(1);
	glLineWidth(1.0);

	glColor3f(0.0, 0.0, 0.8); //<R,G,B>
	glBegin(GL_LINE_STRIP);
		for (int i = 1; i < _openGLRLT_.line_x.size(); i++)
		{
			glVertex3f(_openGLRLT_.line_x[i], 0.0f, _openGLRLT_.line_z[i]);
		}
	glEnd();
	glFlush();

	glColor3f(1.0, 0.0, 0.8); //<R,G,B>
	glBegin(GL_TRIANGLE_FAN);
		for (int i = 1; i < _openGLRLT_.line_x.size(); i++)
		{
			glVertex3f(_openGLRLT_.line_x[i], 0.0f, _openGLRLT_.line_z[i]);
			_openGLRLT_.line_x.pop_back();
			_openGLRLT_.line_z.pop_back();
		}
	glEnd();

	glFlush();

	//Render Area
	renderSubWindowScene();
	glutSwapBuffers();
}

//Initial Render Event cycle.

int render_index = 0;
int gps_index    = 0;
// Global render func
void renderSceneAll()
{
	// check for keyboard movement
	// Up down movement reference check.
	if (_openGLKS_.deltaMove)
	{
		computePos(_openGLKS_.deltaMove);
		glutSetWindow(_openGLMV_.mainWindow);
		glutPostRedisplay();
	}

	// Left right movement reference check.
	if (_openGLKS_.deltaAngle)
	{
		computeDir(_openGLKS_.deltaAngle);
		glutSetWindow(_openGLMV_.mainWindow);
		glutPostRedisplay();
	}
	renderScene();

	//-------------------------------Control Segment--------------------------------
	//PRE-RENDERING REQUIREMENTS ARE HANDLED HERE..
	//All the handles are here for determining the next gps point.
	//If error or need modification this is the section to do it in.
	//------------------------------------------------------------------------------
	//Create Lidar Connection and grab information from lidar.
	render_index++;
	namespace python = boost::python;
	if(render_index > 10)
	{
		try
		{
			object main_module ((handle<>(borrowed(PyImport_AddModule("__main__")))));
			object main_namespace = main_module.attr("__dict__");

			//Import Segments
			object ignored = exec("import socket", main_namespace);
			ignored = exec("import sys", main_namespace);
			ignored = exec("import pickle", main_namespace);
			ignored = exec("from time import sleep", main_namespace);

			//Variable Declerations
			ignored = exec("host = \"localhost\"", main_namespace);
			ignored = exec("port = 7080", main_namespace);
			ignored = exec("BUFFER_SIZE = 8192", main_namespace);

			ignored = exec("s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)", main_namespace);
			ignored = exec("print \"Socket successfully created\"",main_namespace);

			ignored = exec("s.connect((host, port))", main_namespace);
			ignored = exec("print 'Received', host", main_namespace);
			ignored = exec("degree = 90", main_namespace);
			ignored = exec("distance = []", main_namespace);

			ignored = exec("s.send(\"ready\")", main_namespace);
			ignored = exec("d = s.recv(8192)", main_namespace);
			ignored = exec("print d", main_namespace);

			ignored = exec("data = d.split(\" \")", main_namespace);
			ignored = exec("dataPoints = int(\"0x\" + str(data[25]), 16)", main_namespace);
			ignored = exec("print(dataPoints)", main_namespace);
			ignored = exec("s.close()", main_namespace);

			//File Creation to import the lidar data to grab.
			ignored = exec("lidar_file = open(\"lidar_data.txt\",\"w\")", main_namespace);
			ignored = exec("lidar_file.write(dataPoints)", main_namespace);
			ignored = exec("lidar_file.close()", main_namespace);
		}
		catch(error_already_set)
		{
			PyErr_Print();
		}

		render_index = 0; //Reset lidar render index.
	}

	//Value correlation...
	bool is_collision = false;

	//Create GPS connection to get current GPS location...
	gps_index++;
	if(gps_index > 5)
	{
		try
		{
			object main_module ((handle<>(borrowed(PyImport_AddModule("__main__")))));
			object main_namespace = main_module.attr("__dict__");
			object ignored = exec("import socket", main_namespace);
			ignored = exec("import sys", main_namespace);
			ignored = exec("s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)", main_namespace);
			ignored = exec("print \"Socket successfully created\"",main_namespace);

			//GPS connection to the server to grab GPS information.
			ignored = exec("port = 2111",main_namespace); //Port for the GPS server.
			ignored = exec("host_ip = socket.gethostbyname('192.168.1.25')", main_namespace); //The ip for the GPS server.
			ignored = exec("s.connect((host_ip, port))", main_namespace);//Connection to the GPS server.
			ignored = exec("s.send(\"ready\")", main_namespace);

			ignored = exec("gps_information = s.recv(8192)", main_namespace);
			ignored = exec("print gps_information", main_namespace);

			ignored = exec("gps_file = open(\"gps_data.txt\",\"w\")", main_namespace);
			ignored = exec("gps_file.write(gps_information)", main_namespace);
			ignored = exec("gps_file.close()", main_namespace);

		}
		catch(error_already_set)
		{
			PyErr_Print();
		}

		gps_index = 0; //Reset lidar render index.
	}
	//Display Current GPS

	//Calculate Distance
	bool calculate_dist = false;
	if(calculate_dist)
	{
		try
		{
			object main_module ((handle<>(borrowed(PyImport_AddModule("__main__")))));
			object main_namespace = main_module.attr("__dict__");

			//Import Segments
			object ignored = exec("from deepstream import get", main_namespace);
			ignored = exec("import math", main_namespace);

			//Variable Declerations
			ignored = exec("currentHeading = get(\"imu\")[\"heading\"]", main_namespace);
			ignored = exec("print(\"No Heading Error\")", main_namespace);

			ignored = exec("heading = math.radians(heading)", main_namespace);
			ignored = exec("radius = 6371", main_namespace); // km
			ignored = exec("dist = distance / 100000.0",main_namespace);
			ignored = exec("lat1, lon1 = origin", main_namespace);
			ignored = exec("lat1 = math.radians(lat1)", main_namespace);
			ignored = exec("lon1 = math.radians(lon1)", main_namespace);
			ignored = exec("lat2 = math.asin( math.sin(lat1)*math.cos(dist/radius) + math.cos(lat1)*math.sin(dist/radius)*math.cos(heading))", main_namespace);
			ignored = exec("lon2 = lon1 + math.atan2(math.sin(heading)*math.sin(dist/radius)*math.cos(lat1), math.cos(dist/radius)-math.sin(lat1)*math.sin(lat2))", main_namespace);
			ignored = exec("lat2 = round(math.degrees(lat2), 9)", main_namespace);
			ignored = exec("lon2 = round(math.degrees(lon2), 9)", main_namespace);

			//File Creation to import the lat and lon data to grab.
			ignored = exec("distance_file = open(\"distance_data.txt\",\"w\")", main_namespace);
			ignored = exec("distance_file.write(lat2)", main_namespace);
			ignored = exec("distance_file.write(lon2)", main_namespace);
			ignored = exec("distance_file.close()", main_namespace);
		}
		catch(error_already_set)
		{
			PyErr_Print();
		}
	}

	//Gather the new gps point information
	string line;
	vector<string> gps_line;

	ifstream myfile ("distance_data.txt");
	if (myfile.is_open())
	{
		while ( getline (myfile,line) )
		{
			cout << line << '\n';
			gps_line.push_back(line);
		}
		myfile.close();
	}
	else
	{
		cout << "Unable to open distance_data.txt file";
	}

	//Parsing might be required for the GPS data.
	//Unsure of how the new GPS data would look.

	//Setting the lat and lon points.
	set_lat(std::stod(gps_line[0]);
	set_lon(std::stod(gps_line[1]);

	gps_line.clear();//Clear the gps_line buffer.

	if(_lat_incoming.size() > 1)
	{
		cout << setprecision(10) << distanceEarth(_lat_incoming[i],_lat_incoming[i-1],_lon_incoming[i],_lon_incoming[i-1]) * 0.0001 << endl;
		_distance_vector.push_back(distanceEarth(_lat_incoming[i],_lat_incoming[i-1],_lon_incoming[i],_lon_incoming[i-1]) * 0.0001);
	}

	//Current GPS to Push back at...
	_distance_x.push_back(_openGLCV_.x);
	_distance_z.push_back(_openGLCV_.z);

	//Calculate Pathing
	//10x8ft for rover safe clearance...
	//NOT AVAILABLE ATM... No D* ;<

	//Send Back to the server - Only if there is an obstacle in the path.
	bool send_back = false;
	if(send_back)
	{
		try
		{
			object main_module ((handle<>(borrowed(PyImport_AddModule("__main__")))));
			object main_namespace = main_module.attr("__dict__");
			object ignored = exec("import socket", main_namespace);
			ignored = exec("import sys", main_namespace);
			ignored = exec("s_back = socket.socket(socket.AF_INET, socket.SOCK_STREAM)", main_namespace);
			ignored = exec("print \"Socket successfully created\"",main_namespace);

			ignored = exec("port = 2111",main_namespace);
			ignored = exec("host_ip = socket.gethostbyname('192.168.1.25')", main_namespace);
			ignored = exec("s_back.connect((host_ip, port))", main_namespace);
			ignored = exec("print 'Received', host_ip", main_namespace);

		}
		catch(error_already_set)
		{
			PyErr_Print();
		}
	}

	//------------------------------------------------------------------------
	//CONTROL SEGMENT END-----------------------------------------------------

	//Sub-Render Scenes with multi-view camera angles.
	renderTopDownScene();
}
#pragma endregion Scene_Render

// --------------------------------------------------------------------------------
//             KEYBOARD SECTION
// --------------------------------------------------------------------------------
#pragma region Keyboard_Section

void processNormalKeys(unsigned char key, GLint xx, GLint yy) {

	if (key == 27)
	{
		glutDestroyWindow(_openGLMV_.mainWindow);
		exit(0);
	}
	else if(key == 'a')
	{
		if(auto_path == false)
		{
			auto_path = true;
		}
		else if(auto_path)
		{
			auto_path = false;
		}

	}
}

void pressKey(GLint key, GLint xx, GLint yy)
{

	switch (key)
	{
		case GLUT_KEY_LEFT:  _openGLKS_.deltaAngle  = -0.01f; break;
		case GLUT_KEY_RIGHT: _openGLKS_.deltaAngle  = 0.01f; break;
		case GLUT_KEY_UP:    _openGLKS_.deltaMove   = 0.2f; break;
		case GLUT_KEY_DOWN:  _openGLKS_.deltaMove   = -0.2f; break;
	}

	glutSetWindow(_openGLMV_.mainWindow);
	glutPostRedisplay();
}

void releaseKey(int key, int x, int y)
{
	switch (key)
	{
		case GLUT_KEY_LEFT:
							//std::cout << "Left Key Pressed" << std::endl;
							break;
		case GLUT_KEY_RIGHT:
							//std::cout << "Right Key Pressed" << std::endl;
							_openGLKS_.deltaAngle = 0.0f;
							break;
		case GLUT_KEY_UP:
							//std::cout << "Up Key Pressed" << std::endl;
							break;
		case GLUT_KEY_DOWN:
							//std::cout << "Down Key Pressed" << std::endl;
							_openGLKS_.deltaMove = 0;
							break;
	}
}

#pragma endregion Keyboard_Section

// --------------------------------------------------------------------------------
//             MOUSE INPUT SECTION
// --------------------------------------------------------------------------------
#pragma region Mouse_Section

#pragma endregion Mouse_Section

// --------------------------------------------------------------------------------
//             MAIN and INIT
// --------------------------------------------------------------------------------
int main(int argc, char **argv)
{
	// Init of GLUT and main window creation.
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowPosition(100, 100);
	glutInitWindowSize(400, 400);
	_openGLMV_.mainWindow = glutCreateWindow("SLAM MAZE");

	// Callback processing for rendering all scenes.
	// These scenes will include sub-set window views.
	glutDisplayFunc(renderSceneAll);
	glutReshapeFunc(changeSize);

	glClearColor(0.0, 0.0, 0.0, 0.0);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);

	// register callbacks
	glutKeyboardFunc(processNormalKeys);
	glutSpecialFunc(pressKey);
	glutIgnoreKeyRepeat(1);
	glutSpecialUpFunc(releaseKey);

	//-----------------------------------------------------------------------------
	//Top down view window decleration.
	std::cout << "Registering callbacks for Top Down started..." << std::endl;
	_openGLMV_.TopDownWindow = glutCreateSubWindow(_openGLMV_.mainWindow, 10, 10, 400, 400);
	glutDisplayFunc(renderTopDownScene);

	glClearColor(0.0, 0.0, 0.0, 0.0);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);

	// register callbacks
	glutKeyboardFunc(processNormalKeys);
	glutSpecialFunc(pressKey);
	glutIgnoreKeyRepeat(1);
	glutSpecialUpFunc(releaseKey);

	try
	{
		cout << "Python Module Starting..." << endl;
		cout << "-------------------------" << endl;
		Py_Initialize();
		cout << "Python Module Initialized..." << endl;
	}
	catch(error_already_set)
	{
		PyErr_Print();
	}

	std::cout << "All callbacks have been initialized..." << std::endl;
	std::cout << "--------------------------------------" << std::endl;
	std::cout << "Main Loop Starting..." << std::endl;
	// Glut loop event cycle.
	glutMainLoop();

	//Anything beyond this point is disregarded.
	//-----------------------------------------------------------------------------

	return 1;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief test_scaled_conversion.cpp

\details
Test unit scaling

Output:
@verbatim
@endverbatim
**/

#define BOOST_TEST_MAIN

#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>

#include <sstream>

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

namespace bu = boost::units;
namespace si = boost::units::si;

BOOST_AUTO_TEST_CASE(test_scaled_to_plain) {
    BOOST_CONSTEXPR_OR_CONST bu::quantity<si::time> s1 = 12.5 * si::seconds;
    BOOST_CONSTEXPR_OR_CONST bu::quantity<si::time> s2(si::nano * s1);
    BOOST_CHECK_CLOSE_FRACTION(1e-9 * s1.value(), s2.value(), 0.000000001);
}

BOOST_AUTO_TEST_CASE(test_plain_to_scaled) {
    BOOST_CONSTEXPR_OR_CONST bu::quantity<si::time> s1 = 12.5 * si::seconds;
    typedef bu::multiply_typeof_helper<si::nano_type, si::time>::type time_unit;
    BOOST_CONSTEXPR_OR_CONST bu::quantity<time_unit> s2(s1);
    BOOST_CHECK_CLOSE_FRACTION(1e9 * s1.value(), s2.value(), 0.000000001);
}

BOOST_AUTO_TEST_CASE(test_scaled_to_scaled) {
    typedef bu::multiply_typeof_helper<si::mega_type, si::time>::type mega_time_unit;
    typedef bu::multiply_typeof_helper<si::micro_type, si::time>::type micro_time_unit;
    BOOST_CONSTEXPR_OR_CONST bu::quantity<mega_time_unit> s1(12.5 * si::seconds);
    BOOST_CONSTEXPR_OR_CONST bu::quantity<micro_time_unit> s2(s1);
    BOOST_CHECK_CLOSE_FRACTION(1e12 * s1.value(), s2.value(), 0.000000001);
}

BOOST_AUTO_TEST_CASE(test_conversion_factor) {
    BOOST_CHECK_CLOSE_FRACTION(conversion_factor(si::nano*si::seconds, si::seconds), 1e-9, 0.000000001);
    BOOST_CHECK_CLOSE_FRACTION(conversion_factor(si::seconds, si::nano*si::seconds), 1e9, 0.000000001);
    BOOST_CHECK_CLOSE_FRACTION(conversion_factor(si::mega*si::seconds, si::micro*si::seconds), 1e12, 0.000000001);
}

BOOST_AUTO_TEST_CASE(test_output) {
    std::stringstream stream;
    stream << si::nano * 12.5 * si::seconds;
    BOOST_CHECK_EQUAL(stream.str(), "12.5 ns");
}

BOOST_AUTO_TEST_CASE(test_output_name) {
    std::stringstream stream;
    stream << bu::name_format << si::nano * 12.5 * si::seconds;
    BOOST_CHECK_EQUAL(stream.str(), "12.5 nanosecond");
}


/**
 * @file multicopterDynamicsSim.hpp
 * @author Ezra Tal
 * @brief Multicopter dynamics simulator class header file
 *
 */

#ifndef MULTICOPTERDYNAMICSSIM_H
#define MULTICOPTERDYNAMICSSIM_H

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include "inertialMeasurementSim.hpp"

#include <iomanip>
#include <fstream>

/**
 * @brief Multicopter dynamics simulator class
 *
 */
class MulticopterDynamicsSim{
    public:
        MulticopterDynamicsSim(int numCopter, double thrustCoefficient, double torqueCoefficient,
                               double minMotorSpeed, double maxMotorSpeed,
                               double motorTimeConstant, double motorRotationalInertia,
                               double vehicleMass,
                               const Eigen::Matrix3d & vehicleInertia,
                               const Eigen::Matrix3d & aeroMomentCoefficient,
                               double dragCoefficient,
                               double momentProcessNoiseAutoCorrelation,
                               double forceProcessNoiseAutoCorrelation,
                               const Eigen::Vector3d & gravity);
        MulticopterDynamicsSim(int numCopter);

        int getNumCopter() { return numCopter_; }
        void setVehicleProperties(double vehicleMass, const Eigen::Matrix3d & vehicleInertia,
                                  const Eigen::Matrix3d & aeroMomentCoefficient,
                                  double dragCoefficient,
                                  double momentProcessNoiseAutoCorrelation,
                                  double forceProcessNoiseAutoCorrelation);
        void setGravityVector(const Eigen::Vector3d & gravity);
        void setMotorFrame(const Eigen::Isometry3d & motorFrame, int motorDirection, int motorIndex);
        void setMotorProperties(double thrustCoefficient, double torqueCoefficient, double motorTimeConstant,
                                double minMotorSpeed, double maxMotorSpeed, double rotationalInertia, int motorIndex);
        void setMotorProperties(double thrustCoefficient, double torqueCoefficient, double motorTimeConstant,
                                double minMotorSpeed, double maxMotorSpeed, double rotationalInertia);
        void setMotorSpeed(double motorSpeed, int motorIndex);
        void setMotorSpeed(double motorSpeed);
        void resetMotorSpeeds(void);
        void setVehiclePosition(const Eigen::Vector3d & position,const Eigen::Quaterniond & attitude);
        void setVehicleState(const Eigen::Vector3d & position,
                             const Eigen::Vector3d & velocity,
                             const Eigen::Vector3d & angularVelocity,
                             const Eigen::Quaterniond & attitude,
                             const std::vector<double> & motorSpeed);
        void getVehicleState(Eigen::Vector3d & position,
                             Eigen::Vector3d & velocity,
                             Eigen::Vector3d & angularVelocity,
                             Eigen::Quaterniond & attitude,
                             std::vector<double> & motorSpeed);
        Eigen::Vector3d getVehiclePosition(void);
        Eigen::Quaterniond getVehicleAttitude(void);
        Eigen::Vector3d getVehicleVelocity(void);
        Eigen::Vector3d getVehicleAngularVelocity(void);
        std::vector<double> getMotorSpeed();

        Eigen::Vector3d getThrust(const std::vector<double> & motorSpeed);
        Eigen::Vector3d getControlMoment(const std::vector<double> & motorSpeed,
                                         const std::vector<double> & motorAcceleration);
        Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity);
        Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity);
        Eigen::Vector3d getVehicleSpecificForce(void);

        void getMotorSpeedDerivative(std::vector<double> & motorSpeedDer,
                                     const std::vector<double> & motorSpeed,
                                     const std::vector<double> & motorSpeedCommand);
        Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce,
                                              const Eigen::Vector3d & velocity, const std::vector<double> & motorSpeed);
        Eigen::Vector3d getAngularVelocityDerivative(const std::vector<double> & motorSpeed,
                                                     const std::vector<double>& motorAcceleration,
                                                     const Eigen::Vector3d & angularVelocity,
                                                     const Eigen::Vector3d & stochMoment);
        Eigen::Vector4d getAttitudeDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & angularVelocity);

        void proceedState_ExplicitEuler(double dt_secs, const std::vector<double> & motorSpeedCommand);
        void proceedState_RK4(double dt_secs, const std::vector<double> & motorSpeedCommand);

        void getIMUMeasurement(Eigen::Vector3d & accOutput, Eigen::Vector3d & gyroOutput);

        /// @name IMU simulator
        inertialMeasurementSim imu_ = inertialMeasurementSim(0.,0.,0.,0.);

    private:
        /// @name  Number of rotors
        int numCopter_;

        /// @name Flag indicating whether to add stochastic forces
        //TODO add ROS parameter querying to set this.
        bool enableStochasticity_;

        /// @name Motor properties
        //@{

        // Transform from motor to vehicle (c.o.g.) frame
        /* Motor frame must have prop spinning around z-axis such that
         a positive motor speed corresponds to a positive thrust in
         positive motor z-axis direction.*/
        std::vector<Eigen::Isometry3d> motorFrame_;

        /* +1 if positive motor speed corresponds to positive moment around the motor frame z-axis
           -1 if positive motor speed corresponds to negative moment around the motor frame z-axis
           i.e. -1 indicates a positive motor speed corresponds to a positive rotation rate around the motor z-axis

           Values:
           motor0 -1 (front left)
           motor1 1 (back left)
           motor2 -1 (back right)
           motor3 1 (front right)
        */
        std::vector<int> motorDirection_;

        std::vector<double> thrustCoefficient_; // N/(rad/s)^2
        std::vector<double> torqueCoefficient_; // Nm/(rad/s)^2
        std::vector<double> motorTimeConstant_; // s
        std::vector<double> motorRotationalInertia_; // kg m^2
        std::vector<double> maxMotorSpeed_; // rad/s
        std::vector<double> minMotorSpeed_; // rad/s
        //@}

        /// @name Vehicle properties
        //@{
        double dragCoefficient_; // N/(m/s)
        Eigen::Matrix3d aeroMomentCoefficient_; // Nm/(rad/s)^2
        double vehicleMass_; // kg
        Eigen::Matrix3d vehicleInertia_; // kg m^2
        double momentProcessNoiseAutoCorrelation_ = 0.; // (Nm)^2s
        double forceProcessNoiseAutoCorrelation_ = 0.; // N^2s
        //@}

        /// @name Std normal RNG
        //@{
        std::default_random_engine randomNumberGenerator_;
        std::normal_distribution<double> standardNormalDistribution_ = std::normal_distribution<double>(0.0,1.0);
        //@}

        // Default reference frame is NED, but can be set by changing gravity direction
        /// @name Gravity vector
        Eigen::Vector3d gravity_; // m/s^2

        /// @name Vehicle state variables
        //@{
        std::vector<double> motorSpeed_; // rad/s
        Eigen::Vector3d velocity_ = Eigen::Vector3d::Zero(); // m/s
        Eigen::Vector3d position_ = Eigen::Vector3d::Zero(); // m
        Eigen::Vector3d angularVelocity_ = Eigen::Vector3d::Zero(); // rad/s
        Eigen::Quaterniond attitude_ = Eigen::Quaterniond::Identity();
        //@}

        /* Vehicle stochastic force vector (in world frame) is maintained
        for accelerometer output, since it must include the same
        random linear acceleration noise as used for dynamics integration*/
        /// @name Vehicle stochastic force vector
        Eigen::Vector3d stochForce_ = Eigen::Vector3d::Zero(); // N

        void vectorAffineOp(const std::vector<double> & vec1, const std::vector<double> & vec2,
                            std::vector<double> & vec3, double val);
        void vectorScalarProd(const std::vector<double> & vec1, std::vector<double> & vec2, double val);
        void vectorBoundOp(const std::vector<double> & vec1, std::vector<double> & vec2,
                           const std::vector<double> &  minvec, const std::vector<double> & maxvec);

        /// State index
        double index_ = 0;
        /// Time stamp after integration step
        double time_ = 0;

        void saveDynamics(const Eigen::Vector3d& position,
                          const Eigen::Quaterniond& attitude,
                          const Eigen::Vector3d& velocity,
                          const Eigen::Vector3d& angularVelocity,
                          const std::vector<double>& motorSpeeds,
                          const Eigen::Vector3d& force,
                          const Eigen::Vector3d& torque) const;
};

#endif // MULTICOPTERDYNAMICSSIM_H


#include "utils.h"

#include <iostream>
#include <string>
#include <armadillo>
#include <cmath>

#include <limits>

namespace Utils{

	bool readData(std::string fileName, unsigned int &nOutcomes, unsigned int &nPredictors, unsigned int &nObservations, arma::mat &Y, arma::mat& X)
	{

		bool status = X.load(fileName,arma::raw_ascii);

		if(!status){
			std::cout<< "Somethign went wrong while reading "<<fileName<<std::endl;
			return false;
		}else{

			if(X.n_cols < (nOutcomes+nPredictors) ){
				std::cout<< "Less columns than the sum of the specified outcomes and predictors. Specify the correct numbers. "<<std::endl;
				return false;
			}

			Y = X.cols(0,nOutcomes-1);
			X.shed_cols(0,nOutcomes-1);

			nObservations = Y.n_rows;

			if( nPredictors < X.n_cols){
				X.shed_cols(nPredictors,X.n_cols-1);
				std::cout<< "More predictors read then what specified in p=" << nPredictors << " -- I kept the firsts and discarded the others." << std::endl;
				nPredictors = X.n_cols;
			}

			if( nOutcomes < Y.n_cols){
				Y.shed_cols(nOutcomes,Y.n_cols-1);
				std::cout<< "More outcomes read then what specified in q=" << nOutcomes << " -- I kept the firsts and discarded the others." << std::endl;
				nOutcomes = Y.n_cols;
			}
		}

		return true;
	}

	bool readDataSEM(std::string fileName, arma::mat &data, arma::ivec &blockIndexes, arma::ivec &variableType,
		arma::uvec &missingDataIndexes, arma::uvec &nOutcomes, unsigned int &nPredictors, unsigned int &nObservations)
	{

		bool status = data.load(fileName,arma::raw_ascii);
		arma::uvec jnk;
		arma::ivec ijnk;

		if(!status){
			std::cout<< "Somethign went wrong while reading "<<fileName<<std::endl;
			return false;
		}else{

			// the first row is a row of blockIndexes
			blockIndexes = arma::conv_to<arma::ivec>::from( data.row(0)/*.t()*/ );
			data.shed_row(0);

			// checks on the blockIndexes
			// index 0 stands for the Xs, predictors
			// index 1+ are the upper-level outcomes
			// so we always need at least some zeros and some ones
			ijnk = arma::unique(blockIndexes);
			if( arma::max( blockIndexes ) < 1 || jnk.n_elem < 2 ) // more indepth check would be length of positive indexes..
			{
				std::cout<< "You need to define at least two blocks -- Xs (block 0) and Ys (block 1)"<<std::endl;
				return false;
			}

			// the second row is a row of variableType
			variableType = arma::conv_to<arma::ivec>::from( data.row(0)/*.t()*/ );
			data.shed_row(0);

			// miscellanea variables
			for( int i=0, n=arma::max(blockIndexes) ; i<n ; ++i )
			{
				jnk = arma::find( blockIndexes == i );   // meh, hate this temporary but is needed
				nOutcomes(i) = jnk.n_elem;
			}
			// first index is the number of predictors
			nPredictors = nOutcomes(0);
			nOutcomes.shed_row(0);

			nObservations = data.n_rows;

			// Now deal with NANs
			if( data.has_nan() )
			{
				missingDataIndexes = arma::find_nonfinite(data);
				data(missingDataIndexes).fill( arma::datum::nan );  // This makes all the ind values into valid armadillo NANs (should be ok even without, but..)
			}
			else
			{
				missingDataIndexes.set_size(0);
			}

		}

		return true;
	}

	// sgn is defined in the header in order for it to be visible

	double logspace_add(const arma::vec& logv)
	{

		if( logv.is_empty() )
			return std::numeric_limits<double>::lowest();
		double m;
		if( logv.has_inf() || logv.has_nan() ) // || logv.is_empty()
		{
			return logspace_add(logv.elem(arma::find_finite(logv)));
		}else{
			m = arma::max(logv);
			return m + std::log( (double)arma::as_scalar( arma::sum( arma::exp( - (m - logv) ) ) ) );
		}
	}

	double logspace_add(double a, double b)
	{

		if(a <= std::numeric_limits<float>::lowest())
      		return b;
    	if(b <= std::numeric_limits<float>::lowest())
      		return a;
    	return std::max(a, b) + std::log( (double)(1. + std::exp( (double)-std::abs((double)(a - b)) )));
	}

	arma::uvec nonZeroLocations_col( arma::sp_umat X)
	{
		std::vector<arma::uword> locations;

		for( arma::sp_umat::const_iterator it = X.begin(); it != X.end(); ++it)
		{
			locations.push_back(it.row());
		}

		return arma::uvec(locations);
	}

	arma::uvec nonZeroLocations_row( arma::sp_umat X)
	{
		std::vector<arma::uword> locations;

		for( arma::sp_umat::const_iterator it = X.begin(); it != X.end(); ++it)
		{
			locations.push_back(it.col());
		}

		return arma::uvec(locations);
	}

}


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

#include "libplump/pyp_sample.h"

#include <cmath>
#include <cassert>
#define BOOST_DISABLE_ASSERTS
#include <boost/multi_array.hpp>

#include "libplump/random.h"
#include "libplump/utils.h"

namespace gatsby { namespace libplump {

////////////////////////////////////////////////////////////////////////////////
//        PYP SAMPLING FUNCTIONS FOR GENERATING SEATING ARRANGEMENTS          //
////////////////////////////////////////////////////////////////////////////////

/**
 * Sample a seating arrangement given the table creation times z.
 */
std::vector<int> sample_crp_given_z(double d, std::vector<int>& z) {
  int t = z[z.size()-1] + 1;
  std::vector<int> arrangement(t,0);
  std::vector<double> probs(t,0);
  arrangement[0] = 1;
  for (int i = 1; i < (int)z.size(); ++i) {
    probs.assign(t,0);
    if (z[i] == z[i-1] + 1) {
      probs[z[i]] = 1;
    } else {
      for(int j=0;j<z[i]+1;++j) {
        probs[j] = (arrangement[j] - d)/(i - (z[i]+1)*d);
      }
    }
    ++arrangement[sample_unnormalized_pdf(probs,z[i])];
  }

  assert(sum(arrangement) == z.size() && (int)arrangement.size() == t);
  for(int i = 0; i < (int)arrangement.size(); ++i) {
    assert(arrangement[i] != 0);
  }
  return arrangement;
}

/**
 * Sample table creation times z_i using forward filtering, backward sampling.
 *
 * Runtime: O(c x t)
 */
std::vector<int> sample_crp_z_fb(double d, int c, int t) {
  tracer << "sample_crp_ct_fb(" << d << ", " << c << ", " << t << ")" << std::endl;
  assert(c!=0);
  assert(c>=t);

  if (t==1) {
    std::vector<int> z(c,0);
    return z;
  }

  typedef boost::multi_array<double, 2> array_type;
  //typedef array_type::index index;
  typedef int index;
  array_type grid(boost::extents[t][c]);
  for(index i = 0; i < t; ++i)
    for(index j = 0; j <  c; ++j)
      grid[i][j] = 0;

  // forward filtering
  grid[0][0] = 1;
  grid[t-1][c-1] = 1;
  for (index i = 1; i < c - 1; ++i) {
    for (index j = 0; j < t; ++j) { // TODO: 0 <= j <= i; grid is upper triangular?
      if (j>0) {
        grid[j][i] += grid[j-1][i-1];
      }
      grid[j][i] += grid[j][i-1] * (i - (j+1)*d);
    }
  }

  // backward sampling
  std::vector<int> z(c,0);
  int cur = t-1;
  z[c-1] = t - 1;
  d_vec probs(t,0);
  for(index i = c - 2; i >= 0; --i) {
    probs[cur] = (i+1-(cur+1)*d) * grid[cur][i];
    probs[cur-1] = grid[cur-1][i];
    cur = sample_unnormalized_pdf(probs);
    z[i] = cur;
    if (cur == 0) {
      break;
    }
    probs.assign(t,0);
  }

  return z;
}

/**
 * Sample table creation times z_i using backward filtering, forward sampling.
 *
 * Returns the number of customers at each table in the resulting sample.
 *
 * Runtime: O(c x t)
 */
std::vector<int> sample_crp_z_bf(double d, int c, int t) {
  tracer << "sample_crp_ct_bf(" << d << ", " << c << ", " << t << ")" << std::endl;
  assert(c!=0);
  assert(c>=t);

  if (t==1) {
    std::vector<int> z(c,0);
    return z;
  }

  typedef boost::multi_array<double, 2> array_type;
  //typedef array_type::index index;
  typedef int index;
  array_type grid(boost::extents[t][c]);
  for(index i = 0; i < t; ++i)
    for(index j = 0; j <  c; ++j)
      grid[i][j] = 0;

  // backward filtering
  grid[0][0]  = 1;
  grid[t-1][c-1] = 1;
  for (index i = c-2; i > 0; --i) {
    for (index j = std::min(t - 1, i); j  >= std::max(0,t - 1 - c + i) ; --j) {
      if (j < t - 1) {
        grid[j][i] += grid[j+1][i+1];
      }
      grid[j][i] += grid[j][i+1] * (i + 1 - (j+1)*d);
    }
  }

  // for (index i = 0; i < t; i++) {
  //   std::cout << iterableToString(grid[i]) << std::endl;
  // }

  // forward sampling
  std::vector<int> z(c,0);
  int cur = 0;
  z[c-1] = t - 1;
  d_vec probs(t,0);
  for(index i = 1; i < c - 1; ++i) {
    probs[cur] = (i-(cur+1)*d) * grid[cur][i];
    probs[cur+1] = grid[cur+1][i];
    //std::cout << iterableToString(probs) << std::endl;
    cur = sample_unnormalized_pdf(probs);
    z[i] = cur;
    if (cur == t - 1) {
      // all remaining customers must join existing tables
      for (index ii = i + 1; ii < c - 1; ++ii) {
        z[ii] = cur;
      }
      break;
    }
    probs.assign(t,0);
  }
  return z;
}

std::vector<int> sample_crp_c(double d, double a, int c) {
  d_vec probs(c,0);
  std::vector<int> arrangement;
  arrangement.push_back(1); // first customer at first table
  for(int i = 1; i < c; ++i) {
    for(int j = 0; j < (int)arrangement.size(); ++j) {
      probs[j] = arrangement[j] - d;
    }
    probs[arrangement.size()] = a + arrangement.size()*d;
    int sample = sample_unnormalized_pdf(probs,arrangement.size());
    if (sample == (int)arrangement.size()) {
      arrangement.push_back(1); // new table
    } else {
      ++arrangement[sample];
    }
  }
  return arrangement;
}

}} // namespace gatsby::libplump