/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
// This file is a part of ChASE.
// Copyright (c) 2015-2021, Simulation and Data Laboratory Quantum Materials,
//   Forschungszentrum Juelich GmbH, Germany. All rights reserved.
// License is 3-clause BSD:
// https://github.com/ChASE-library/ChASE

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <limits>
#include <random>
#include <memory>
#include <random>
#include <vector>
#include <iostream>
#include <fstream>
#include <chrono>

#include "algorithm/performance.hpp"
#include "ChASE-MPI/chase_mpi.hpp"

#include "ChASE-MPI/impl/chase_mpidla_blaslapack_seq.hpp"
#include "ChASE-MPI/impl/chase_mpidla_blaslapack_seq_inplace.hpp"

#ifdef USE_MPI
#include "ChASE-MPI/impl/chase_mpidla_blaslapack.hpp"
  #ifdef DRIVER_BUILD_MGPU
  #include "ChASE-MPI/impl/chase_mpidla_mgpu.hpp"
  #endif
#endif

using namespace chase;
using namespace chase::mpi;

namespace po = boost::program_options;
namespace bf = boost::filesystem;

template <typename T>
void readMatrix(T* H, std::string path_in, std::string spin, std::size_t kpoint,
                std::size_t index, std::string suffix, std::size_t size,
                bool legacy) {
  std::ostringstream problem(std::ostringstream::ate);
  if (legacy)
    problem << path_in << "gmat  1 " << std::setw(2) << index << suffix;
  else
    problem << path_in << "mat_" << spin << "_" << std::setfill('0')
            << std::setw(2) << kpoint << "_" << std::setfill('0')
            << std::setw(2) << index << suffix;

  std::cout << problem.str() << std::endl;
  std::ifstream input(problem.str().c_str(), std::ios::binary);

  std::cout << problem.str().c_str() << " " << "---" << bf::file_size(problem.str().c_str()) << '\n';

  if (input.is_open()) {
    input.read((char*)H, sizeof(T) * size);
  } else {
    throw std::string("error reading file: ") + problem.str();
  }
}

template <typename T>
void readMatrix(T* H, std::string path_in, std::string spin, std::size_t kpoint,
                std::size_t index, std::string suffix, std::size_t size,
                bool legacy, std::size_t xoff, std::size_t yoff,
                std::size_t xlen, std::size_t ylen) {
  std::size_t N = std::sqrt(size);
  std::ostringstream problem(std::ostringstream::ate);

  int rank;

#ifdef USE_MPI
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
#else
  rank = 0;
#endif

  if (legacy){
	problem << path_in << "gmat  1 " << std::setw(2) << index << suffix;
  }
  else{
    problem << path_in << "mat_" << spin << "_" << std::setfill('0')
            << std::setw(2) << kpoint << "_" << std::setfill('0')
            << std::setw(2) << index << suffix;
  }

  if (rank == 0) std::cout << problem.str() << std::endl;

  std::size_t file_size = bf::file_size(problem.str().c_str());

  try{
    if(size * sizeof(T) != file_size ){
      throw std::logic_error(std::string("The given file : ") +
                               problem.str() + std::string(" of size ") + std::to_string(file_size) +
                               std::string(" doesn't equals to the required size of matrix of size ") + std::to_string(size * sizeof(T)));
    }
  }
  catch(std::exception &e)
  {
    std::cerr << "Caught " << typeid( e ).name( ) << " : "<< e.what( ) << std::endl;
    return ;
  }

  std::ifstream input(problem.str().c_str(), std::ios::binary);
  if (!input.is_open()) {
    throw new std::logic_error(std::string("error reading file: ") +
                               problem.str());
  }

  for (std::size_t y = 0; y < ylen; y++) {
    input.seekg(((xoff) + N * (yoff + y)) * sizeof(T));
    input.read(reinterpret_cast<char*>(H + xlen * y), xlen * sizeof(T));
  }
}

template <typename T>
void readMatrix(T* H, std::string path_in, std::string spin, std::size_t kpoint,
                std::size_t index, std::string suffix, std::size_t size, bool legacy,
		std::size_t m, std::size_t mblocks, std::size_t nblocks,
            	std::size_t* r_offs, std::size_t* r_lens, std::size_t* r_offs_l,
		std::size_t* c_offs, std::size_t* c_lens, std::size_t* c_offs_l){
  std::size_t N = std::sqrt(size);
  std::ostringstream problem(std::ostringstream::ate);

  int rank;

#ifdef USE_MPI
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
#else
  rank = 0;
#endif

  if (legacy){
        problem << path_in << "gmat  1 " << std::setw(2) << index << suffix;
  }
  else{
    problem << path_in << "mat_" << spin << "_" << std::setfill('0')
            << std::setw(2) << kpoint << "_" << std::setfill('0')
            << std::setw(2) << index << suffix;
  }

  if (rank == 0) std::cout << problem.str() << std::endl;

  std::size_t file_size = bf::file_size(problem.str().c_str());

  try{
    if(size * sizeof(T) != file_size ){
      throw std::logic_error(std::string("The given file : ") +
                               problem.str() + std::string(" of size ") + std::to_string(file_size) +
                               std::string(" doesn't equals to the required size of matrix of size ") + std::to_string(size * sizeof(T)));
    }
  }
  catch(std::exception &e)
  {
    std::cerr << "Caught " << typeid( e ).name( ) << " : "<< e.what( ) << std::endl;
    return ;
  }

  std::ifstream input(problem.str().c_str(), std::ios::binary);
  if (!input.is_open()) {
    throw new std::logic_error(std::string("error reading file: ") +
                               problem.str());
  }

  for(std::size_t j = 0; j < nblocks; j++){
      for(std::size_t i = 0; i < mblocks; i++){
          for(std::size_t q = 0; q < c_lens[j]; q++){
	      input.seekg(((q + c_offs[j]) * N + r_offs[i])* sizeof(T));
	      input.read(reinterpret_cast<char*>(H + (q + c_offs_l[j]) * m + r_offs_l[i]), r_lens[i] * sizeof(T));
	  }
      }
  }

}

template <typename T>
void readMatrix(T* H, std::string path_in, std::size_t size,
                std::size_t xoff, std::size_t yoff,
                std::size_t xlen, std::size_t ylen) {
  std::size_t N = std::sqrt(size);
  std::ostringstream problem(std::ostringstream::ate);
  problem << path_in;
  int rank;
#ifdef USE_MPI
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
#else
  rank = 0;
#endif

  if (rank == 0) std::cout << problem.str() << std::endl;

  std::size_t file_size = bf::file_size(problem.str().c_str());

  try{
    if(size * sizeof(T) != file_size ){
      throw std::logic_error(std::string("The given file : ") +
                               problem.str() + std::string(" of size ") + std::to_string(file_size) +
                               std::string(" doesn't equals to the required size of matrix of size ") + std::to_string(size * sizeof(T)));
    }
  }
  catch(std::exception &e)
  {
    std::cerr << "Caught " << typeid( e ).name( ) << " : "<< e.what( ) << std::endl;
    return ;
  }

  std::ifstream input(problem.str().c_str(), std::ios::binary);

  if (!input.is_open()) {
    throw new std::logic_error(std::string("error reading file: ") +
                               problem.str());
  }

  for (std::size_t y = 0; y < ylen; y++) {
    input.seekg(((xoff) + N * (yoff + y)) * sizeof(T));
    input.read(reinterpret_cast<char*>(H + xlen * y), xlen * sizeof(T));
  }
}

template <typename T>
void readMatrix(T* H, std::string path_in, std::size_t size,
                std::size_t m, std::size_t mblocks, std::size_t nblocks,
                std::size_t* r_offs, std::size_t* r_lens, std::size_t* r_offs_l,
                std::size_t* c_offs, std::size_t* c_lens, std::size_t* c_offs_l){

      	std::size_t N = std::sqrt(size);
  std::ostringstream problem(std::ostringstream::ate);
  problem << path_in;

  int rank;

#ifdef USE_MPI
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
#else
  rank = 0;
#endif

  if (rank == 0) std::cout << problem.str() << std::endl;

  std::size_t file_size = bf::file_size(problem.str().c_str());

  try{
    if(size * sizeof(T) != file_size ){
      throw std::logic_error(std::string("The given file : ") +
                               problem.str() + std::string(" of size ") + std::to_string(file_size) +
                               std::string(" doesn't equals to the required size of matrix of size ") + std::to_string(size * sizeof(T)));
    }
  }
  catch(std::exception &e)
  {
    std::cerr << "Caught " << typeid( e ).name( ) << " : "<< e.what( ) << std::endl;
    return ;
  }

  std::ifstream input(problem.str().c_str(), std::ios::binary);
  if (!input.is_open()) {
    throw new std::logic_error(std::string("error reading file: ") +
                               problem.str());
  }

  for(std::size_t j = 0; j < nblocks; j++){
      for(std::size_t i = 0; i < mblocks; i++){
          for(std::size_t q = 0; q < c_lens[j]; q++){
              input.seekg(((q + c_offs[j]) * N + r_offs[i])* sizeof(T));
              input.read(reinterpret_cast<char*>(H + (q + c_offs_l[j]) * m + r_offs_l[i]), r_lens[i] * sizeof(T));
          }
      }
  }

}

struct ChASE_DriverProblemConfig {
  std::size_t N;    // Size of the Matrix
  std::size_t nev;  // Number of sought after eigenvalues
  std::size_t nex;  // Extra size of subspace
  std::size_t deg;  // initial degree
  std::size_t bgn;  // beginning of sequence
  std::size_t end;  // end of sequence

  std::size_t maxIter; // maximum number of subspace iterations within ChASE.
  std::size_t maxDeg; //maximum value of the degree of the Chebyshev filter
  double tol;     // desired tolerance
  bool sequence;  // handle this as a sequence?

  std::string path_in;    // path to the matrix input files
  std::string mode;       // Approx or Random mode
  std::string opt;        // enable optimisation of degree
  std::string arch;       // ??
  std::string path_eigp;  // TODO
  std::string path_out;
  std::string path_name;

  std::size_t kpoint;
  bool legacy;
  std::string spin;

  bool iscomplex;
  bool isdouble;

  std::size_t lanczosIter;
  std::size_t numLanczos;

#ifdef USE_BLOCK_CYCLIC
  std::size_t mbsize;
  std::size_t nbsize;
  int dim0;
  int dim1;
  int irsrc;
  int icsrc;
  std::string major;
#endif
};

template <typename T>
int do_chase(ChASE_DriverProblemConfig& conf) {
  // todo due to legacy reasons we unpack the struct
  std::size_t N = conf.N;
  std::size_t nev = conf.nev;
  std::size_t nex = conf.nex;
  std::size_t deg = conf.deg;
  std::size_t bgn = conf.bgn;
  std::size_t end = conf.end;
  std::size_t maxDeg = conf.maxDeg;
  std::size_t maxIter = conf.maxIter;

  double tol = conf.tol;
  bool sequence = conf.sequence;

  std::string path_in = conf.path_in;
  std::string mode = conf.mode;
  std::string opt = conf.opt;
  std::string arch;
  std::string path_eigp = conf.path_eigp;
  std::string path_out = conf.path_out;
  std::string path_name = conf.path_name;

  std::size_t lanczosIter = conf.lanczosIter;
  std::size_t numLanczos = conf.numLanczos;

  std::size_t kpoint = conf.kpoint;
  bool legacy = conf.legacy;
  std::string spin = conf.spin;
#ifdef USE_BLOCK_CYCLIC
  std::size_t mbsize = conf.mbsize;
  std::size_t nbsize = conf.nbsize;
  int dim0 = conf.dim0;
  int dim1 = conf.dim1;
  int irsrc = conf.irsrc;
  int icsrc = conf.irsrc;
  std::string major = conf.major;

  if(dim0 == 0 || dim1 == 0){
    int dims[2];
    dims[0] = dims[1] = 0;
    int gsize;
    MPI_Comm_size(MPI_COMM_WORLD, &gsize);
    MPI_Dims_create(gsize, 2, dims);
    dim0 = dims[0];
    dim1 = dims[1];
  }
#endif
  int rank, size;

#ifdef USE_MPI
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
  MPI_Comm_size(MPI_COMM_WORLD, &size);
#else
  rank = 0;
  size = 1;
#endif

  //----------------------------------------------------------------------------
  std::cout << std::setprecision(16);

  auto V__ = std::unique_ptr<T[]>(new T[N * (nev + nex)]);
  auto Lambda__ = std::unique_ptr<Base<T>[]>(new Base<T>[(nev + nex)]);

  T* V = V__.get();
  Base<T>* Lambda = Lambda__.get();

#ifdef USE_MPI
    #ifdef DRIVER_BUILD_MGPU
        typedef ChaseMpi<ChaseMpiDLAMultiGPU, T> CHASE;
    #else
        typedef ChaseMpi<ChaseMpiDLABlaslapack, T> CHASE;
    #endif //CUDA or not
#else
    typedef ChaseMpi<ChaseMpiDLABlaslapackSeq, T> CHASE;
#endif //seq ChASE

#ifdef USE_MPI
#ifdef USE_BLOCK_CYCLIC
  CHASE single(new ChaseMpiProperties<T>(N, mbsize, nbsize, nev, nex, dim0, dim1, const_cast<char*>(major.c_str()), irsrc, icsrc, MPI_COMM_WORLD),
                    V, Lambda);
#else
  CHASE single(new ChaseMpiProperties<T>(N, nev, nex, MPI_COMM_WORLD), V,
               Lambda);
#endif
#else
  CHASE single(N, nev, nex, V, Lambda);
#endif

  ChaseConfig<T>& config = single.GetConfig();
  config.SetTol(tol);
  config.SetDeg(deg);
  config.SetOpt(opt == "S");
  config.SetLanczosIter(lanczosIter);
  config.SetNumLanczos(numLanczos);
  config.SetMaxDeg(maxDeg);
  config.SetMaxIter(maxIter);

  std::mt19937 gen(1337.0);
  std::normal_distribution<> d;

  T* H = single.GetMatrixPtr();

  if(!sequence){
    bgn = end = 1;
  }

  for (auto i = bgn; i <= end; ++i) {
    if (i == bgn || !sequence) {
      if (mode[0] == 'A') {
        readMatrix(V, path_eigp, spin, kpoint, i - 1, ".vct", N * (nev + nex),
                     legacy);
        readMatrix(Lambda, path_eigp, spin, kpoint, i - 1, ".vls", (nev + nex),
                     legacy);
      }else{
        for (std::size_t i = 0; i < N * (nev + nex); ++i) {
          V[i] = getRandomT<T>([&]() {
			return d(gen);
		     }
                 );
        }

        for (int j = 0; j < (nev + nex); j++) {
	  Lambda[j] = 0.0;
        };
      }
    }else{
      config.SetApprox(true);
    }

#ifdef USE_BLOCK_CYCLIC
  /*local block number = mblocks x nblocks*/
    std::size_t mblocks = single.get_mblocks();
    std::size_t nblocks = single.get_nblocks();

    /*local matrix size = m x n*/
    std::size_t m = single.get_m();
    std::size_t n = single.get_n();

    /*global and local offset/length of each block of block-cyclic data*/
    std::size_t *r_offs, *c_offs, *r_lens, *c_lens, *r_offs_l, *c_offs_l;

    single.get_offs_lens(r_offs, r_lens, r_offs_l, c_offs, c_lens, c_offs_l);
#else
    std::size_t xoff;
    std::size_t yoff;
    std::size_t xlen;
    std::size_t ylen;

    single.GetOff(&xoff, &yoff, &xlen, &ylen);
#endif

    std::chrono::high_resolution_clock::time_point start, end;
    std::chrono::duration<double> elapsed;

    start = std::chrono::high_resolution_clock::now();

    if(rank == 0) std::cout << "start reading matrix\n";
#ifdef USE_BLOCK_CYCLIC
    if(sequence){
      readMatrix(H, path_in, spin, kpoint, i, ".bin", N*N, legacy, m, mblocks, nblocks, r_offs, r_lens, r_offs_l, c_offs, c_lens, c_offs_l);
    }else{
      readMatrix(H, path_in, N*N, m, mblocks, nblocks, r_offs, r_lens, r_offs_l, c_offs, c_lens, c_offs_l);
    }
#else
    if(sequence){
      readMatrix(H, path_in, spin, kpoint, i, ".bin", N * N, legacy, xoff, yoff, xlen, ylen);
    }else{
      readMatrix(H, path_in, N * N, xoff, yoff, xlen, ylen);
    }
#endif

    MPI_Barrier(MPI_COMM_WORLD);

    end = std::chrono::high_resolution_clock::now();

    elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(end - start);

    if(rank == 0) std::cout <<  "matrix are loaded in " << elapsed.count() << " seconds" << std::endl;

    PerformanceDecoratorChase<T> performanceDecorator(&single);

#ifdef USE_MPI
    MPI_Barrier(MPI_COMM_WORLD);
#endif
    chase::Solve(&performanceDecorator);

#ifdef USE_MPI
    MPI_Barrier(MPI_COMM_WORLD);
#endif

    if (rank == 0) {
      std::cout << " ChASE timings: " << "\n";
      performanceDecorator.GetPerfData().print();
#ifdef PRINT_EIGENVALUES
      Base<T>* resid = single.GetResid();
      std::cout << "Finished Problem \n";
      std::cout << "Printing first 5 eigenvalues and residuals\n";
      std::cout
          << "| Index |       Eigenvalue      |         Residual      |\n"
          << "|-------|-----------------------|-----------------------|\n";
      std::size_t width = 20;
      std::cout << std::setprecision(12);
      std::cout << std::setfill(' ');
      std::cout << std::scientific;
      std::cout << std::right;
      for (auto i = 0; i < std::min(std::size_t(5), nev); ++i)
        std::cout << "|  " << std::setw(4) << i + 1 << " | " << std::setw(width)
                  << Lambda[i] << "  | " << std::setw(width) << resid[i]
                  << "  |\n";
      std::cout << "\n\n\n";
#endif
    }
  }

  return 0;

}

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

#ifdef USE_MPI
  MPI_Init(&argc, &argv);
#endif

  ChASE_DriverProblemConfig conf;

  po::options_description desc("ChASE Options");

  desc.add_options()(                                                     //
      "help,h",                                                           //
      "show this message"                                                 //
      )(                                                                  //
      "n", po::value<std::size_t>(&conf.N)->required(),                   //
      "Size of the Input Matrix"                                          //
      )(                                                                  //
      "double", po::value<bool>(&conf.isdouble)->default_value(true),     //
      "Is matrix double valued, false indicates the single type"  		  //
      )(                                                                  //
      "complex", po::value<bool>(&conf.iscomplex)->default_value(true),   //
      "Matrix is complex, false indicated the real matrix"		          //
      )(                                                                  //
      "nev", po::value<std::size_t>(&conf.nev)->required(),               //
      "Wanted Number of Eigenpairs"                                       //
      )(                                                                  //
      "nex", po::value<std::size_t>(&conf.nex)->default_value(25),        //
      "Extra Search Dimensions"                                           //
      )(                                                                  //
      "deg", po::value<std::size_t>(&conf.deg)->default_value(20),        //
      "Initial filtering degree"                                          //
      )(
      "maxDeg", po::value<std::size_t>(&conf.maxDeg)->default_value(36),  //
      "Sets the maximum value of the degree of the Chebyshev filter"
      )(
      "maxIter", po::value<std::size_t>(&conf.maxIter)->default_value(25), //
      "Sets the value of the maximum number of subspace iterations"
      "within ChASE"
      )(                                                                  //
      "bgn", po::value<std::size_t>(&conf.bgn)->default_value(2),         //
      "Start ell"                                                         //
      )(                                                                  //
      "end", po::value<std::size_t>(&conf.end)->default_value(2),         //
      "End ell"                                                           //
      )(                                                                  //
      "spin", po::value<std::string>(&conf.spin)->default_value("d"),     //
      "spin"                                                              //
      )(                                                                  //
      "kpoint", po::value<std::size_t>(&conf.kpoint)->default_value(0),   //
      "kpoint"                                                            //
      )(                                                                  //
      "tol", po::value<double>(&conf.tol)->default_value(1e-10),          //
      "Tolerance for Eigenpair convergence"                               //
      )(                                                                  //
      "path_in", po::value<std::string>(&conf.path_in)->required(),       //
      "Path to the input matrix/matrices"                                 //
      )(                                                                  //
      "mode", po::value<std::string>(&conf.mode)->default_value("A"),     //
      "valid values are R(andom) or A(pproximate)"                        //
      )(                                                                  //
      "opt", po::value<std::string>(&conf.opt)->default_value("S"),       //
      "Optimi(S)e degree, or do (N)ot optimise"                           //
      )(                                                                  //
      "path_eigp", po::value<std::string>(&conf.path_eigp),               //
      "Path to approximate solutions, only required when mode"            //
      "is Approximate, otherwise not used"                                //
      )(                                                                  //
      "sequence", po::value<bool>(&conf.sequence)->default_value(false),  //
      "Treat as sequence of Problems. Previous ChASE solution is used,"   //
      "when available"                                                    //
      )(
      "lanczosIter",po::value<std::size_t>(&conf.lanczosIter)->default_value(25),
      "Sets the number of Lanczos iterations executed by ChASE."
      )(
      "numLanczos", po::value<std::size_t>(&conf.numLanczos)->default_value(4),
      " Sets the number of stochastic vectors used for the spectral estimates"
      "in Lanczos"
      )
#ifdef USE_BLOCK_CYCLIC
      (                                                                   //
      "mbsize", po::value<std::size_t>(&conf.mbsize)->default_value(400),  //
      "block size for the row"                                            //
      )(                                                                  //
      "nbsize", po::value<std::size_t>(&conf.nbsize)->default_value(400),  //
      "block size for the column"                                         //
      )(                                                                  //
      "dim0", po::value<int>(&conf.dim0)->default_value(0),               //
      "row number of MPI proc grid"                                       //
      )(                                                                  //
      "dim1", po::value<int>(&conf.dim1)->default_value(0),               //
      "column number of MPI proc grid"                                    //
      )(								  //
      "irsrc", po::value<int>(&conf.irsrc)->default_value(0),             //
      "The process row over which the first row of matrix is"             //
      "distributed."                                                      //
      )(                                                                  //
      "icsrc", po::value<int>(&conf.icsrc)->default_value(0),             //
      "The process column over which the first column of the array A is"  //
      "distributed."                                                      //
      )(                                                                  //
      "major", po::value<std::string>(&conf.mode)->default_value("C"),    //
      "Major of MPI proc grid, valid values are R(ow) or C(olumn)"        //
      )
#endif
      ("legacy", po::value<bool>(&conf.legacy)->default_value(false),     //
      "Use legacy naming scheme?");                                       //

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);

  // print help
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 1;
  }

  po::notify(vm);
  conf.mode = toupper(conf.mode.at(0));
  conf.opt = toupper(conf.opt.at(0));

  if (conf.bgn > conf.end) {
    std::cout << "Begin must be smaller than End!" << std::endl;
    return -1;
  }

  if (conf.mode != "R" && conf.mode != "A") {
    std::cout << "Illegal value for mode: \"" << conf.mode << "\"" << std::endl
              << "Legal values are R or A" << std::endl;
    return -1;
  }

  if (conf.opt != "N" && conf.opt != "S") {
    std::cout << "Illegal value for opt: " << conf.opt << std::endl
              << "Legal values are N, S" << std::endl;
    return -1;
  }

  if (conf.path_eigp.empty() && conf.mode == "A") {
    std::cout << "eigp is required when mode is " << conf.mode << std::endl;
    return -1;
  }

  if (conf.isdouble) {
	if (conf.iscomplex) {
    	do_chase<std::complex<double>>(conf);
	} else {
		do_chase<double>(conf);
	}
  } else {
    std::cout << "single not implemented\n";
  }

#ifdef USE_MPI
  MPI_Finalize();
#else
  return 0;
#endif

}


// Copyright (c) 2012-2013 The PPCoin developers
// Copyright (c) 2014 The ABCChain developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/assign/list_of.hpp>

#include "kernel.h"
#include "txdb.h"

using namespace std;

// Get time weight
int64_t GetWeight(int64_t nIntervalBeginning, int64_t nIntervalEnd)
{
    // Kernel hash weight starts from 0 at the min age
    // this change increases active coins participating the hash and helps
    // to secure the network when proof-of-stake difficulty is low

    return nIntervalEnd - nIntervalBeginning - nStakeMinAge;
}

// Stake Modifier (hash modifier of proof-of-stake):
// The purpose of stake modifier is to prevent a txout (coin) owner from
// computing future proof-of-stake generated by this txout at the time
// of transaction confirmation. To meet kernel protocol, the txout
// must hash with a future stake modifier to generate the proof.
uint256 ComputeStakeModifier(const CBlockIndex* pindexPrev, const uint256& kernel)
{
    if (!pindexPrev)
        return 0;  // genesis block's modifier is 0

    CDataStream ss(SER_GETHASH, 0);
    ss << kernel << pindexPrev->bnStakeModifier;
    return Hash(ss.begin(), ss.end());
}

// ABCChain kernel protocol
// coinstake must meet hash target according to the protocol:
// kernel (input 0) must meet the formula
//     hash(nStakeModifier + txPrev.block.nTime + txPrev.nTime + txPrev.vout.hash + txPrev.vout.n + nTime) < bnTarget * nWeight
// this ensures that the chance of getting a coinstake is proportional to the
// amount of coins one owns.
// The reason this hash is chosen is the following:
//   nStakeModifier: scrambles computation to make it very difficult to precompute
//                   future proof-of-stake
//   txPrev.nTime: slightly scrambles computation
//   txPrev.vout.hash: hash of txPrev, to reduce the chance of nodes
//                     generating coinstake at the same time
//   txPrev.vout.n: output number of txPrev, to reduce the chance of nodes
//                  generating coinstake at the same time
//   nTime: current timestamp
//   block/tx hash should not be used here as they can be generated in vast
//   quantities so as to generate blocks faster, degrading the system back into
//   a proof-of-work situation.
//
static bool CheckStakeKernelHash(CBlockIndex* pindexPrev, unsigned int nBits, const CTransaction& txPrev, const COutPoint& prevout, unsigned int nTimeTx, uint256& hashProofOfStake, uint256& targetProofOfStake, bool fPrintProofOfStake=false)
{
    if (nTimeTx < txPrev.nTime)  // Transaction timestamp violation
        return error("CheckStakeKernelHash() : nTime violation");

    // Base target
    CBigNum bnTarget;
    bnTarget.SetCompact(nBits);

    // Weighted target
    int64_t nValueIn = txPrev.vout[prevout.n].nValue;
    CBigNum bnWeight = CBigNum(nValueIn);
    bnTarget *= bnWeight;

    targetProofOfStake = bnTarget.getuint256();

    uint256 bnStakeModifier = pindexPrev->bnStakeModifier;
    int nStakeModifierHeight = pindexPrev->nHeight;
    int64_t nStakeModifierTime = pindexPrev->nTime;

    // Calculate hash
    CDataStream ss(SER_GETHASH, 0);

    ss << bnStakeModifier << txPrev.nTime << prevout.hash << prevout.n << nTimeTx;
    hashProofOfStake = Hash(ss.begin(), ss.end());

    if (fPrintProofOfStake)
    {
        LogPrintf("CheckStakeKernelHash() : using modifier %s at height=%d timestamp=%s\n",
            bnStakeModifier.ToString(), nStakeModifierHeight,
            DateTimeStrFormat(nStakeModifierTime));
        LogPrintf("CheckStakeKernelHash() : check modifier=%s nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s\n",
            bnStakeModifier.ToString(),
            txPrev.nTime, prevout.n, nTimeTx,
            hashProofOfStake.ToString());
    }

    // Now check if proof-of-stake hash meets target protocol
    if (CBigNum(hashProofOfStake) > bnTarget)
        return false;

    if (fDebug && !fPrintProofOfStake)
    {
        LogPrintf("CheckStakeKernelHash() : using modifier %s at height=%d timestamp=%s\n",
            bnStakeModifier.ToString(), nStakeModifierHeight,
            DateTimeStrFormat(nStakeModifierTime));
        LogPrintf("CheckStakeKernelHash() : pass modifier=%s nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s\n",
            bnStakeModifier.ToString(),
            txPrev.nTime, prevout.n, nTimeTx,
            hashProofOfStake.ToString());
    }

    return true;
}

// Check kernel hash target and coinstake signature
bool CheckProofOfStake(CBlockIndex* pindexPrev, const CTransaction& tx, unsigned int nBits, uint256& hashProofOfStake, uint256& targetProofOfStake)
{
    if (!tx.IsCoinStake())
        return error("CheckProofOfStake() : called on non-coinstake %s", tx.GetHash().ToString());

    // Kernel (input 0) must match the stake hash target per coin age (nBits)
    const CTxIn& txin = tx.vin[0];

    // First try finding the previous transaction in database
    CTxDB txdb("r");
    CTransaction txPrev;
    CTxIndex txindex;
    if (!txPrev.ReadFromDisk(txdb, txin.prevout, txindex))
        return tx.DoS(1, error("CheckProofOfStake() : INFO: read txPrev failed"));  // previous transaction not in main chain, may occur during initial download

    // Verify signature
    if (!VerifySignature(txPrev, tx, 0, SCRIPT_VERIFY_NONE, 0))
        return tx.DoS(100, error("CheckProofOfStake() : VerifySignature failed on coinstake %s", tx.GetHash().ToString()));

    // Read block header
    CBlock block;
    if (!block.ReadFromDisk(txindex.pos.nFile, txindex.pos.nBlockPos, false))
        return fDebug? error("CheckProofOfStake() : read block failed") : false; // unable to read block of previous transaction

    // Min age requirement
    int nDepth;
    if (IsConfirmedInNPrevBlocks(txindex, pindexPrev, nStakeMinConfirmations - 1, nDepth))
        return tx.DoS(100, error("CheckProofOfStake() : tried to stake at depth %d", nDepth + 1));

    if (!CheckStakeKernelHash(pindexPrev, nBits, txPrev, txin.prevout, tx.nTime, hashProofOfStake, targetProofOfStake, fDebug))
        return tx.DoS(1, error("CheckProofOfStake() : INFO: check kernel failed on coinstake %s, hashProof=%s", tx.GetHash().ToString(), hashProofOfStake.ToString())); // may occur during initial download or if behind on block chain sync

    return true;
}

// Check whether the coinstake timestamp meets protocol
bool CheckCoinStakeTimestamp(int nHeight, int64_t nTimeBlock, int64_t nTimeTx)
{
    return (nTimeBlock == nTimeTx) && ((nTimeTx & STAKE_TIMESTAMP_MASK) == 0);
}

bool CheckKernel(CBlockIndex* pindexPrev, unsigned int nBits, int64_t nTime, const COutPoint& prevout, int64_t* pBlockTime)
{
    uint256 hashProofOfStake, targetProofOfStake;

    CTxDB txdb("r");
    CTransaction txPrev;
    CTxIndex txindex;
    if (!txPrev.ReadFromDisk(txdb, prevout, txindex))
        return false;

    // Read block header
    CBlock block;
    if (!block.ReadFromDisk(txindex.pos.nFile, txindex.pos.nBlockPos, false))
        return false;

    int nDepth;
    if (IsConfirmedInNPrevBlocks(txindex, pindexPrev, nStakeMinConfirmations - 1, nDepth))
        return false;

    if (pBlockTime)
        *pBlockTime = block.GetBlockTime();

    return CheckStakeKernelHash(pindexPrev, nBits, txPrev, prevout, nTime, hashProofOfStake, targetProofOfStake);
}


#include <CCeilPI.h>
#include <COSNaN.h>
#include <CInvNorm.h>
#include <boost/math/special_functions/erf.hpp>
#include <cerrno>

void
ClParserReal::
copy(const ClParserObj &obj)
{
  assert(base_type_ == obj.getBaseType());

  const ClParserReal &rhs = castObj(obj);

  *this = rhs;
}

ClParserReal *
ClParserReal::
dup() const
{
  return new ClParserReal(*this);
}

bool
ClParserReal::
isNanValue() const
{
  if (COSNaN::is_nan(real_))
    return true;

  return false;
}

long
ClParserReal::
getInteger() const
{
  // TODO: range check

  if (! isNanValue())
    return long(real_);
  else
    return 0;
}

bool
ClParserReal::
toBool() const
{
  return (! isNanValue() && real_ != 0.0);
}

//-----------------

// Inline Ops

const ClParserObj &
ClParserReal::
increment()
{
  if (! isNanValue())
    ++real_;

  return *this;
}

const ClParserObj &
ClParserReal::
decrement()
{
  if (! isNanValue())
    --real_;

  return *this;
}

//-----------------

// Unary Ops

ClParserValuePtr
ClParserReal::
unaryPlus() const
{
  return ClParserValueMgrInst->createValue( real_);
}

ClParserValuePtr
ClParserReal::
unaryMinus() const
{
  return ClParserValueMgrInst->createValue(-real_);
}

ClParserValuePtr
ClParserReal::
bitNot() const
{
  if (! isNanValue())
    return ClParserValueMgrInst->createValue(~getInteger());
  else
    return ClParserValueMgrInst->createValue( long(0    ));
}

//-----------------

// Binary Ops

ClParserValuePtr
ClParserReal::
plus(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ + real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
minus(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ - real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
times(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ * real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
divide(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if      (real.real_ == 0.0) {
    if (ClParserInst->getMathFail())
      ClErrThrow(ClErr::DIVIDE_BY_ZERO);

    if (real_ < 0.0)
      SetNegInf(result);
    else
      SetPosInf(result);
  }
  else if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ / real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
modulus(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  errno = 0;

  double result = CMathGen::modulus(real_, real.real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
power(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  errno = 0;

  double result = CMathGen::pow(real_, real.real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
approxEqual(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(::fabs(real_ - real.real_) < ClParserInst->getTolerance());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitAnd(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(getInteger() & real.getInteger());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitOr(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(getInteger() | real.getInteger());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitXor(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(getInteger() ^ real.getInteger());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitLShift(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result = real_*CMathGen::pow(2.0, real.real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitRShift(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result = real_/CMathGen::pow(2.0, real.real_);

  return ClParserValueMgrInst->createValue(result);
}

//-----------------

// Intern Fn

ClParserValuePtr
ClParserReal::
abs() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else if (real_ < 0.0)
    result = -real_;
  else
    result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
ceil() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::ceil(real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
floor() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::floor(real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sign() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else if (real_ > 0.0)
    result =  1.0;
  else if (real_ < 0.0)
    result = -1.0;
  else
    result =  0.0;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sqr() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ * real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sqrt() const
{
  errno = 0;

  double result = CMathGen::sqrt(real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
cos() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::cos(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sin() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::sin(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
tan() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::tan(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
acos() const
{
  errno = 0;

  double result = ClParserInst->fromRadians(CMathGen::acos(real_));

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
asin() const
{
  errno = 0;

  double result = ClParserInst->fromRadians(CMathGen::asin(real_));

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
atan() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ClParserInst->fromRadians(::atan(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
atan(double real) const
{
  double result;

  if (isNanValue() || COSNaN::is_nan(real))
    COSNaN::set_nan(&result);
  else
    result = ClParserInst->fromRadians(::atan2(real_, real));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
exp() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::exp(real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
log() const
{
  errno = 0;

  double result = CMathGen::log(real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
log10() const
{
  errno = 0;

  double result = CMathGen::log10(real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
cosh() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::cosh(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sinh() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::sinh(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
tanh() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::tanh(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
norm() const
{
  double result = real_;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else {
    result = 0.5*std::sqrt(2.0)*result;
    result = 0.5*erfc(-result);
  }

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
invnorm() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else {
    result = CInvNorm::calc(real_);

    if (IsPosInf(result) || IsNegInf(result))
      SetNaN(result);
  }

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toChar() const
{
  std::string result(" ");

  result[0] = (char) getInteger();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toInt() const
{
  long result = getInteger();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toReal() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toString() const
{
  std::string result = asString();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
isNan() const
{
  long result = isNanValue();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toLower() const
{
  int c = (int) getInteger();

  if (isupper(c))
    c = tolower(c);

  long value = (long) c;

  return ClParserValueMgrInst->createValue(value);
}

ClParserValuePtr
ClParserReal::
toUpper() const
{
  int c = (int) getInteger();

  if (islower(c))
    c = toupper(c);

  long value = (long) c;

  return ClParserValueMgrInst->createValue(value);
}

ClParserValuePtr
ClParserReal::
min() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
max() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sum() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
index(const ClParserObj &) const
{
  ClErrThrow(ClErr::INVALID_TYPE_FOR_OPERATOR);

  return ClParserValuePtr();
}

ClParserValuePtr
ClParserReal::
rindex(const ClParserObj &) const
{
  ClErrThrow(ClErr::INVALID_TYPE_FOR_OPERATOR);

  return ClParserValuePtr();
}

ClParserValuePtr
ClParserReal::
sort(ClParserSortDirection) const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
doAssert() const
{
  assert(real_);

  return ClParserValuePtr();
}

//-----------------

int
ClParserReal::
cmp(const ClParserObj &obj) const
{
  if (base_type_ != obj.getBaseType())
    return CMathGen::sign((long) (base_type_ - obj.getBaseType()));

  const ClParserReal &rhs = castObj(obj);

  if      (isNanValue() && rhs.isNanValue())
    return 1;
  else if (isNanValue())
    return 1;
  else if (rhs.isNanValue())
    return -1;
  else if (real_ < rhs.real_)
    return -1;
  else if (real_ > rhs.real_)
    return 1;
  else
    return 0;
}

//------

std::string
ClParserReal::
asString() const
{
  if      (IsPosInf(real_))
    return "+Inf";
  else if (IsNegInf(real_))
    return "-Inf";
  else if (isNanValue())
    return "NaN";
  else
    return ClParserInst->toString(real_);
}

void
ClParserReal::
print() const
{
  ClParserInst->output("%s", asString().c_str());
}

void
ClParserReal::
print(std::ostream &os) const
{
  os << asString();
}

void
ClParserReal::
debugPrint() const
{
  fprintf(stderr, " %s", asString().c_str());
}


#include <string>
#include <ros/ros.h>											// 包含ROS的头文件
#include <ros/time.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <boost/asio.hpp>										//包含boost库函数
#include <boost/bind.hpp>
#include <math.h>
#include <geometry_msgs/Twist.h>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>

using namespace std;
using namespace boost::asio;									//定义一个命名空间，用于后面的读写操作
ros::Time current_time, last_time;
void writeSerial(const geometry_msgs::Twist& msg);

io_service iosev;												//节点文件
serial_port sp(iosev, "/dev/mrobot");
char* FILENAME;
void readConfigFile(char* filename);
void writeConfigFile(char* filename);

#define		R			105.00
#define		L			210.50
#define		fastAbs(n)	n > 0 ? n : -n
#define		PI			3.1415926535897932f

int _ID_FLAG = 0, _READ = 1;
unsigned char Get_Crc8(unsigned char *ptr, unsigned short len)
{
	unsigned char crc;
	unsigned char i;
	crc = 0;
	while(len--)
	{
		crc ^= *ptr++;
		for(i = 0; i < 8; i++)
		{
			if(crc&0x01)crc=(crc>>1)^0x8C;
			else crc >>= 1;
		}
	}
	return crc;
}

const string toHexString(const unsigned char* input, const int datasize)
{
	string output;
	char ch[3];

	for(int i = 0; i < datasize; ++i)
	{
		snprintf(ch, 3, "%02x", input[i]);
		output += ch;
	}
	return output;
}

bool compareID(const unsigned char* ID, string ID_FROM_FILE)
{
	string temp;
	temp = toHexString(ID, 12);
	if (temp == ID_FROM_FILE)
	{
		return true;
	} else
	{
		return false;
	}
}

double x = 0.0;
double y = 0.0;
double th = 0.0;

double vx = 0.0;
double vy = 0.0;
double vth = 0.0;

enum SendCommandType {
	SIMPLE_INFO = 23130,
	WITH_SONAR_INFO = 23125,
	WITH_SIX_AXIS_SENSOR_INFO = 23210,
	ALL_INFO = 23205,
	SPEED_INFO = 42330,
	BATTERY_INFO = 42325,
	SONAR_INFO = 42410,
	SIX_AXIS_SENSOR_INFO = 42405,
};

enum ReceiveCommandType {
	REQUIRE_WITH_SIMPLE_INFO = 0,
	REQUIRE_WITH_SONAR_INFO,
	REQUIRE_WITH_SIX_AXIS_SENSOR_INFO,
	REQUIRE_WITH_ALL_INFO,
	REQUIRE_WITH_SPEED_INFO,
	REQUIRE_WITH_BATTERY_INFO,
	REQUIRE_WITH_ONLY_SONAR_INFO,
	REQUIRE_WITH_ONLY_SIX_AXIS_SENSOR_INFO,
};

union sendData
{
	int d;
	unsigned char data[4];
}leftdata, rightdata;

union checkSum
{
	short d;
	unsigned char data[1];
}SendCheckSum, ReceiveCheckSum;

union receiveHeader
{
	int d;
	unsigned char data[2];
}receive_command, receive_header;

union receiveEnder
{
	int d;
	unsigned char data[2];
}receive_ender;

union sendCommand
{
	int d;
	unsigned char data[2];
}send_command;

union odometry													//里程计数据共用体
{
	float odoemtry_float;
	unsigned char odometry_char[4];
}vel_left, vel_right, temprature, odom_yaw, odom_pitch, odom_roll, sonar_front, sonar_back, sonar_left, sonar_right;

union battery													//电池数据共用体
{
	float battery_float;
	unsigned char battery_char[4];
}electricity, battery_percentage;

unsigned char buf[200];											//定义字符串长度
unsigned char _ID[12];											//存放芯片ID的临时变量
string read_str;
short command = -1;
const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};

void readSerial()
{
	int i, _length = 0;
	unsigned char _checkSum;
	read(sp, buffer(buf));
	ros::Time curr_time;
	for (i = 0; i < 2; i++)
	{
		receive_header.data[i] = buf[i];
	}
	if (receive_header.data[0] == header[0] && receive_header.data[1] == header[1])
	{
		for (i = 0; i < 2; i++)
		{
			receive_command.data[i] = buf[i + 2];
		}
		_length = buf[4];
		_checkSum = Get_Crc8(buf, 5 + _length);
		switch(receive_command.d)
		{
			case SIMPLE_INFO:
				command = REQUIRE_WITH_SIMPLE_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							vel_left.odometry_char[i] = buf[i + 5];
							vel_right.odometry_char[i] = buf[i + 9];
							electricity.battery_char[i] = buf[i + 13];
							battery_percentage.battery_char[i] = buf[i + 17];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
			case WITH_SONAR_INFO:
				command = REQUIRE_WITH_SONAR_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							vel_left.odometry_char[i] = buf[i + 5];
							vel_right.odometry_char[i] = buf[i + 9];
							electricity.battery_char[i] = buf[i + 13];
							battery_percentage.battery_char[i] = buf[i + 17];
							sonar_front.odometry_char[i] = buf[i + 21];
							sonar_back.odometry_char[i] = buf[i + 25];
							sonar_left.odometry_char[i] = buf[i + 29];
							sonar_right.odometry_char[i] = buf[i + 33];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
			case WITH_SIX_AXIS_SENSOR_INFO:
				command = REQUIRE_WITH_SIX_AXIS_SENSOR_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							vel_left.odometry_char[i] = buf[i + 5];
							vel_right.odometry_char[i] = buf[i + 9];
							electricity.battery_char[i] = buf[i + 13];
							battery_percentage.battery_char[i] = buf[i + 17];
							odom_pitch.odometry_char[i] = buf[i + 21];
							odom_roll.odometry_char[i] = buf[i + 25];
							odom_yaw.odometry_char[i] = buf[i + 29];
							temprature.odometry_char[i] = buf[i + 33];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
			case ALL_INFO:
				command = REQUIRE_WITH_ALL_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							vel_left.odometry_char[i] = buf[i + 5];
							vel_right.odometry_char[i] = buf[i + 9];
							electricity.battery_char[i] = buf[i + 13];
							battery_percentage.battery_char[i] = buf[i + 17];
							odom_pitch.odometry_char[i] = buf[i + 21];
							odom_roll.odometry_char[i] = buf[i + 25];
							odom_yaw.odometry_char[i] = buf[i + 29];
							temprature.odometry_char[i] = buf[i + 33];
							sonar_front.odometry_char[i] = buf[i + 37];
							sonar_back.odometry_char[i] = buf[i + 41];
							sonar_left.odometry_char[i] = buf[i + 45];
							sonar_right.odometry_char[i] = buf[i + 49];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
			case SPEED_INFO:
				command = REQUIRE_WITH_SPEED_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							vel_left.odometry_char[i] = buf[i + 5];
							vel_right.odometry_char[i] = buf[i + 9];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
			case BATTERY_INFO:
				command = REQUIRE_WITH_BATTERY_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							electricity.battery_char[i] = buf[i + 5];
							battery_percentage.battery_char[i] = buf[i + 9];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
			case SONAR_INFO:
				command = REQUIRE_WITH_ONLY_SONAR_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							sonar_front.odometry_char[i] = buf[i + 5];
							sonar_back.odometry_char[i] = buf[i + 9];
							sonar_left.odometry_char[i] = buf[i + 13];
							sonar_right.odometry_char[i] = buf[i + 17];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
			case SIX_AXIS_SENSOR_INFO:
				command = REQUIRE_WITH_ONLY_SIX_AXIS_SENSOR_INFO;
				if (buf[6 + _length] == ender[0] && buf[6 + _length + 1] == ender[1])
				{
					ReceiveCheckSum.data[0] = buf[5 + _length];
					if (_checkSum == ReceiveCheckSum.d)
					{
						for(i = 0; i < 4; i++)
						{
							odom_pitch.odometry_char[i] = buf[i + 5];
							odom_roll.odometry_char[i] = buf[i + 9];
							odom_yaw.odometry_char[i] = buf[i + 13];
							temprature.odometry_char[i] = buf[i + 17];
						}
						vx = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000;
						vth = (vel_right.odoemtry_float - vel_left.odoemtry_float) / L;

						curr_time = ros::Time::now();

						double dt = (curr_time - last_time).toSec();
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						double delta_th = vth * dt;

						x += delta_x;
						y += delta_y;
						th += delta_th;
						last_time = curr_time;
					}
					else
					{
						ROS_INFO("Received data check sum error!");
					}
				}
			break;
		}
		for(i = 0; i < 12; i++)
		{
			_ID[i] = buf[8 + _length + i];
		}
		readConfigFile(FILENAME);
	}
}

void writeSerial(const geometry_msgs::Twist& msg)
{
	double RobotV = msg.linear.x * 1000;
	double YawRate = msg.angular.z;
	unsigned char buf_[16] = {0};
	int i, length = 0;
	double r = RobotV / YawRate;
	if(RobotV == 0)
	{
		leftdata.d = -YawRate * R;
		rightdata.d = YawRate * R;
	} else if(YawRate == 0)
	{
		leftdata.d = RobotV;
		rightdata.d = RobotV;
	}
	else
	{
		leftdata.d = YawRate * (r - R);
		rightdata.d = YawRate * (r + R);
	}
	for(i = 0; i < 2; i++)
	{
		buf_[i] = header[i];
	}
	send_command.d = 21850;
	for(i = 0; i < 2; i++)
	{
		buf_[i + 2] = send_command.data[i];
	}
	length = 8;
	buf_[4] = length;
	for(i = 0; i < 4; i++)
	{
		buf_[i + 5] = rightdata.data[i];
		buf_[i + 9] = leftdata.data[i];
	}
	buf_[5 + length] = Get_Crc8(buf_, 5 + length);
	buf_[6 + length] = ender[0];
	buf_[6 + length + 1] = ender[1];
	write(sp, buffer(buf_));
	readSerial();
}

void readConfigFile(char* filename)
{
	if (_READ)
	{
		ifstream file;
		file.open(filename, ios::in | ios::binary);
		if (file)
		{
			file >> read_str;
			if (compareID(_ID, read_str))
			{
				ROS_INFO("ID Compare Successful");
			}else
			{
				ROS_ERROR("ID Compare Failed");
				exit(-1);
			}
		} else
		{
			writeConfigFile(filename);
		}
		file.close();
		_READ = 0;
	}
}

void writeConfigFile(char* filename)
{
	ofstream file;
	file.open(filename, ios::out | ios::app | ios::binary);
	if (file)
	{
		file << toHexString(_ID, 12);
	} else
	{
		ROS_INFO("Error opening Config file %s!", filename);
	}
	file.close();
	_READ = 1;
}

int main(int argc, char** argv)
{
	FILENAME = strcat(getenv("HOME"), "/mrobot.config");
	try
	{
		// 设置参数
		sp.set_option(serial_port::baud_rate(115200));
		sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
		sp.set_option(serial_port::parity(serial_port::parity::none));
		sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
		sp.set_option(serial_port::character_size(8));
		ROS_INFO("Serial Port initialized successful.");
		ros::init(argc, argv, "serial_node");									//初始化节点
		ros::Time::init();
		current_time = ros::Time::now();
		last_time = ros::Time::now();
		ros::Rate loop_rate(50);
		ros::NodeHandle nh;
		ros::Publisher pub = nh.advertise<nav_msgs::Odometry>("odom", 50);		//定义发布消息的名称
		ros::Subscriber sub = nh.subscribe("cmd_vel", 50, &writeSerial);
		tf::TransformBroadcaster odom_broadcaster;
		ROS_INFO("ROS Node initialized successful.");

		while (ros::ok()) {
			ros::spinOnce();

			current_time = ros::Time::now();
			geometry_msgs::TransformStamped odom_trans;

			odom_trans.header.stamp = current_time;
			odom_trans.header.frame_id = "odom";
			odom_trans.child_frame_id = "base_footprint";

			geometry_msgs::Quaternion odom_quat;
			if (command == REQUIRE_WITH_ALL_INFO)
			{
				odom_quat = tf::createQuaternionMsgFromYaw(th);
				odom_trans.transform.translation.x = x;
				odom_trans.transform.translation.y = y;
				odom_trans.transform.translation.z = 0.0;
				odom_trans.transform.rotation = odom_quat;
				ROS_INFO("Left Speed: %.4f m/s.", vel_left.odoemtry_float / 1000);
				ROS_INFO("Right Speed: %.4f m/s.", vel_right.odoemtry_float / 1000);
				ROS_INFO("Battery Voltage: %.3f mV.", electricity.battery_float);
				ROS_INFO("Battery Electricity Percentage: %.3f%%.", battery_percentage.battery_float);
				ROS_INFO("Temprature: %.2f C.", temprature.odoemtry_float);
				ROS_INFO("Pitch: %.3f.", odom_pitch.odoemtry_float);
				ROS_INFO("Roll: %.3f.", odom_roll.odoemtry_float);
				ROS_INFO("Yaw: %.3f.", odom_yaw.odoemtry_float);
				ROS_INFO("Sonar front: %.3f m.", sonar_front.odoemtry_float);
				ROS_INFO("Sonar back: %.3f m.", sonar_back.odoemtry_float);
				ROS_INFO("Sonar left: %.3f m.", sonar_left.odoemtry_float);
				ROS_INFO("Sonar right: %.3f m.", sonar_right.odoemtry_float);
			} else if (command == REQUIRE_WITH_SIMPLE_INFO)
			{
				odom_quat = tf::createQuaternionMsgFromYaw(th);
				odom_trans.transform.translation.x = x;
				odom_trans.transform.translation.y = y;
				odom_trans.transform.translation.z = 0.0;
				odom_trans.transform.rotation = odom_quat;
				ROS_INFO("Left Speed: %.4f m/s.", vel_left.odoemtry_float / 1000);
				ROS_INFO("Right Speed: %.4f m/s.", vel_right.odoemtry_float / 1000);
				ROS_INFO("Battery Voltage: %.3f mV.", electricity.battery_float);
				ROS_INFO("Battery Electricity Percentage: %.3f%%.", battery_percentage.battery_float);
			} else if(command == REQUIRE_WITH_SIX_AXIS_SENSOR_INFO)
			{
				odom_quat = tf::createQuaternionMsgFromYaw(th);
				odom_trans.transform.translation.x = x;
				odom_trans.transform.translation.y = y;
				odom_trans.transform.translation.z = 0.0;
				odom_trans.transform.rotation = odom_quat;
				ROS_INFO("Left Speed: %.4f m/s.", vel_left.odoemtry_float / 1000);
				ROS_INFO("Right Speed: %.4f m/s.", vel_right.odoemtry_float / 1000);
				ROS_INFO("Battery Voltage: %.3f mV.", electricity.battery_float);
				ROS_INFO("Battery Electricity Percentage: %.3f%%.", battery_percentage.battery_float);
				ROS_INFO("Temprature: %.2f C.", temprature.odoemtry_float);
				ROS_INFO("Pitch: %.3f.", odom_pitch.odoemtry_float);
				ROS_INFO("Roll: %.3f.", odom_roll.odoemtry_float);
				ROS_INFO("Yaw: %.3f.", odom_yaw.odoemtry_float);
			} else
			{
				odom_quat = tf::createQuaternionMsgFromYaw(th);
				odom_trans.transform.translation.x = x;
				odom_trans.transform.translation.y = y;
				odom_trans.transform.translation.z = 0.0;
				odom_trans.transform.rotation = odom_quat;
			}

			//send the transform
			odom_broadcaster.sendTransform(odom_trans);

			nav_msgs::Odometry msgl;
			msgl.header.stamp = current_time;
			msgl.header.frame_id = "odom";

			msgl.pose.pose.position.x = x;
			msgl.pose.pose.position.y = y;
			msgl.pose.pose.position.z = 0.0;
			msgl.pose.pose.orientation = odom_quat;

			msgl.child_frame_id = "base_footprint";
			msgl.twist.twist.linear.x = vx;
			msgl.twist.twist.linear.y = vy;
			msgl.twist.twist.angular.z = vth;

			pub.publish(msgl);   											//发布消息
			command = -1;
			loop_rate.sleep();
		}
		iosev.run();
	}
	catch (exception& err)
	{
		ROS_ERROR("Exception Error: %s", err.what());
		return -1;
	}
	return 0;
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ReduceNMaxClassGather
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>

#include "poplibs_support/Algorithm.hpp"
#include "poplibs_test/Util.hpp"
#include "popnn/codelets.hpp"
#include "poputil/VertexTemplates.hpp"

#include <boost/program_options.hpp>
#include <boost/random.hpp>
#include <cstdint>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace poputil;
using namespace poplibs_support;

template <typename DataType>
std::vector<std::vector<DataType>> cpp_model(std::vector<DataType> in,
                                             unsigned topK, unsigned size,
                                             unsigned divisorLog2) {
  const unsigned divisor = (1u << divisorLog2);
  const int nOutputs = (size + divisor - 1) / divisor;
  std::vector<std::vector<DataType>> modelMaxActs;

  for (int i = 0; i < nOutputs; ++i) {
    modelMaxActs.push_back({});
    std::vector<DataType> &v = modelMaxActs.back();

    unsigned maxI = divisor * i;
    const unsigned end = (maxI + divisor > size) ? size : maxI + divisor;
    for (std::size_t j = maxI; j < end; ++j) {
      v.push_back(in[j]);
    }

    while (v.size() < topK) {
      v.push_back(std::numeric_limits<DataType>::lowest());
    }
    std::sort(v.begin(), v.end(), std::greater<DataType>());
    v.erase(v.begin() + topK, v.end());
  }

  return modelMaxActs;
}

template <typename DataType>
static bool doTest(const DeviceType &deviceType, const Type &dataType,
                   const Type &labelType, unsigned divisor, unsigned size,
                   unsigned topK, bool sort) {
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();

  const short unsigned int divisorLog2 = poplibs_support::ceilLog2(divisor);
  divisor = (1u << divisorLog2);

  const auto nOutputs = (size + divisor - 1) / divisor;

  Graph graph(target);
  popnn::addCodelets(graph);

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;

  // Allocate the activations, a.k.a the inputs.
  auto activations = graph.addVariable(dataType, {size}, "activations");
  graph.setTileMapping(activations, 0);
  auto rawHostActivations = allocateHostMemoryForTensor(
      activations, "activations", graph, uploadProg, downloadProg, tmap);

  // Allocate the input indices.
  auto maxIndices =
      graph.addVariable(UNSIGNED_INT, {nOutputs * topK}, "maxValuesIndices");
  graph.setTileMapping(maxIndices, 0);
  auto rawHostMaxIndices = allocateHostMemoryForTensor(
      maxIndices, "maxValuesIndices", graph, uploadProg, downloadProg, tmap);

  // Allocate the partial values.
  auto partialValues =
      graph.addVariable(dataType, {nOutputs * topK}, "maxValuePartials");
  graph.setTileMapping(partialValues, 0);
  auto rawModelPartialValues = allocateHostMemoryForTensor(
      partialValues, "maxValuePartials", graph, uploadProg, downloadProg, tmap);

  std::mt19937 randomEngine;

  std::vector<DataType> hostActivations(size);
  boost::random::uniform_int_distribution<unsigned> randDist(
      std::numeric_limits<unsigned>::lowest(),
      std::numeric_limits<unsigned>::max());

  for (auto &a : hostActivations) {
    unsigned tmp = randDist(randomEngine);
    std::memcpy(&a, &tmp, sizeof(DataType));

    // Remove NANs.
    if (std::isnan(a)) {
      tmp = tmp >> 2;
      std::memcpy(&a, &tmp, sizeof(DataType));
    }

    // Flush denormals to zero.
    if (std::is_floating_point<DataType>::value &&
        std::fabs(a) < std::numeric_limits<DataType>::min()) {
      a = 0;
    }
  }
  copy(target, hostActivations.data(), size, dataType,
       rawHostActivations.get());

  auto cs = graph.addComputeSet();

  std::string vertexName =
      templateVertex("popnn::ReduceMaxNClassGather", dataType, sort);

  auto v = graph.addVertex(cs, vertexName);
  graph.setTileMapping(v, 0);

  unsigned index = 0;
  graph.connect(v["activations"], activations);
  graph.connect(v["maxValues"], partialValues);
  graph.connect(v["maxValuesIndices"], maxIndices);
  graph.setInitialValue(v["index"], index);
  graph.setInitialValue(v["size"], size);
  graph.setInitialValue(v["divisorLog2"], divisorLog2);
  graph.setInitialValue(v["numK"], topK);
  graph.setInitialValue(v["shouldSort"], sort);

  Engine e(std::move(graph), Sequence{uploadProg, Execute(cs), downloadProg});
  attachStreams(e, tmap);

  device.bind([&](const Device &d) { e.loadAndRun(d); });

  // Create a flattened array of outputs from the device.
  std::vector<DataType> flattened_device_array;
  flattened_device_array.resize(nOutputs * topK);

  copy(target, dataType, rawModelPartialValues.get(),
       flattened_device_array.data(), nOutputs * topK);

  std::vector<unsigned> flattened_index_array(nOutputs * topK);
  copy(target, labelType, rawHostMaxIndices.get(), flattened_index_array.data(),
       nOutputs * topK);

  // Run the C++ model.
  std::vector<std::vector<DataType>> cpp_out =
      cpp_model(hostActivations, topK, size, divisorLog2);

  std::vector<DataType> flattened_cpp_array;
  for (std::vector<DataType> &topk : cpp_out) {
    for (DataType val : topk) {
      flattened_cpp_array.push_back(val);
    }
  }
  bool success = true;
  // We check the indices before we do any sorting stuff.
  for (unsigned i = 0; i < nOutputs * topK; ++i) {
    unsigned ind = flattened_index_array[i];
    if (ind != std::numeric_limits<unsigned>::max()) {
      success &= hostActivations[ind] == flattened_device_array[i];
    }
  }

  // We have to carefuly sort the flattened device output by sorting each topK
  // subarray separately. We do this if we aren't testing the inbuilt sorting
  // mechanism in the vertex.
  if (!sort) {
    for (unsigned i = 0; i < nOutputs; ++i) {
      std::sort(flattened_device_array.begin() + i * topK,
                flattened_device_array.begin() + ((i + 1) * topK),
                std::greater<DataType>());
    }
  }

  // Tolerance is zero as we are only copying the data around, should be no
  // floating point error.`
  for (unsigned i = 0; i < nOutputs * topK; ++i) {
    success &= flattened_cpp_array[i] == flattened_device_array[i];
  }
  return success;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType;
  Type activationType;
  Type labelType = UNSIGNED_INT;
  unsigned divisor, size, topK;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()("help", "Print help")(
      "device-type", po::value<DeviceType>(&deviceType)->required(),
      "Device Type")("activation-type",
                     po::value<Type>(&activationType)->required(),
                     "Element type for input activations")(
      "divisor", po::value<unsigned>(&divisor)->required(),
      "Factor by which to reduce max class")(
      "size", po::value<unsigned>(&size)->required(),
      "Total size to process with vertex")(
      "k", po::value<unsigned>(&topK)->required(),
      "Find the 'k' amount of top elements in the set");
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  if (activationType == FLOAT) {
    // Test without sorting the output.
    if (!doTest<float>(deviceType, activationType, labelType, divisor, size,
                       topK, false))
      return 1;

    // Check with sorting the output.
    if (!doTest<float>(deviceType, activationType, labelType, divisor, size,
                       topK, true))
      return 1;
  } else if (activationType == INT) {
    // Test without sorting the output.
    if (!doTest<int>(deviceType, activationType, labelType, divisor, size, topK,
                     false))
      return 1;

    // Check with sorting the output.
    if (!doTest<int>(deviceType, activationType, labelType, divisor, size, topK,
                     true))
      return 1;
  } else if (activationType == UNSIGNED_INT) {
    // Test without sorting the output.
    if (!doTest<unsigned>(deviceType, activationType, labelType, divisor, size,
                          topK, false))
      return 1;

    // Check with sorting the output.
    if (!doTest<unsigned>(deviceType, activationType, labelType, divisor, size,
                          topK, true))
      return 1;
  } else {
    // Type is unsupported.
    return 1;
  }
  return 0;
}


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

#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;
using namespace std;

struct cube_data_generator
{
   mp_t operator()(mp_t z)
   {
      mp_t result = z*z*z;
      // if result is out of range of a float,
      // don't include in test data as it messes up our results:
      if(result > (std::numeric_limits<float>::max)())
         throw std::domain_error("");
      if(result < (std::numeric_limits<float>::min)())
         throw std::domain_error("");
      return result;
   }
};

int main(int argc, char* argv[])
{
   parameter_info<mp_t> arg1;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the cbrt function:\n\n";

   bool cont;
   std::string line;

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(cube_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=cbrt_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "cbrt_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, "cbrt_data");

   return 0;
}


// Copyright (c) 2009-2010 Satoshi Nakamoto
// Copyright (c) 2009-2017 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <utilmoneystr.h>

#include <primitives/transaction.h>
#include <tinyformat.h>
#include <utilstrencodings.h>

// NOKUBIT: Nokubit AssetCoin
#include <boost/algorithm/string.hpp>

std::string FormatMoney(const CAmount& n)
{
    // Note: not using straight sprintf here because we do NOT want
    // localized number formatting.
    int64_t n_abs = (n > 0 ? n : -n);
    int64_t quotient = n_abs/COIN;
    int64_t remainder = n_abs%COIN;
    std::string str = strprintf("%d.%08d", quotient, remainder);

    // Right-trim excess zeros before the decimal point:
    int nTrim = 0;
    for (int i = str.size()-1; (str[i] == '0' && isdigit(str[i-2])); --i)
        ++nTrim;
    if (nTrim)
        str.erase(str.size()-nTrim, nTrim);

    if (n < 0)
        str.insert((unsigned int)0, 1, '-');
    return str;
}

// NOKUBIT: Nokubit AssetCoin
std::string FormatMoney(const asset_tag::AssetAmount& n)
{
    if (n != asset_tag::COIN_ASSET) {
        return strprintf("%d %s", n.GetAmountValue(), n.GetAmountName());
    }
    return FormatMoney(n.GetAmountValue());
}

// NOKUBIT: Nokubit AssetCoin
std::string FormatMoney(const asset_tag::CAmountMap& nRef)
{
    bool flag = false;
    std::string str = "";
    for (asset_tag::CAmountMap::const_iterator it = nRef.begin(); it != nRef.end(); it++) {
        if (flag) str += ", ";
        flag = true;
        str += FormatMoney(asset_tag::AssetAmount(*it));
    }
    return str;
}

bool ParseMoney(const std::string& str, CAmount& nRet)
{
    return ParseMoney(str.c_str(), nRet);
}

bool ParseMoney(const char* pszIn, CAmount& nRet)
{
    std::string strWhole;
    int64_t nUnits = 0;
    const char* p = pszIn;
    while (isspace(*p))
        p++;
    for (; *p; p++)
    {
        if (*p == '.')
        {
            p++;
            int64_t nMult = CENT*10;
            while (isdigit(*p) && (nMult > 0))
            {
                nUnits += nMult * (*p++ - '0');
                nMult /= 10;
            }
            break;
        }
        if (isspace(*p))
            break;
        if (!isdigit(*p))
            return false;
        strWhole.insert(strWhole.end(), *p);
    }
    for (; *p; p++)
        if (!isspace(*p))
            return false;
    if (strWhole.size() > 10) // guard against 63 bit overflow
        return false;
    if (nUnits < 0 || nUnits > COIN)
        return false;
    int64_t nWhole = atoi64(strWhole);
    CAmount nValue = nWhole*COIN + nUnits;

    nRet = nValue;
    return true;
}

// NOKUBIT: Nokubit AssetCoin
bool ParseMoney(const std::string& str, asset_tag::AssetAmount& nRet)
{
    asset_tag::AssetName name(asset_tag::COIN_ASSET);
    std::vector<std::string> vStrInputParts;
    boost::split(vStrInputParts, str, boost::is_any_of(" "));

    if (vStrInputParts.size() > 2)
        return false;
    if (vStrInputParts.size() == 2) {
        std::string strAsset;
        const char* p = vStrInputParts[1].c_str();
        while (isspace(*p))
            p++;
        if (!isdigit(*p))
            return false;
        for (; *p; p++)
        {
            if (isspace(*p))
                break;
            strAsset.insert(strAsset.end(), *p);
        }
        for (; *p; p++)
            if (!isspace(*p))
                return false;
        if (strAsset.size() < asset_tag::ASSET_NAME_MIN_SIZE || strAsset.size() > asset_tag::ASSET_NAME_MAX_SIZE)
            return false;
        name = strAsset;
    }

    CAmount nValue;
    if (!ParseMoney(vStrInputParts[0].c_str(), nValue))
        return false;

    nRet = asset_tag::AssetAmount(name, nValue);
    return true;
}


#include <costar_task_plan/trajectory_distribution.h>

/* KDL includes */
#include <kdl/trajectory_composite.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/velocityprofile_spline.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <kdl/path_line.hpp>
#include <kdl/path_roundedcomposite.hpp>

#include <Eigen/Dense>

#define SHOW_SAMPLED_VALUES 0
#define DEFAULT_SIGMA 0.01

using namespace KDL;
using namespace Eigen;

namespace costar {

  /**
   * Initialize a trajectory distribution with velocity profile, etc.
   */
  TrajectoryDistribution::TrajectoryDistribution(int nseg_, int k_)
    : nseg(nseg_),
    dist(nseg_*(POSE_FEATURES_SIZE + SPLINE_DIM),k_),
    verbose(false),
    diagonal_sigma(1e-5),
    def_step_size(0.80)
  {
    nvars = (POSE_FEATURES_SIZE + SPLINE_DIM) * nseg; // velocity, acceleration, position setpoints for each segment
  }

  /**
   * initialize
   * set up the distribution based on a skill and an environment
   */
  void TrajectoryDistribution::initialize(TestFeatures &features, const Skill &skill, std::vector<double> sigma) {
    Pose p0 = features.lookup(AGENT);
    Pose p1 = features.lookup(skill.getInitializationFeature()) * skill.getInitializationFinalPose();
    double eqradius = 1;

    RotationalInterpolation_SingleAxis *ri = new RotationalInterpolation_SingleAxis();
    Path_Line *path = new Path_Line(p0,p1,ri,eqradius);

    std::cout << "Path length: " << path->PathLength() << std::endl;
    for (int i = 0; i < nseg; ++i) {
      double s = ((double)(i+1)/(double)nseg) * path->PathLength();
      std::cout << "(" << i+1 << "/" << nseg << ") position = " << s << std::endl;
      Pose p = path->Pos(s);

      int idx = (POSE_FEATURES_SIZE + SPLINE_DIM) * i;

      // set up x, y, z
      dist.ns[0].mu[idx+POSE_FEATURE_X] = p.p.x();
      dist.ns[0].mu[idx+POSE_FEATURE_Y] = p.p.y();
      dist.ns[0].mu[idx+POSE_FEATURE_Z] = p.p.z();

      // set up roll, pitch, yaw
      {
#ifdef USE_ROTATION_RPY
        double roll, pitch, yaw;
        p.M.GetRPY(roll,pitch,yaw);
        dist.ns[0].mu[idx+POSE_FEATURE_YAW] = yaw;
        dist.ns[0].mu[idx+POSE_FEATURE_PITCH] = pitch;
        dist.ns[0].mu[idx+POSE_FEATURE_ROLL] = roll;
#else
        double x,y,z,w;
        p.M.GetQuaternion(x,y,z,w);
        dist.ns[0].mu[idx+POSE_FEATURE_WX] = x;
        dist.ns[0].mu[idx+POSE_FEATURE_WY] = y;
        dist.ns[0].mu[idx+POSE_FEATURE_WZ] = z;
        dist.ns[0].mu[idx+POSE_FEATURE_WW] = w;
#endif
      }

      idx += POSE_FEATURES_SIZE;
      dist.ns[0].mu[idx+SPLINE_POS1] = 0.0;
      dist.ns[0].mu[idx+SPLINE_VEL1] = 0.01;
      dist.ns[0].mu[idx+SPLINE_ACC1] = 0.01;
      dist.ns[0].mu[idx+SPLINE_POS2] = 1;
      dist.ns[0].mu[idx+SPLINE_VEL2] = 0.01;
      dist.ns[0].mu[idx+SPLINE_ACC2] = -0.01;
      dist.ns[0].mu[idx+SEGMENT_DURATION] = 1.0;
    }

    if (sigma.size() < nvars) {
      if (verbose) {
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Noise argument for trajectory search initialization was the wrong size!" << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Should be: " << nvars << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Was: " << sigma.size() << std::endl;
      }
      for (int j = 0; j < nvars; ++j) {
        dist.ns[0].P(j,j) = DEFAULT_SIGMA;
      }

    } else {
      for (int j = 0; j < nvars; ++j) {
        dist.ns[0].P(j,j) = sigma[j];
      }
    }
    dist.Update();

    //std::cout << "<<<<<<>>>>>>>" << std::endl;
    //std::cout << dist.ns[0].mu << std::endl;
    //std::cout << "<<<<<<>>>>>>>" << std::endl;

    initial = p0;
    delete path;
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void TrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps,
      double diagonal_noise)
  {
    update(params,ps,diagonal_noise,def_step_size);
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void TrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps)
  {
    update(params,ps,diagonal_sigma,def_step_size);
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void TrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps,
      double diagonal_noise,
      double step_size)
  {

    double psum = 0;
    for (double &d: ps) {
      psum += d;
    }

    if (dist.k == 1) {

      // one cluster only
      // compute mean

      dist.ns[0].mu *= (1 - step_size); //setZero();
      dist.ns[0].P *= (1 - step_size); //setZero();

      for (unsigned int i = 0; i < params.size(); ++i) {
        //std::cout << "mu rows = " << dist.ns[0].mu.rows() << ", vec rows = " << vec.rows() << std::endl;
        //std::cout << "mu cols = " << dist.ns[0].mu.cols() << ", vec cols = " << vec.cols() << std::endl;
        double wt = step_size * ps[i] / psum;
        dist.ns[0].mu += params[i] * wt;
      }

      for (unsigned int i = 0; i < params.size(); ++i) {
        double wt = step_size * ps[i] / psum;
        //std::cout << wt << ", " << ps[i] << ", " << psum << std::endl;
        dist.ns[0].P += wt * (params[i] - dist.ns[0].mu) * (params[i] - dist.ns[0].mu).transpose();
      }

    } else {

      // set up weighted data
      // and then fit GMM again

      std::vector<std::pair<EigenVectornd,double> > data(ps.size());
      for (unsigned int i = 0; i < ps.size(); ++i) {
        data[0].first = params[i];
        data[0].second = ps[i] / psum;
      }

      dist.Fit(data);

    }

    for (unsigned int i = 0; i < dist.k; ++i) {
      dist.ns[0].P += diagonal_noise * Matrix<double,Dynamic,Dynamic>::Identity(nvars,nvars);
    }

    dist.Update();
  }

  /**
   * sample
   * Pull a random trajectory from the gmm
   * Convert it into a KDL trajectory
   */
  void TrajectoryDistribution::sample(std::vector<EigenVectornd> &params, std::vector<Trajectory *> &trajs) {
    //Trajectory_Composite *traj = new Trajectory_Composite[nsamples];
    unsigned int nsamples = params.size();
    trajs.resize(nsamples);

    for (int sample = 0; sample < nsamples; ++sample) {
      Frame prev = Frame(initial);

      Trajectory_Composite *ctraj = new Trajectory_Composite();

      EigenVectornd vec;
      vec.resize(nvars);
      dist.Sample(vec);

      params[sample] = vec;

#if SHOW_SAMPLED_VALUES
      std::cout << "Sampled: ";
      for (int j = 0; j < nvars; ++j) {
        std::cout << vec[j] << " ";
      }
      std::cout << std::endl;
#endif

      int idx = 0;
      for (int i = 0; i < nseg; ++i) {

        idx = (POSE_FEATURES_SIZE+SPLINE_DIM)*i;

        double prc1 = 0.1;
        double prc2 = 0.2;
        RotationalInterpolation_SingleAxis *ri = new RotationalInterpolation_SingleAxis();
        Path_RoundedComposite *path = new Path_RoundedComposite(prc1,prc2,ri);

        // generate a random set point and add it to the path
        {

#ifdef USE_ROTATION_RPY
          Rotation r1 = Rotation::RPY(vec[idx+POSE_FEATURE_ROLL],vec[idx+POSE_FEATURE_PITCH],vec[idx+POSE_FEATURE_YAW]);
#else
          double x,y,z,w;
          x = vec[idx+POSE_FEATURE_WX];
          y = vec[idx+POSE_FEATURE_WY];
          z = vec[idx+POSE_FEATURE_WZ];
          w = vec[idx+POSE_FEATURE_WW];
          double norm = 1/sqrt((x*x) + (y*y) + (z*z) + (w*w));
          Rotation r1 = Rotation::Quaternion(norm*vec[idx+POSE_FEATURE_WX],norm*vec[idx+POSE_FEATURE_WY],norm*vec[idx+POSE_FEATURE_WZ],norm*vec[idx+POSE_FEATURE_WW]);
#endif
          Vector v1 = Vector(vec[idx+POSE_FEATURE_X],vec[idx+POSE_FEATURE_Y],vec[idx+POSE_FEATURE_Z]);

          Frame t1 = Frame(r1,v1);
          path->Add(prev);
          path->Add(t1);
          path->Finish();
          prev = t1;
        }

        // generate random parameters for velocity profile
        VelocityProfile_Spline *velprof = new VelocityProfile_Spline();
        {
          idx += POSE_FEATURES_SIZE;

          double pos1,vel1,acc1,pos2,acc2,vel2,duration;
          pos1 = 0; //vec[idx+SPLINE_POS1];
          pos2 = path->PathLength(); //vec[idx+SPLINE_POS2]*path->PathLength();
          vel1 = vec[idx+SPLINE_VEL1];
          vel2 = vec[idx+SPLINE_VEL2];
          acc1 = vec[idx+SPLINE_ACC1];
          acc2 = vec[idx+SPLINE_ACC2];
          duration = fabs(vec[idx+SEGMENT_DURATION]);

          //std::cout << "Path length: " << path->PathLength() << std::endl;
          velprof->SetProfile(0,path->PathLength());
          velprof->SetProfileDuration(pos1,vel1,acc1,pos2,vel2,acc2,duration);
          //velprof->SetProfileDuration(pos1,vel1,pos2,vel2,duration);
          //velprof->SetProfileDuration(pos1,pos2,duration);
          //velprof->Write(std::cout);
        }
        //VelocityProfile_Trap *velprof = new VelocityProfile_Trap(1,0.5);
        //velprof->SetProfile(0,path->PathLength());

        // add to the trajectory
        Trajectory_Segment *seg = new Trajectory_Segment(path, velprof);
        ctraj->Add(seg);
      }

      trajs[sample] = ctraj;
    }
  }

}


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_LOGICAL_OR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOGICAL_OR_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/function/simd/mask2logical.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/genmask.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/as_logical.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF(logical_or_
                            , (typename A0,typename A1,typename X)
                            , (detail::same_size<A0,A1>)
                            , bd::cpu_
                            , bs::pack_<bd::fundamental_<A0>,X>
                            , bs::pack_<bd::fundamental_<A1>,X>
                            )

  {
    BOOST_FORCEINLINE bs::as_logical_t<A0> operator()( const A0& a0
                                                     , const A1& a1) const BOOST_NOEXCEPT
    {
      return mask2logical(bitwise_or(genmask(a0), genmask(a1)));
    }
  };
} } }

#endif


// Copyright (c) 2008-2017 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_QVM_48D38F75E6D91D2AE5C0F72B78818744
#define BOOST_QVM_48D38F75E6D91D2AE5C0F72B78818744

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/vec_traits.hpp>

namespace boost {
namespace qvm {
template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 2 &&
                                  mat_traits<A>::cols == 2 &&
                                  vec_traits<B>::dim == 2,
                              deduce_vec2<A, B, 2>>::type
    operator*(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  typedef typename deduce_vec2<A, B, 2>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 2);
  R r;
  vec_traits<R>::template write_element<0>(r) = a00 * b0 + a01 * b1;
  vec_traits<R>::template write_element<1>(r) = a10 * b0 + a11 * b1;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_mv_defined;

template <> struct mul_mv_defined<2, 2> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<B>::rows == 2 &&
                                  mat_traits<B>::cols == 2 &&
                                  vec_traits<A>::dim == 2,
                              deduce_vec2<A, B, 2>>::type
    operator*(A const &a, B const &b) {
  typedef typename vec_traits<A>::scalar_type Ta;
  typedef typename mat_traits<B>::scalar_type Tb;
  Ta const a0 = vec_traits<A>::template read_element<0>(a);
  Ta const a1 = vec_traits<A>::template read_element<1>(a);
  Tb const b00 = mat_traits<B>::template read_element<0, 0>(b);
  Tb const b01 = mat_traits<B>::template read_element<0, 1>(b);
  Tb const b10 = mat_traits<B>::template read_element<1, 0>(b);
  Tb const b11 = mat_traits<B>::template read_element<1, 1>(b);
  typedef typename deduce_vec2<A, B, 2>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 2);
  R r;
  vec_traits<R>::template write_element<0>(r) = a0 * b00 + a1 * b10;
  vec_traits<R>::template write_element<1>(r) = a0 * b01 + a1 * b11;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_vm_defined;

template <> struct mul_vm_defined<2, 2> { static bool const value = true; };
} // namespace qvm_detail

} // namespace qvm
} // namespace boost

#endif