#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <numeric>
#include <cmath>
#include <stdexcept>
#ifdef _OPENMP
#include <omp.h>
#endif

#include "domain_partition.hpp"

#include "mba.hpp"

#include <boost/scope_exit.hpp>
#include <memory>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <boost/multi_array.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/backend/vexcl.hpp>
   typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
   typedef amgcl::backend::cuda<double> Backend;
#else
#  ifndef SOLVER_BACKEND_BUILTIN
#    define SOLVER_BACKEND_BUILTIN
#  endif
#  include <amgcl/backend/builtin.hpp>
   typedef amgcl::backend::builtin<double> Backend;
#endif

#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/mpi/direct_solver/runtime.hpp>
#include <amgcl/mpi/subdomain_deflation.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/zero_copy.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

struct partitioned_deflation {
    unsigned nparts;
    std::vector<unsigned> domain;

    partitioned_deflation(
            boost::array<ptrdiff_t, 2> LO,
            boost::array<ptrdiff_t, 2> HI,
            unsigned nparts
            ) : nparts(nparts)
    {
        domain_partition<2> part(LO, HI, nparts);

        ptrdiff_t nx = HI[0] - LO[0] + 1;
        ptrdiff_t ny = HI[1] - LO[1] + 1;

        domain.resize(nx * ny);
        for(unsigned p = 0; p < nparts; ++p) {
            boost::array<ptrdiff_t, 2> lo = part.domain(p).min_corner();
            boost::array<ptrdiff_t, 2> hi = part.domain(p).max_corner();

            for(int j = lo[1]; j <= hi[1]; ++j) {
                for(int i = lo[0]; i <= hi[0]; ++i) {
                    domain[(j - LO[1]) * nx + (i - LO[0])] = p;
                }
            }
        }
    }

    size_t dim() const { return nparts; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return domain[i] == j;
    }
};

struct linear_deflation {
    std::vector<double> x;
    std::vector<double> y;

    linear_deflation(
            ptrdiff_t chunk,
            boost::array<ptrdiff_t, 2> lo,
            boost::array<ptrdiff_t, 2> hi
            )
    {
        double hx = 1.0 / (hi[0] - lo[0]);
        double hy = 1.0 / (hi[1] - lo[1]);

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        x.reserve(chunk);
        y.reserve(chunk);

        for (ptrdiff_t j = 0; j < ny; ++j) {
            for (ptrdiff_t i = 0; i < nx; ++i) {
                x.push_back(i * hx - 0.5);
                y.push_back(j * hy - 0.5);
            }
        }
    }

    size_t dim() const { return 3; }

    double operator()(ptrdiff_t i, unsigned j) const {
        switch(j) {
            default:
            case 0:
                return 1;
            case 1:
                return x[i];
            case 2:
                return y[i];
        }
    }
};

struct bilinear_deflation {
    size_t nv, chunk;
    std::vector<double> v;

    bilinear_deflation(
            ptrdiff_t n,
            ptrdiff_t chunk,
            boost::array<ptrdiff_t, 2> lo,
            boost::array<ptrdiff_t, 2> hi
            ) : nv(0), chunk(chunk)
    {
        // See which neighbors we have.
        int neib[2][2] = {
            {lo[0] > 0 || lo[1] > 0,     hi[0] + 1 < n || lo[1] > 0    },
            {lo[0] > 0 || hi[1] + 1 < n, hi[0] + 1 < n || hi[1] + 1 < n}
        };

        for(int j = 0; j < 2; ++j)
            for(int i = 0; i < 2; ++i)
                if (neib[j][i]) ++nv;

        if (nv == 0) {
            // Single MPI process?
            nv = 1;
            v.resize(chunk, 1);
            return;
        }

        v.resize(chunk * nv, 0);

        double *dv = v.data();

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        double hx = 1.0 / (nx - 1);
        double hy = 1.0 / (ny - 1);

        for(int j = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i) {
                if (!neib[j][i]) continue;

                boost::multi_array_ref<double, 2> V(dv, boost::extents[ny][nx]);

                for(ptrdiff_t jj = 0; jj < ny; ++jj) {
                    double y = jj * hy;
                    double b = std::abs((1 - j) - y);
                    for(ptrdiff_t ii = 0; ii < nx; ++ii) {
                        double x = ii * hx;

                        double a = std::abs((1 - i) - x);
                        V[jj][ii] = a * b;
                    }
                }

                dv += chunk;
            }
        }
    }

    size_t dim() const { return nv; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return v[j * chunk + i];
    }
};

#ifndef SOLVER_BACKEND_CUDA
struct mba_deflation {
    size_t chunk, nv;
    std::vector<double> v;

    mba_deflation(
            ptrdiff_t n,
            ptrdiff_t chunk,
            boost::array<ptrdiff_t,2> lo,
            boost::array<ptrdiff_t,2> hi
            ) : chunk(chunk), nv(1)
    {
        // See which neighbors we have.
        int neib[2][2] = {
            {lo[0] > 0 || lo[1] > 0,     hi[0] + 1 < n || lo[1] > 0    },
            {lo[0] > 0 || hi[1] + 1 < n, hi[0] + 1 < n || hi[1] + 1 < n}
        };

        for(int j = 0; j < 2; ++j)
            for(int i = 0; i < 2; ++i)
                if (neib[j][i]) ++nv;

        v.resize(chunk * nv, 0);

        double *dv = v.data();
        std::fill(dv, dv + chunk, 1.0);
        dv += chunk;

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        double hx = 1.0 / (nx - 1);
        double hy = 1.0 / (ny - 1);

        std::array<double, 2> cmin = {-0.01, -0.01};
        std::array<double, 2> cmax = { 1.01,  1.01};
        std::array<size_t, 2> grid = {3, 3};

        std::array< std::array<double, 2>, 4 > coo;
        std::array< double, 4 > val;

        for(int j = 0, idx = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i, ++idx) {
                coo[idx][0] = i;
                coo[idx][1] = j;
            }
        }

        for(int j = 0, idx = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i, ++idx) {
                if (!neib[j][i]) continue;

                std::fill(val.begin(), val.end(), 0.0);
                val[idx] = 1.0;

                mba::MBA<2> interp(cmin, cmax, grid, coo, val, 8, 1e-8, 0.5, zero);

                boost::multi_array_ref<double, 2> V(dv, boost::extents[ny][nx]);

                for(int jj = 0; jj < ny; ++jj)
                    for(int ii = 0; ii < nx; ++ii) {
                        std::array<double, 2> p = {ii * hx, jj * hy};
                        V[jj][ii] = interp(p);
                    }

                dv += chunk;
            }
        }
    }

    size_t dim() const { return nv; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return v[j * chunk + i];
    }

    static double zero(const std::array<double, 2>&) {
        return 0;
    }
};
#endif

struct harmonic_deflation {
    size_t nv, chunk;
    std::vector<double> v;

    harmonic_deflation(
            ptrdiff_t n,
            ptrdiff_t chunk,
            boost::array<ptrdiff_t, 2> lo,
            boost::array<ptrdiff_t, 2> hi
            ) : nv(0), chunk(chunk)
    {
        // See which neighbors we have.
        int neib[2][2] = {
            {lo[0] > 0 || lo[1] > 0,     hi[0] + 1 < n || lo[1] > 0    },
            {lo[0] > 0 || hi[1] + 1 < n, hi[0] + 1 < n || hi[1] + 1 < n}
        };

        for(int j = 0; j < 2; ++j)
            for(int i = 0; i < 2; ++i)
                if (neib[j][i]) ++nv;

        if (nv == 0) {
            // Single MPI process?
            nv = 1;
            v.resize(chunk, 1);
            return;
        }

        v.resize(chunk * nv, 0);
        double *dv = v.data();

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        std::vector<ptrdiff_t> ptr;
        std::vector<ptrdiff_t> col;
        std::vector<double>    val;
        std::vector<double>    rhs(chunk, 0.0);

        ptr.reserve(chunk + 1);
        col.reserve(chunk * 5);
        val.reserve(chunk * 5);

        ptr.push_back(0);

        for(int j = 0, k = 0; j < ny; ++j) {
            for(int i = 0; i < nx; ++i, ++k) {
                if (
                        (i == 0    && j == 0   ) ||
                        (i == 0    && j == ny-1) ||
                        (i == nx-1 && j == 0   ) ||
                        (i == nx-1 && j == ny-1)
                   )
                {
                    col.push_back(k);
                    val.push_back(1);
                } else {
                    col.push_back(k);
                    val.push_back(1.0);

                    if (j == 0) {
                        col.push_back(k + nx);
                        val.push_back(-0.5);
                    } else if (j == ny-1) {
                        col.push_back(k - nx);
                        val.push_back(-0.5);
                    } else {
                        col.push_back(k - nx);
                        val.push_back(-0.25);

                        col.push_back(k + nx);
                        val.push_back(-0.25);
                    }

                    if (i == 0) {
                        col.push_back(k + 1);
                        val.push_back(-0.5);
                    } else if (i == nx-1) {
                        col.push_back(k - 1);
                        val.push_back(-0.5);
                    } else {
                        col.push_back(k - 1);
                        val.push_back(-0.25);

                        col.push_back(k + 1);
                        val.push_back(-0.25);
                    }
                }

                ptr.push_back(col.size());
            }
        }

        amgcl::make_solver<
            amgcl::amg<
                amgcl::backend::builtin<double>,
                amgcl::coarsening::smoothed_aggregation,
                amgcl::relaxation::gauss_seidel
                >,
            amgcl::solver::gmres<
                amgcl::backend::builtin<double>
                >
            > solve( amgcl::adapter::zero_copy(chunk, ptr.data(), col.data(), val.data()) );

        for(int j = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i) {
                if (!neib[j][i]) continue;

                ptrdiff_t idx = i * (nx - 1) + j * (ny - 1) * nx;
                rhs[idx] = 1.0;

                boost::iterator_range<double*> x(dv, dv + chunk);
                solve(rhs, x);

                rhs[idx] = 0.0;

                dv += chunk;
            }
        }
    }

    size_t dim() const { return nv; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return v[j * chunk + i];
    }
};

struct renumbering {
    const domain_partition<2> ∂
    const std::vector<ptrdiff_t> &dom;

    renumbering(
            const domain_partition<2> &p,
            const std::vector<ptrdiff_t> &d
            ) : part(p), dom(d)
    {}

    ptrdiff_t operator()(ptrdiff_t i, ptrdiff_t j) const {
        boost::array<ptrdiff_t, 2> p = {{i, j}};
        std::pair<int,ptrdiff_t> v = part.index(p);
        return dom[v.first] + v.second;
    }
};

int main(int argc, char *argv[]) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    amgcl::mpi::communicator world(MPI_COMM_WORLD);

    if (world.rank == 0)
        std::cout << "World size: " << world.size << std::endl;

    // Read configuration from command line
    ptrdiff_t n = 1024;
    std::string deflation_type = "bilinear";

    amgcl::runtime::coarsening::type  coarsening       = amgcl::runtime::coarsening::smoothed_aggregation;
    amgcl::runtime::relaxation::type  relaxation       = amgcl::runtime::relaxation::spai0;
    amgcl::runtime::solver::type      iterative_solver = amgcl::runtime::solver::bicgstabl;
    amgcl::runtime::mpi::direct::type direct_solver    = amgcl::runtime::mpi::direct::skyline_lu;

    bool just_relax = false;
    bool symm_dirichlet = true;
    std::string problem = "laplace2d";
    std::string parameter_file;
    std::string out_file;

    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "problem",
         po::value<std::string>(&problem)->default_value(problem),
         "laplace2d, recirc2d"
        )
        (
         "symbc",
         po::value<bool>(&symm_dirichlet)->default_value(symm_dirichlet),
         "Use symmetric Dirichlet conditions in laplace2d"
        )
        (
         "size,n",
         po::value<ptrdiff_t>(&n)->default_value(n),
         "domain size"
        )
        (
         "coarsening,c",
         po::value<amgcl::runtime::coarsening::type>(&coarsening)->default_value(coarsening),
         "ruge_stuben, aggregation, smoothed_aggregation, smoothed_aggr_emin"
        )
        (
         "relaxation,r",
         po::value<amgcl::runtime::relaxation::type>(&relaxation)->default_value(relaxation),
         "gauss_seidel, ilu0, iluk, ilut, damped_jacobi, spai0, spai1, chebyshev"
        )
        (
         "iter_solver,i",
         po::value<amgcl::runtime::solver::type>(&iterative_solver)->default_value(iterative_solver),
         "cg, bicgstab, bicgstabl, gmres"
        )
        (
         "dir_solver,d",
         po::value<amgcl::runtime::mpi::direct::type>(&direct_solver)->default_value(direct_solver),
         "skyline_lu"
#ifdef AMGCL_HAVE_PASTIX
         ", pastix"
#endif
        )
        (
         "deflation,v",
         po::value<std::string>(&deflation_type)->default_value(deflation_type),
         "constant, partitioned, linear, bilinear, mba, harmonic"
        )
        (
         "subparts",
         po::value<int>()->default_value(16),
         "number of partitions for partitioned deflation"
        )
        (
         "params,P",
         po::value<std::string>(&parameter_file),
         "parameter file in json format"
        )
        (
         "prm,p",
         po::value< std::vector<std::string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        (
         "just-relax,0",
         po::bool_switch(&just_relax),
         "Do not create AMG hierarchy, use relaxation as preconditioner"
        )
        (
         "out,o",
         po::value<std::string>(&out_file),
         "out file"
        )
        ;

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

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

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(parameter_file, prm);

    if (vm.count("prm")) {
        for(const std::string &v : vm["prm"].as< std::vector<std::string> >()) {
            amgcl::put(prm, v);
        }
    }

    prm.put("isolver.type", iterative_solver);
    prm.put("dsolver.type", direct_solver);

    const ptrdiff_t n2 = n * n;
    const double hinv  = (n - 1);
    const double h2i   = (n - 1) * (n - 1);
    const double h     = 1 / hinv;

    boost::array<ptrdiff_t, 2> lo = { {0, 0} };
    boost::array<ptrdiff_t, 2> hi = { {n - 1, n - 1} };

    using amgcl::prof;

    prof.tic("partition");
    domain_partition<2> part(lo, hi, world.size);
    ptrdiff_t chunk = part.size( world.rank );

    std::vector<ptrdiff_t> domain(world.size + 1);
    MPI_Allgather(
            &chunk, 1, amgcl::mpi::datatype<ptrdiff_t>(),
            &domain[1], 1, amgcl::mpi::datatype<ptrdiff_t>(), world);
    std::partial_sum(domain.begin(), domain.end(), domain.begin());

    lo = part.domain(world.rank).min_corner();
    hi = part.domain(world.rank).max_corner();
    prof.toc("partition");

    renumbering renum(part, domain);

    prof.tic("deflation");
    std::function<double(ptrdiff_t,unsigned)> dv;
    unsigned ndv = 1;

    if (deflation_type == "constant") {
        dv = amgcl::mpi::constant_deflation(1);
    } else if (deflation_type == "partitioned") {
        ndv = vm["subparts"].as<int>();
        dv  = partitioned_deflation(lo, hi, ndv);
    } else if (deflation_type == "linear") {
        ndv = 3;
        dv  = linear_deflation(chunk, lo, hi);
    } else if (deflation_type == "bilinear") {
        bilinear_deflation bld(n, chunk, lo, hi);
        ndv = bld.dim();
        dv  = bld;
#ifndef SOLVER_BACKEND_CUDA
    } else if (deflation_type == "mba") {
        mba_deflation mba(n, chunk, lo, hi);
        ndv = mba.dim();
        dv  = mba;
#endif
    } else if (deflation_type == "harmonic") {
        harmonic_deflation hd(n, chunk, lo, hi);
        ndv = hd.dim();
        dv  = hd;
    } else {
        throw std::runtime_error("Unsupported deflation type");
    }

    prm.put("num_def_vec", ndv);
    prm.put("def_vec", &dv);
    prof.toc("deflation");

    prof.tic("assemble");
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    ptr.reserve(chunk + 1);
    col.reserve(chunk * 5);
    val.reserve(chunk * 5);
    rhs.reserve(chunk);

    ptr.push_back(0);

    if (problem == "recirc2d") {
        const double eps  = 1e-5;

        for(ptrdiff_t j = lo[1]; j <= hi[1]; ++j) {
            double y = h * j;
            for(ptrdiff_t i = lo[0]; i <= hi[0]; ++i) {
                double x = h * i;

                if (i == 0 || j == 0 || i + 1 == n || j + 1 == n) {
                    col.push_back(renum(i,j));
                    val.push_back(1);
                    rhs.push_back(
                            sin(M_PI * x) + sin(M_PI * y) +
                            sin(13 * M_PI * x) + sin(13 * M_PI * y)
                            );
                } else {
                    double a = -sin(M_PI * x) * cos(M_PI * y) * hinv;
                    double b =  sin(M_PI * y) * cos(M_PI * x) * hinv;

                    if (j > 0) {
                        col.push_back(renum(i,j-1));
                        val.push_back(-eps * h2i - std::max(b, 0.0));
                    }

                    if (i > 0) {
                        col.push_back(renum(i-1,j));
                        val.push_back(-eps * h2i - std::max(a, 0.0));
                    }

                    col.push_back(renum(i,j));
                    val.push_back(4 * eps * h2i + fabs(a) + fabs(b));

                    if (i + 1 < n) {
                        col.push_back(renum(i+1,j));
                        val.push_back(-eps * h2i + std::min(a, 0.0));
                    }

                    if (j + 1 < n) {
                        col.push_back(renum(i,j+1));
                        val.push_back(-eps * h2i + std::min(b, 0.0));
                    }

                    rhs.push_back(1.0);
                }
                ptr.push_back( col.size() );
            }
        }
    } else {
        for(ptrdiff_t j = lo[1]; j <= hi[1]; ++j) {
            for(ptrdiff_t i = lo[0]; i <= hi[0]; ++i) {
                if (!symm_dirichlet && (i == 0 || j == 0 || i + 1 == n || j + 1 == n)) {
                    col.push_back(renum(i,j));
                    val.push_back(1);
                    rhs.push_back(0);
                } else {
                    if (j > 0)  {
                        col.push_back(renum(i,j-1));
                        val.push_back(-h2i);
                    }

                    if (i > 0) {
                        col.push_back(renum(i-1,j));
                        val.push_back(-h2i);
                    }

                    col.push_back(renum(i,j));
                    val.push_back(4 * h2i);

                    if (i + 1 < n) {
                        col.push_back(renum(i+1,j));
                        val.push_back(-h2i);
                    }

                    if (j + 1 < n) {
                        col.push_back(renum(i,j+1));
                        val.push_back(-h2i);
                    }

                    rhs.push_back(1);
                }
                ptr.push_back( col.size() );
            }
        }
    }
    prof.toc("assemble");

    Backend::params bprm;

#if defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    std::cout << ctx << std::endl;
    bprm.q = ctx;
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
#endif

    auto f = Backend::copy_vector(rhs, bprm);
    auto x = Backend::create_vector(chunk, bprm);

    amgcl::backend::clear(*x);

    size_t iters;
    double resid, tm_setup, tm_solve;

    if (just_relax) {
        prm.put("local.class", "relaxation");
        prm.put("local.type", relaxation);
    } else {
        prm.put("local.coarsening.type", coarsening);
        prm.put("local.relax.type", relaxation);
    }

    prof.tic("setup");
    typedef
        amgcl::mpi::subdomain_deflation<
            amgcl::runtime::preconditioner< Backend >,
            amgcl::runtime::solver::wrapper,
            amgcl::runtime::mpi::direct::solver<double>
        > SDD;

    SDD solve(world, std::tie(chunk, ptr, col, val), prm, bprm);
    tm_setup = prof.toc("setup");

    prof.tic("solve");
    std::tie(iters, resid) = solve(*f, *x);
    tm_solve = prof.toc("solve");

    if (world.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << resid << std::endl
            << prof << std::endl;

#ifdef _OPENMP
        int nt = omp_get_max_threads();
#else
        int nt = 1;
#endif
        std::ostringstream log_name;
        log_name << "log_" << n2 << "_" << nt << "_" << world.size << ".txt";
        std::ofstream log(log_name.str().c_str(), std::ios::app);
        log << n2 << "\t" << nt << "\t" << world.size
            << "\t" << tm_setup << "\t" << tm_solve
            << "\t" << iters << "\t" << std::endl;
    }

    if (!out_file.empty()) {
        std::vector<double> X(world.rank == 0 ? n2 : chunk);

#if defined(SOLVER_BACKEND_VEXCL)
        vex::copy(x->begin(), x->end(), X.begin());
#elif defined(SOLVER_BACKEND_CUDA)
        thrust::copy(x->begin(), x->end(), X.begin());
#else
        std::copy(x->data(), x->data() + chunk, X.begin());
#endif

        if (world.rank == 0) {
            for(int i = 1; i < world.size; ++i)
                MPI_Recv(&X[domain[i]], domain[i+1] - domain[i], MPI_DOUBLE, i, 42, MPI_COMM_WORLD, MPI_STATUS_IGNORE);

            std::ofstream f(out_file.c_str(), std::ios::binary);
            int m = n;
            f.write((char*)&m, sizeof(int));
            for(int j = 0; j < n; ++j) {
                for(int i = 0; i < n; ++i) {
                    double buf = X[renum(i,j)];
                    f.write((char*)&buf, sizeof(double));
                }
            }
        } else {
            MPI_Send(X.data(), chunk, MPI_DOUBLE, 0, 42, MPI_COMM_WORLD);
        }
    }
}


#include "video/preserve_aspect_ratio_scaler.hpp"

#include <bits/exception.h>
#include <spdlog/fmt/bundled/format.h>
#include <spdlog/spdlog.h>

#include <algorithm>
#include <array>
#include <cstdint>
#include <stdexcept>

#include <boost/cstdint.hpp>
#include <boost/exception/exception.hpp>
#include <boost/rational.hpp>

#include "video/yuv.hpp"

namespace hisui::video {

PreserveAspectRatioScaler::PreserveAspectRatioScaler(
    const std::uint32_t t_width,
    const std::uint32_t t_height,
    const libyuv::FilterMode t_filter_mode)
    : Scaler(t_width, t_height), m_filter_mode(t_filter_mode) {
  m_intermediate = std::make_shared<YUVImage>(m_width, m_height);
}

const std::shared_ptr<YUVImage> PreserveAspectRatioScaler::scale(
    const std::shared_ptr<YUVImage> src) {
  const auto src_width = src->getWidth(0);
  const auto src_height = src->getHeight(0);

  if (src_width == m_width && src_height == m_height) {
    return src;
  }

  boost::rational<std::uint32_t> width_ratio(m_width, src_width);
  boost::rational<std::uint32_t> height_ratio(m_height, src_height);

  if (width_ratio == height_ratio) {
    return simpleScale(src);
  }

  if (width_ratio > height_ratio) {
    return marginInWidthScale(src, src_width, height_ratio);
  }
  return marginInHeightScale(src, src_height, width_ratio);
}  // namespace hisui::video

const std::shared_ptr<YUVImage> PreserveAspectRatioScaler::marginInHeightScale(
    const std::shared_ptr<YUVImage> src,
    const std::uint32_t src_height,
    const boost::rational<std::uint32_t>& width_ratio) {
  const std::uint32_t intermediate_height =
      ((boost::rational_cast<std::uint32_t>(width_ratio * src_height) + 3) >> 2)
      << 2;
  if (intermediate_height >= m_height) {
    return simpleScale(src);
  }

  spdlog::trace("marginInHeightScale: {}x{}", m_width, intermediate_height);

  m_intermediate->setWidthAndHeight(m_width, intermediate_height);

  const int ret = libyuv::I420Scale(
      src->yuv[0], static_cast<int>(src->getWidth(0)), src->yuv[1],
      static_cast<int>(src->getWidth(1)), src->yuv[2],
      static_cast<int>(src->getWidth(2)), static_cast<int>(src->getWidth(0)),
      static_cast<int>(src->getHeight(0)), m_intermediate->yuv[0],
      static_cast<int>(m_intermediate->getWidth(0)), m_intermediate->yuv[1],
      static_cast<int>(m_intermediate->getWidth(1)), m_intermediate->yuv[2],
      static_cast<int>(m_intermediate->getWidth(2)),
      static_cast<int>(m_intermediate->getWidth(0)),
      static_cast<int>(m_intermediate->getHeight(0)), m_filter_mode);

  if (ret != 0) {
    throw std::runtime_error(
        fmt::format("I420Scale() failed: error_code={}", ret));
  }

  m_scaled->setBlack();

  std::copy_n(m_intermediate->yuv[0],
              m_intermediate->getWidth(0) * m_intermediate->getHeight(0),
              m_scaled->yuv[0] + ((m_height - intermediate_height) >> 1) *
                                     m_intermediate->getWidth(0));

  std::copy_n(m_intermediate->yuv[1],
              m_intermediate->getWidth(1) * m_intermediate->getHeight(1),
              m_scaled->yuv[1] + ((m_height - intermediate_height) >> 2) *
                                     m_intermediate->getWidth(1));

  std::copy_n(m_intermediate->yuv[2],
              m_intermediate->getWidth(2) * m_intermediate->getHeight(2),
              m_scaled->yuv[2] + ((m_height - intermediate_height) >> 2) *
                                     m_intermediate->getWidth(2));
  return m_scaled;
}

const std::shared_ptr<YUVImage> PreserveAspectRatioScaler::marginInWidthScale(
    const std::shared_ptr<YUVImage> src,
    const std::uint32_t src_width,
    const boost::rational<std::uint32_t>& height_ratio) {
  const std::uint32_t intermediate_width =
      ((boost::rational_cast<std::uint32_t>(height_ratio * src_width) + 3) >> 2)
      << 2;
  if (intermediate_width >= m_width) {
    return simpleScale(src);
  }

  spdlog::trace("marginInWidthScale: {}x{}", intermediate_width, m_height);

  m_intermediate->setWidthAndHeight(intermediate_width, m_height);

  const int ret = libyuv::I420Scale(
      src->yuv[0], static_cast<int>(src->getWidth(0)), src->yuv[1],
      static_cast<int>(src->getWidth(1)), src->yuv[2],
      static_cast<int>(src->getWidth(2)), static_cast<int>(src->getWidth(0)),
      static_cast<int>(src->getHeight(0)), m_intermediate->yuv[0],
      static_cast<int>(m_intermediate->getWidth(0)), m_intermediate->yuv[1],
      static_cast<int>(m_intermediate->getWidth(1)), m_intermediate->yuv[2],
      static_cast<int>(m_intermediate->getWidth(2)),
      static_cast<int>(m_intermediate->getWidth(0)),
      static_cast<int>(m_intermediate->getHeight(0)), m_filter_mode);

  if (ret != 0) {
    throw std::runtime_error(
        fmt::format("I420Scale() failed: error_code={}", ret));
  }

  m_scaled->setBlack();

  for (std::uint32_t h = 0, m = m_intermediate->getHeight(0); h < m; ++h) {
    std::copy_n(
        m_intermediate->yuv[0] + h * intermediate_width, intermediate_width,
        m_scaled->yuv[0] + m_width * h + ((m_width - intermediate_width) >> 1));
  }

  const auto m_width2 = m_scaled->getWidth(1);
  const auto intermediate_width2 = intermediate_width >> 1;
  for (std::uint32_t h = 0, m = m_intermediate->getHeight(1); h < m; ++h) {
    std::copy_n(m_intermediate->yuv[1] + h * intermediate_width2,
                intermediate_width2,
                m_scaled->yuv[1] + m_width2 * h +
                    ((m_width2 - intermediate_width2) >> 1));
  }

  for (std::uint32_t h = 0, m = m_intermediate->getHeight(2); h < m; ++h) {
    std::copy_n(m_intermediate->yuv[2] + h * intermediate_width2,
                intermediate_width2,
                m_scaled->yuv[2] + m_width2 * h +
                    ((m_width2 - intermediate_width2) >> 1));
  }

  return m_scaled;
}

const std::shared_ptr<YUVImage> PreserveAspectRatioScaler::simpleScale(
    const std::shared_ptr<YUVImage> src) {
  const int ret = libyuv::I420Scale(
      src->yuv[0], static_cast<int>(src->getWidth(0)), src->yuv[1],
      static_cast<int>(src->getWidth(1)), src->yuv[2],
      static_cast<int>(src->getWidth(2)), static_cast<int>(src->getWidth(0)),
      static_cast<int>(src->getHeight(0)), m_scaled->yuv[0],
      static_cast<int>(m_scaled->getWidth(0)), m_scaled->yuv[1],
      static_cast<int>(m_scaled->getWidth(1)), m_scaled->yuv[2],
      static_cast<int>(m_scaled->getWidth(2)),
      static_cast<int>(m_scaled->getWidth(0)),
      static_cast<int>(m_scaled->getHeight(0)), m_filter_mode);

  if (ret != 0) {
    throw std::runtime_error(
        fmt::format("I420Scale() failed: error_code={}", ret));
  }
  return m_scaled;
}

}  // namespace hisui::video


/*
  Copyright (c) 2018-2019 Alexander A. Ganin. All rights reserved.
  Twitter: @alxga. Website: alexganin.com.
  Licensed under the MIT License.
  See LICENSE file in the project root for full license information.
*/

#include "stdafx.h"

#include "Utils/distr.h"

#include "Utils/except.h"

#ifdef HAVE_BOOST

#include <boost/version.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/binomial_distribution.hpp>
struct BoostBinDistrInternal
{
  boost::mt19937 m_eng;
  boost::random::binomial_distribution<> m_dist;
  boost::variate_generator<boost::mt19937&, boost::random::binomial_distribution<>> m_var;

  BoostBinDistrInternal(int n, double p) :
    m_eng(((unsigned int) time(NULL)) * rand()),
    m_dist(n, p),
    m_var(m_eng, m_dist)
  {
  }
};

#else

struct BoostBinDistrInternal
{
  BoostBinDistrInternal(int n, double p)
  {
    throw Exception("Rebuild the Utils library with HAVE_BOOST macro "
                    "defined to use BoostBinDistr");
  }

  inline int m_var() const
  {
    throw Exception("Rebuild the Utils library with HAVE_BOOST macro "
                    "defined to use BoostBinDistr");
  }
};

#endif

BoostBinDistr::BoostBinDistr(int n, double p)
{
  m_internal = new BoostBinDistrInternal(n, p);
}

BoostBinDistr::~BoostBinDistr()
{
  delete m_internal;
}

int BoostBinDistr::genInt()
{
  return m_internal->m_var();
}


#include <string>
#include <vector>
#include <fstream>
#include <stdexcept>
#include <boost/progress.hpp>
#include <boost/program_options.hpp>
#include <kgraph.h>
#include <kgraph-data.h>
#include <fmt/format.h>

using std::cerr;
using std::cout;
using std::endl;
using std::string;
using std::vector;
using std::ifstream;

namespace kgraph {
    namespace metric {
        /// L2 square distance.
        struct chi2 {
            template <typename T>
            /// L2 square distance.
            static float apply (T const *t1, T const *t2, unsigned dim) {
                float r = 0;
                for (unsigned i = 0; i < dim; ++i) {
                    float v = float(t1[i]) - float(t2[i]);
                    float b = float(t1[i]) + float(t2[i]);
                    if (b > 0) {
                        r += v * v / b;
                    }
                }
                return r;
            }
        };

        struct dot {
            template <typename T>
            /// L2 square distance.
            static float apply (T const *t1, T const *t2, unsigned dim) {
                float r = 0;
                float l1 = 0, l2 = 0;
                for (unsigned i = 0; i < dim; ++i) {
                    r += t1[i] * t2[i];
                    l1 += t1[i] * t1[i];
                    l2 += t2[i] * t2[i];
                }
                return -r/sqrt(l1 * l2);
            }
        };
    }
}

int main (int argc, char *argv[]) {
    string list;
    string matrix;
    string metric;
    unsigned N, K;

    namespace po = boost::program_options;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message.")
        ("list", po::value(&list)->default_value("list"), "")
        ("matrix", po::value(&matrix), "")
        (",N", po::value(&N)->default_value(100), "")
        (",K", po::value(&K)->default_value(10), "")
        ("metric,m", po::value(&metric)->default_value("chi2"), "")
        ;

    po::positional_options_description p;
    p.add("matrix", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).
                     options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help") || (vm.count("matrix") == 0)) {
        cout << "Usage:" << endl;
        cout << desc;
        cout << endl;
        return 0;
    }

    vector<string> paths;
    {
        ifstream is(list.c_str());
        string path;
        while (is >> path) {
            paths.push_back(path);
        }
    }
    vector<unsigned> idx(paths.size());
    for (unsigned i = 0; i < idx.size(); ++i) idx[i] = i;
    std::random_shuffle(idx.begin(), idx.end());
    idx.resize(N);

    kgraph::Matrix<float> data;
    data.load_lshkit(matrix);
    if (data.size() != paths.size()) {
        cerr << "size not match" << endl;
        throw 0;
    }
    kgraph::MatrixOracle<float, kgraph::metric::l2sqr> l2oracle(data);
    kgraph::MatrixOracle<float, kgraph::metric::chi2> chi2oracle(data);
    kgraph::MatrixOracle<float, kgraph::metric::dot> dotoracle(data);
    kgraph::KGraph *index = kgraph::KGraph::create();
    kgraph::KGraph::IndexParams params;
    if (metric == "l2") {
        index->build(l2oracle, params, NULL);
    }
    else if (metric == "chi2") {
        index->build(chi2oracle, params, NULL);
    }
    else if (metric == "dot") {
        index->build(dotoracle, params, NULL);
    }
    else {
        throw 0;
    }
    cout << "
";
    vector<unsigned> nns(params.L);
    vector<float> dists(params.L);
    for (unsigned i: idx) {
        unsigned M, L;
        index->get_nn(i, &nns[0], &dists[0], &M, &L);
        cout << "
<img src='" << paths[i] << "'/>";
        for (unsigned j = 0; j < K; ++j) {
            cout << "<img src='" << paths[nns[j]] << "'/>
" << dists[j] << "";
        }
        cout << "
" << endl;
    }
    cout << "
" << endl;
    delete index;
}


#include "haptic_icon.hpp"
#include "ImgProcUtil.hpp"
#include "StereoTracker.hpp"
#include "CameraDevice.hpp"
#include "odcs.hpp"
#include "additionalGain.hpp"
#include "autd3.hpp"
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>
#define NOMINMAX
#include <Windows.h>

#pragma comment(lib, "winmm")

using namespace haptic_icon;

int main(int argc, char** argv) {
	//user-defined parameters
	Eigen::Vector3f posTgt(0, 0, 0);
	std::string record_name("hapt_log.csv");
	std::string target_image_name("target.png");
	std::string leftCamId("32434751");
	std::string rightCamId("43435351");
	Eigen::Vector3f pos_sensor(0.0f, 0.0f, 0.0f);
	Eigen::Quaternionf quo_sensor(-7.98142e-05f, -0.0105203f, -0.0108641f, 0.707026f);
	int lowerb = 100, upperb = 255, hist_size = 30;
	std::cout << "loading image ..." << std::endl;
	cv::Mat img_target = cv::imread(target_image_name);
	if (img_target.empty()) {
		std::cerr << "Failed to open the target image." << std::endl;
	}
	std::cout << "initializing extractor ..." << std::endl;
	//initialize tracking algorithm
	std::vector<cv::Mat> imgs_target = { img_target };
	auto extractorPtr = imgProc::hue_backproject_extractor::create(imgs_target, lowerb, upperb, hist_size);

	//initialize camera
	std::cout << "initializing camera ..." << std::endl;
	auto leftCamPtr = ximeaCameraDevice::create(leftCamId);
	auto rightCamPtr = ximeaCameraDevice::create(rightCamId);
	auto stereoCamPtr = dynaman::stereoCamera::create(leftCamPtr, rightCamPtr);
	std::cout << "opening camera..." << std::endl;
	stereoCamPtr->open();
	std::cout << "initializing tracker..." << std::endl;
	dynaman::stereoTracker tracker(stereoCamPtr, extractorPtr, pos_sensor, quo_sensor);

	//top autd
	const float theta0 = 0.f, phi0 = -3 * pi / 4; bool isUpper0 = true;
	//upper autd
	const float theta1 = pi / 4, phi1 = pi / 4; bool isUpper1 = true;
	const float theta2 = pi / 4, phi2 = 3 * pi / 4; bool isUpper2 = true;
	const float theta3 = pi / 4, phi3 = - 3 * pi / 4; bool isUpper3 = true;
	const float theta4 = pi / 4, phi4 = - pi / 4; bool isUpper4 = true;
	//middle autd
	const float theta5 = pi / 2, phi5 = pi / 2; bool isUpper5 = true;
	const float theta6 = pi / 2, phi6 = 3 * pi / 2; bool isUpper6 = true;
	//lower autd
	const float theta7 = 3 * pi / 4, phi7 = pi / 4; bool isUpper7 = false;
	const float theta8 = 3 * pi / 4, phi8 = 3 * pi / 4; bool isUpper8 = false;
	const float theta9 = 3 * pi / 4, phi9 = -3 * pi / 4; bool isUpper9 = false;
	const float theta10 = 3 * pi / 4, phi10 = - pi / 4; bool isUpper10 = false;

	dynaman::odcs manipulator(tracker);
	manipulator.Initialize();
	manipulator.AddDevice(Polar2Position(theta9, phi9, isUpper9), Polar2Euler(theta9, phi9, isUpper9));
	manipulator.AddDevice(Eigen::Vector3f(-528.f, 10.16f * 6.5f, -10.16f * 8.5f), Eigen::Vector3f(0, pi / 2, pi));
	manipulator.AddDevice(Polar2Position(theta8, phi8, isUpper8), Polar2Euler(theta8, phi8, isUpper8));
	manipulator.AddDevice(Polar2Position(theta7, phi7, isUpper7), Polar2Euler(theta7, phi7, isUpper7));
	manipulator.AddDevice(Eigen::Vector3f(528.f, -10.16f * 6.5f, -10.16f * 8.5f), Eigen::Vector3f(0, -pi / 2, 0));
	manipulator.AddDevice(Polar2Position(theta10, phi10, isUpper10), Polar2Euler(theta10, phi10, isUpper10));
	manipulator.AddDevice(Polar2Position(theta4, phi4, isUpper4), Polar2Euler(theta4, phi4, isUpper4));
	manipulator.AddDevice(Polar2Position(theta1, phi1, isUpper1), Polar2Euler(theta1, phi1, isUpper1));
	manipulator.AddDevice(Polar2Position(theta2, phi2, isUpper2), Polar2Euler(theta2, phi2, isUpper2));
	manipulator.AddDevice(Polar2Position(theta3, phi3, isUpper3), Polar2Euler(theta3, phi3, isUpper3));
	manipulator.AddDevice(Polar2Position(theta0, phi0, isUpper0), Polar2Euler(theta0, phi0, isUpper0));

	dynaman::FloatingObjectPtr objPtr = dynaman::FloatingObject::Create(posTgt, Eigen::Vector3f(-300, -300, -300), Eigen::Vector3f(300, 300, 300), 1.0e-5, 50);
	manipulator.RegisterObject(objPtr);
	std::cout << "opening manipulator ..." << std::endl;
	manipulator.StartControl();
	auto timeInit = timeGetTime();

	std::ofstream ofs(record_name);
	ofs << "t, x, y, z, vx, vy, vz, xTgt, yTgt, zTgt, vxTgt, vyTgt, vzTgt, axTgt, ayTgt, azTgt" << std::endl;
	while (timeGetTime() - timeInit < 30000) {
		auto currentTime = timeGetTime();
		auto pos = objPtr->getPosition();
		auto vel = objPtr->getVelocity();
		auto posTgt = objPtr->getPositionTarget();
		auto velTgt = objPtr->getVelocityTarget();
		auto accelTgt = objPtr->getAccelTarget();
		ofs << currentTime << ", " << pos.x() << ", " << pos.y() << ", " << pos.z() << ", "
			<< vel.x() << ", " << vel.y() << ", " << vel.z() << ", "
			<< posTgt.x() << ", " << posTgt.y() << ", " << posTgt.z() << ", "
			<< velTgt.x() << ", " << velTgt.y() << ", " << velTgt.z() << ", "
			<< accelTgt.x() << ", " << accelTgt.y() << ", " << accelTgt.z() << std::endl;
		Sleep(33);
	}
	std::cout << "Press any key to close " << std::endl;
	getchar();
	ofs.close();
	manipulator.Close();

	return 0;
}


#ifndef _STDEX_VALUE_TRAITS_HPP_INCLUDED_
#define _STDEX_VALUE_TRAITS_HPP_INCLUDED_

/******************************************************************************
*
*   Copyright (c) 2019 AT&T Intellectual Property.
*   Copyright (c) 2018-2019 Nokia.
*
*   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 <boost/utility.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/less_equal.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/and.hpp>

#include "type_defs.h"

namespace stdex {
namespace value {

namespace mpl = boost::mpl;

//NOTE! length is in bits
typedef mpl::int_<8>	_8;
typedef mpl::int_<16>	_16;
typedef mpl::int_<24>	_24;
typedef mpl::int_<32>	_32;
typedef mpl::int_<40>	_40;
typedef mpl::int_<48>	_48;
typedef mpl::int_<56>	_56;
typedef mpl::int_<64>	_64;

template<int num_bits>
struct bits_to_bytes : mpl::int_< (num_bits + 7) / 8 > {};

/******************************************************************************
* Class:		value::traits<LEN>
* Description:	select min integer type to fit LEN bits
* Notes:		LEN is positive integer type (mpl::int_) = number of bits
******************************************************************************/
template <class LEN, typename Enabler = void> struct traits;

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::less_equal<LEN, _8>
	>::type
>
{
	typedef _8            value_length;
	typedef u8            value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::and_<mpl::greater<LEN, _8>, mpl::less_equal<LEN, _16> >
	>::type
>
{
	typedef _16           value_length;
	typedef u16           value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::and_<mpl::greater<LEN, _16>, mpl::less_equal<LEN, _24> >
	>::type
>
{
	typedef _24           value_length;
	typedef u32           value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::and_<mpl::greater<LEN, _24>, mpl::less_equal<LEN, _32> >
	>::type
>
{
	typedef _32           value_length;
	typedef u32           value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::and_<mpl::greater<LEN, _32>, mpl::less_equal<LEN, _40> >
	>::type
>
{
	typedef _40           value_length;
	typedef u64           value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::and_<mpl::greater<LEN, _40>, mpl::less_equal<LEN, _48> >
	>::type
>
{
	typedef _48           value_length;
	typedef u64           value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::and_<mpl::greater<LEN, _48>, mpl::less_equal<LEN, _56> >
	>::type
>
{
	typedef _56           value_length;
	typedef u64           value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::and_<mpl::greater<LEN, _56>, mpl::less_equal<LEN, _64> >
	>::type
>
{
	typedef _64           value_length;
	typedef u64           value_type;
	typedef value_type    param_type;
};

template <class LEN>
struct traits<LEN, typename boost::enable_if<
		mpl::greater<LEN, _64>
	>::type
>
{
	typedef LEN                         value_length;
	struct value_type {u8 value[bits_to_bytes<LEN::value>::value];};
	typedef value_type const&           param_type;
};

//template <class LEN>
//struct traits<LEN,
//	typename boost::enable_if<
//		mpl::greater<LEN, _32>
//	>::type
//>
//{
//	typedef LEN					value_length;
//	struct value_type {unsigned char value[LEN::value/8];};
//	typedef value_type const&	param_type;
//
//};

/******************************************************************************
* Class:		value::traits_c<BITS>
* Description:	select min integer type to fit BITS bits
* Notes:		N is positive integer value = number of bits
******************************************************************************/
template <size_t BITS>
struct traits_c : traits<boost::mpl::int_<BITS> >
{
};

}	//end:	namespace value

}	//end:	namespace stdex

#ifdef _MSC_VER
#pragma component( mintypeinfo, off )
#endif

#endif	//_STDEX_VALUE_TRAITS_HPP_INCLUDED_


/*
 * File:   Feature3DEngine.cpp
 * Author: arwillis
 *
 * Created on August 18, 2015, 10:35 AM
 */
#include <math.h>
#include <iostream>
#include <fstream>
#include <string>
#include <iterator>

// ROS includes
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <message_filters/subscriber.h>

// Eigen includes
#include <Eigen/Dense>
#include <Eigen/Geometry>

// TF includes
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

// Includes for this Library
#include <calibrate_mocap_and_camera/calibrate_mocap_and_camera.h>

#include <eigen_conversions/eigen_msg.h>

bool validTransform(tf::StampedTransform & transform) {
	bool result = true;
	double normTranslation = transform.getOrigin().x() *transform.getOrigin().x() + transform.getOrigin().y() *transform.getOrigin().y() +transform.getOrigin().z() *transform.getOrigin().z() ;
	double normQuaternion = transform.getRotation().x()*transform.getRotation().x() + transform.getRotation().y()*transform.getRotation().y() + transform.getRotation().z()*transform.getRotation().z() + transform.getRotation().w()*transform.getRotation().w();
	if (( fabs(sqrt(normQuaternion)) > 1 ) or (sqrt(normTranslation) > 100000))
		{
		result = false;
		}
	return result;

    }

void CalibrateMocapAndCamera::ar_calib_pose_Callback(const geometry_msgs::TransformStampedConstPtr& ar_calib_pose) {

    static tf::TransformListener listener;
    static tf::TransformBroadcaster br;
    static geometry_msgs::TransformStampedConstPtr prior_tf;

    std::string cam_frame_id_str("tf_cam");
    std::string calib_frame_id_str("tf_calib");
    tf::StampedTransform cam_marker_pose;
    tf::StampedTransform calib_marker_pose;
    tf::StampedTransform tf_cam_to_rgb_optical_frame;
    static bool DEBUG = false;
    if (DEBUG) {
      ROS_INFO("Looking up transform from frame '%s' to frame '%s'", map_frame_id_str.c_str(),
               cam_frame_id_str.c_str());
    }
    ros::Time queryTime = ros::Time::now();
    try {
      listener.waitForTransform(map_frame_id_str, cam_frame_id_str,
                                queryTime, ros::Duration(1));
      listener.lookupTransform(map_frame_id_str, cam_frame_id_str,
                               queryTime, cam_marker_pose);
      listener.waitForTransform(map_frame_id_str, calib_frame_id_str,
                                queryTime, ros::Duration(1));
      listener.lookupTransform(map_frame_id_str, calib_frame_id_str,
                               queryTime, calib_marker_pose);
    } catch (tf::TransformException ex) {
      //ROS_ERROR("%s", ex.what());
      //        ros::Duration(1.0).sleep();
    }
    if (DEBUG) {
      std::cout << "cam_pose = " << cam_marker_pose.getOrigin().x() << ", " <<
                cam_marker_pose.getOrigin().y() << ", " << cam_marker_pose.getOrigin().z() << std::endl;
      std::cout << "calib_pose = " << calib_marker_pose.getOrigin().x() << ", " <<
                calib_marker_pose.getOrigin().y() << ", " << calib_marker_pose.getOrigin().z() << std::endl;
      std::cout << "ar_pose = " << ar_calib_pose->transform.translation << std::endl;
    }
    static geometry_msgs::TransformStamped ar_calib_to_camera;

    tf::Transform transform;

    transform.setOrigin(tf::Vector3(ar_calib_pose->transform.translation.x,
                                    ar_calib_pose->transform.translation.y, ar_calib_pose->transform.translation.z));
    transform.setRotation(tf::Quaternion(ar_calib_pose->transform.rotation.x, ar_calib_pose->transform.rotation.y,
                                         ar_calib_pose->transform.rotation.z, ar_calib_pose->transform.rotation.w).normalize());
    tf::Transform itransform = transform.inverse();
    br.sendTransform(tf::StampedTransform(itransform,
                                          ros::Time::now(), "ar_optical_frame", "rgb_optical_pose"));
    try {
      listener.waitForTransform(cam_frame_id_str, "rgb_optical_pose",
                                queryTime, ros::Duration(1));
      listener.lookupTransform(cam_frame_id_str, "rgb_optical_pose",
                               queryTime, tf_cam_to_rgb_optical_frame);
    } catch (tf::TransformException ex) {
    }

		if(validTransform(tf_cam_to_rgb_optical_frame))
    {
      br.sendTransform(tf::StampedTransform(tf_cam_to_rgb_optical_frame,
                                            ros::Time::now(), "tf_cam", "calib_rgb_optical_pose"));
    }

    if (DEBUG) {
      std::cout << "calib_result = " << tf_cam_to_rgb_optical_frame.getOrigin().x() << ", " <<
                tf_cam_to_rgb_optical_frame.getOrigin().y() << ", " << tf_cam_to_rgb_optical_frame.getOrigin().z() << std::endl;
    }
    tf::Quaternion calib_rot = tf_cam_to_rgb_optical_frame.getRotation();
    tf::Vector3 calib_translation = tf_cam_to_rgb_optical_frame.getOrigin();
    prior_tf = ar_calib_pose;
    if (itransform.getRotation().getW() != 1.) {
        std::cout << "calib_tran = [" << calib_translation.getX() << " "
              << calib_translation.getY() << " "
              << calib_translation.getZ() << "] "
              << "calib_quat = ["
              << calib_rot.getX() << " "
              << calib_rot.getY() << " "
              << calib_rot.getZ() << " "
              << calib_rot.getW() << "]" << std::endl;
        if (logdata) {
            fos << queryTime << ", "
                << calib_translation.getX() << ", "
                << calib_translation.getY() << ", "
                << calib_translation.getZ() << ", "
                << calib_rot.getX() << ", "
                << calib_rot.getY() << ", "
                << calib_rot.getZ() << ", "
                << calib_rot.getW() << ", "
                << cam_marker_pose.getOrigin().x() << ", "
                << cam_marker_pose.getOrigin().y() << ", "
                << cam_marker_pose.getOrigin().z() << ", "
                << cam_marker_pose.getRotation().x() << ", "
                << cam_marker_pose.getRotation().y() << ", "
                << cam_marker_pose.getRotation().z() << ", "
                << cam_marker_pose.getRotation().w() << ", "
                << calib_marker_pose.getOrigin().x() << ", "
                << calib_marker_pose.getOrigin().y() << ", "
                << calib_marker_pose.getOrigin().z() << ", "
                << calib_marker_pose.getRotation().x() << ", "
                << calib_marker_pose.getRotation().y() << ", "
                << calib_marker_pose.getRotation().z() << ", "
                << calib_marker_pose.getRotation().w() << ", "
                << ar_calib_pose->transform.translation.x << ", "
                << ar_calib_pose->transform.translation.y << ", "
                << ar_calib_pose->transform.translation.z << ", "
                << ar_calib_pose->transform.rotation.x << ", "
                << ar_calib_pose->transform.rotation.y << ", "
                << ar_calib_pose->transform.rotation.z << ", "
                << ar_calib_pose->transform.rotation.w <<std::endl;
        }
    }
}

int main(int argc, char **argv) {
    /**
     * The ros::init() function needs to see argc and argv so that it can perform
     * any ROS arguments and name remapping that were provided at the command line.
     * For programmatic remappings you can use a different version of init() which takes
     * remappings directly, but for most command-line programs, passing argc and argv is
     * the easiest way to do it.  The third argument to init() is the name of the node.
     *
     * You must call one of the versions of ros::init() before using any other
     * part of the ROS system.
     */
    ros::init(argc, argv, "calibrate_mocap_and_camera");
    /**
     * NodeHandle is the main access point to communications with the ROS system.
     * The first NodeHandle constructed will fully initialize this node, and the last
     * NodeHandle destructed will close down the node.
     */
    std::string tf_camera_marker_topic, tf_calib_marker_topic, ar_calib_topic;
    std::string optical_parent, optical_frame, _logfilename;
    bool _logdata;
    ros::NodeHandlePtr nodeptr(new ros::NodeHandle);
    ros::NodeHandle privnh("~");

    privnh.param<std::string>("tf_cam_topic", tf_camera_marker_topic, "/tf_cam/pose");
    privnh.param<std::string>("tf_calib_topic", tf_calib_marker_topic, "/tf_calib/pose");
    privnh.param<std::string>("ar_calib_topic", ar_calib_topic, "/ar_charuco/transform");
    privnh.param("logdata", _logdata, false);
    privnh.param<std::string>("logfilename", _logfilename, "");
    privnh.param<std::string>("optical_parent", optical_parent, "optitrack");
    privnh.param<std::string>("optical_frame", optical_frame, "rgb_optical_frame");
    std::cout << "RGBD parent coordinate frame name = \"" << optical_parent << "\"" << std::endl;
    std::cout << "RGBD coordinate frame name =  \"" << optical_frame << "\"" << std::endl;
    CalibrateMocapAndCamera::Ptr engineptr(new CalibrateMocapAndCamera(optical_parent,
            optical_frame, _logdata, _logfilename));

    engineptr->initializeSubscribers(nodeptr, tf_camera_marker_topic, tf_calib_marker_topic,
            ar_calib_topic);
    ros::spin();
    return 0;
}


#include "ros/ros.h"

#include <iostream>

#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>

#include <opencv2/core/core.hpp>

#include <opencv2/contrib/contrib.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud.h>

#include <boost/math/distributions/normal.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>

namespace fs = boost::filesystem;

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/PCLPointCloud2.h>
//#include <pcl/PCLPointCloud.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <tinyxml.h>

#include "ColorMaps.h"

using std::cout;
using std::endl;

sensor_msgs::PointCloud cloud_in_;
sensor_msgs::PointCloud cloud_;

image_transport::Publisher img_pub_;

ros::Publisher camera_info_pub_;
sensor_msgs::CameraInfo camera_info_;

//cv::VideoWriter record_;
int frame_count = 0;
std::string output_dir = "/home/syllogismrxs/temp/sonar";

#define PI 3.14159265359

// Normal Distribution Setup
typedef boost::mt19937                     ENG;    // Mersenne Twister
typedef boost::normal_distribution<double> DIST;   // Normal Distribution
typedef boost::variate_generator<ENG,DIST> GEN;    // Variate generator
ENG  eng;
DIST dist(0,0.01); // mean, std
GEN  gauss_sample(eng,dist);

void add_salt_and_pepper(cv::Mat& I, int amt)
{
     // accept only char type matrices
     CV_Assert(I.depth() != sizeof(uchar));

     //struct color_t c;

     const int channels = I.channels();
     switch(channels) {
     case 1: {
          cv::MatIterator_<uchar> it, end;
          for( it = I.begin<uchar>(), end = I.end<uchar>(); it != end; ++it) {
               *it += rand() % amt;
          }
          break;
     }
     case 3: {
          cv::MatIterator_<cv::Vec3b> it, end;
          for( it = I.begin<cv::Vec3b>(), end = I.end<cv::Vec3b>(); it != end; ++it) {
               //(*it)[0] += scale(c.b); // B
               //(*it)[1] += scale(c.g); // G
               //(*it)[2] += scale(c.r); // R
          }
     }}
}

double min_angle_;
double max_angle_;
double beam_width_;

std::string sonar_link_name;

void cloudCallback(const sensor_msgs::PointCloudConstPtr& msg)
{
     cloud_in_ = *msg;

     tf::TransformListener listener;
     tf::StampedTransform transform;
     try{
          listener.transformPointCloud(sonar_link_name, cloud_in_, cloud_);
     }
     catch (tf::TransformException ex){
          ROS_ERROR("%s",ex.what());
          return;
     }

     if (cloud_.points.size () == 0) {
          cout << "Point Cloud of size 0" << endl;
          return;
     }

     /////////////////////////////////////////////
     // Add Gaussian Distribution to PointCloud (in range direction)
     /////////////////////////////////////////////
     for (unsigned int i = 0; i < cloud_.points.size(); i++) {
          cloud_.points[i].x += gauss_sample();
     }

     double x_min = 0.1;  // R_min
     double x_max = 10.0; // R_max
     double y_min = 0;    //
     //double y_max = sin(0.392699082)*x_max*2;
     double y_max = sin(max_angle_)*x_max*2;

     int img_width = 600;
     //int img_width = 1500;
     int img_height = 600;

     camera_info_.height = img_height;
     camera_info_.width = img_width;

     cv::Mat img = cv::Mat::zeros(img_height, img_width, CV_8UC1);

     //add_salt_and_pepper(img, 10);

#if 0
     sensor_msgs::PointCloud2 sm_pcl2;
     sensor_msgs::convertPointCloudToPointCloud2(cloud_, sm_pcl2);

     pcl::PCLPointCloud2 pcl_pc2;
     pcl_conversions::toPCL(sm_pcl2, pcl_pc2);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
     pcl::fromPCLPointCloud2(pcl_pc2, *cloud);

     // Normal estimation*
     pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
     pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
     tree->setInputCloud (cloud);
     n.setInputCloud (cloud);
     n.setSearchMethod (tree);
     n.setKSearch (20);
     n.compute (*normals);

     // Concatenate the XYZ and normal fields*
     pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
     pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
     //* cloud_with_normals = cloud + normals

     // Create search tree*
     pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
     tree2->setInputCloud (cloud_with_normals);

     // Initialize objects
     pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
     pcl::PolygonMesh triangles;

     // Set the maximum distance between connected points (maximum edge length)
     //gp3.setSearchRadius (0.025);
     gp3.setSearchRadius (15);

     // Set typical values for the parameters
     gp3.setMu (2.5);
     gp3.setMaximumNearestNeighbors (100);
     gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
     gp3.setMinimumAngle(M_PI/18); // 10 degrees
     gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
     gp3.setNormalConsistency(false);

     // Get result
     gp3.setInputCloud (cloud_with_normals);
     gp3.setSearchMethod (tree2);
     gp3.reconstruct (triangles);

     // Additional vertex information
     //std::vector<int> parts = gp3.getPartIDs();
     //std::vector<int> states = gp3.getPointStates();

     std::vector<pcl::Vertices>::iterator it = triangles.polygons.begin();
     for(; it != triangles.polygons.end(); it++) {
          pcl::Vertices verts = *it;

          std::vector<uint32_t>::iterator it2 = verts.vertices.begin();
          int count = 0;
          double retro_sum = 0;
          cv::Point points[0][3];
          for (; it2 != verts.vertices.end(); it2++) {
               //int x_pos = img_width/2 + -cloud->points[*it2].y / (y_max/2) * img_height*sin(0.392699082);
               int x_pos = img_width/2 + -cloud->points[*it2].y / (y_max/2) * img_height*sin(max_angle_);
               int y_pos = cloud->points[*it2].x / x_max * img_height;

               points[0][count++] = cv::Point( x_pos, y_pos );

               retro_sum += cloud_.channels[0].values[*it2];

               cv::circle(img,cv::Point(x_pos, y_pos),1, cloud_.channels[0].values[*it2], 1,8,0);
          }

          const cv::Point* ppt[1] = { points[0] };
          int npt[] = { 3 };

          double retro = retro_sum / 3.0;
          if (retro > 255) {
               retro = 255;
          } else if (retro < 0) {
               retro = 0;
          }

          cv::fillPoly( img,
                        ppt,
                        npt,
                        1,
                        retro,
                        8,
                        0,
                        cv::Point(0,0));
     }

#else
     // Add the returned values
     for (unsigned int i = 0; i < cloud_.points.size(); i++) {
          if (cloud_.points[i].x > x_max) {
               cout << "Over x_max: " << cloud_.points[i].x << endl;
          }

          if (cloud_.points[i].y > y_max) {
               cout << "Over y_max: " << cloud_.points[i].y << endl;
          }

          //int x_pos = img_width/2 + -cloud_.points[i].y / (y_max/2) * img_height*sin(0.392699082);
          int x_pos = img_width/2 + -cloud_.points[i].y / (y_max/2) * img_height*sin(max_angle_);
          int y_pos = cloud_.points[i].x / x_max * img_height;

          if (x_pos < 0 || x_pos > img.cols || y_pos < 0 || y_pos > img.rows) {
               // error, skip
               cout << "bounds" << endl;
               continue;
          }
          double retro = cloud_.channels[0].values[i];
          if (retro > 255) {
               retro = 255;
          } else if (retro < 0) {
               retro = 0;
          }
          cv::circle(img,cv::Point(x_pos, y_pos),1,retro,1,8,0);
     }
#endif

     cv::medianBlur(img,img,3);
     //add_salt_and_pepper(img,20);

     cv::Mat img_color;
     //cv::applyColorMap(img,img_color,cv::COLORMAP_JET);
     /// COLORMAP_AUTUMN
     /// COLORMAP_BONE
     /// COLORMAP_COOL
     /// COLORMAP_HOT
     /// COLORMAP_HSV
     /// COLORMAP_JET
     /// COLORMAP_OCEAN
     /// COLORMAP_PINK
     /// COLORMAP_RAINBOW
     /// COLORMAP_SPRING
     /// COLORMAP_SUMMER
     /// COLORMAP_WINTER
     //cv::applyColorMap(img,img_color,cv::COLORMAP_JET);
     Gray2Jet_matlab(img, img_color);
     ///////////////////////////////

     //double start_angle = 1.178097245;

     double start_angle = (PI - beam_width_) / 2 ;
     double end_angle = 90.0 * PI / 180.0;//1.963495408;
     cv::Point start(img_width/2,0);
     cv::Point end_1(start.x+img_height*cos(start_angle), start.y+img_height*sin(start_angle));
     cv::Point end_2(start.x+img_height*cos(end_angle), start.y+img_height*sin(end_angle));

     std::vector<cv::Point> contour;
     contour.push_back(start);
     contour.push_back(end_1);

     double num_pts = 25;
     double step = (end_angle - start_angle) / num_pts;
     for (double ang = start_angle; ang <= end_angle; ang += step) {
          contour.push_back(cv::Point(start.x+img_height*cos(ang), start.y+img_height*sin(ang)));
     }

     contour.push_back(end_2);
     contour.push_back(cv::Point(img_width-1,img_height-1));
     contour.push_back(cv::Point(img_width-1,0));

     // create a pointer to the data as an array of points (via a conversion to
     // a Mat() object)
     const cv::Point *pts = (const cv::Point*) cv::Mat(contour).data;
     int npts = cv::Mat(contour).rows;

     // Fill the outside of the sonar image with black
     cv::fillPoly(img_color, &pts, &npts, 1, cv::Scalar(0,0,0), 8, 0, cv::Point(0,0));

     ///////////////////

     //start_angle = 112.5  * PI / 180.0;
     start_angle = ((PI - beam_width_) / 2) + beam_width_;
     end_angle = 90 * PI / 180.0;//1.963495408;
     end_1 = cv::Point(start.x+img_height*cos(start_angle), start.y+img_height*sin(start_angle));
     end_2 = cv::Point(start.x+img_height*cos(end_angle), start.y+img_height*sin(end_angle));

     contour.clear();
     contour.push_back(start);
     contour.push_back(end_1);

     num_pts = 25;
     step = (start_angle - end_angle) / num_pts;
     for (double ang = start_angle; ang >= end_angle; ang -= step) {
          contour.push_back(cv::Point(start.x+img_height*cos(ang), start.y+img_height*sin(ang)));
     }

     contour.push_back(end_2);
     contour.push_back(cv::Point(0,img_height-1));
     contour.push_back(cv::Point(0,0));

     // create a pointer to the data as an array of points (via a conversion to
     // a Mat() object)
     pts = (const cv::Point*) cv::Mat(contour).data;
     npts = cv::Mat(contour).rows;

     // Fill the outside of the sonar image with black
     cv::fillPoly(img_color, &pts, &npts, 1, 0, 8, 0, cv::Point(0,0));

     ////////////////////

     // Rotate the sonar image to pointing "up"
     cv::Point center = cv::Point( img_color.cols/2, img_color.rows/2 );
     double angle = 180.0;
     double rot_scale = 1.0;

     cv::Mat rot_mat(2,3,CV_8UC1);
     rot_mat = cv::getRotationMatrix2D( center, angle, rot_scale );
     cv::warpAffine(img_color, img_color, rot_mat, img_color.size());

     //cv::imshow("sonar_image",img);
     //cv::waitKey(1);
     std_msgs::Header img_header;
     img_header.stamp = ros::Time::now();

     sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(img_header,
                                                        "bgr8",
                                                        img_color).toImageMsg();

     camera_info_.header.stamp = img_header.stamp;

     camera_info_pub_.publish(camera_info_);
     img_pub_.publish(img_msg);

     std::ostringstream convert;
     convert << frame_count;

     std::string out_str = output_dir + "/sonar" + convert.str() + ".png";
     cv::imwrite(out_str, img_color);
     frame_count++;
     //if (!record_.isOpened()) {
     //     record_.open("/home/syllogismrxs/output.avi", CV_FOURCC('M','J','P','G'), 10, img_color.size(), true);
     //}
     //
     //if (record_.isOpened()) {
     //     record_ << img_color;
     //}
}

int main(int argc, char * argv[])
{
     srand (time(NULL));

     ros::init(argc, argv, "imaging_sonar_sim");
     ros::NodeHandle n_;

     ros::param::get("~sonar_link_name", sonar_link_name);

     std::string key;
     std::string robot_description;
     if (n_.searchParam("robot_description", key)) {
          n_.getParam(key, robot_description);
     } else {
          cout << "===========================" << endl;
          cout << "Warning: unable to find robot_description." << endl;
     }

     // Create output directory for sonar images if it doesn't exist
     fs::path dir(output_dir);
     if (!fs::is_directory(dir)) {
          fs::create_directories(output_dir);
     }

     // Need to extract the min and max angles for sonar point cloud
     TiXmlDocument doc;
     const char* pTest = doc.Parse(robot_description.c_str(), 0 , TIXML_ENCODING_UTF8);
     if(pTest != NULL){
          cout << "Error parsing robot_description" << endl;
     }
     TiXmlElement * robot_element = doc.FirstChildElement("robot");
     // Search for gazebo with reference="sonar_link"
     TiXmlElement *elem = robot_element->FirstChildElement();
     TiXmlElement *sonar_link_elem;
     while (elem) {
          if (std::string(elem->Value()) == "gazebo") {
               if (elem->Attribute("reference") != NULL) {
                    if (std::string(elem->Attribute("reference")) == sonar_link_name) {
                         sonar_link_elem = elem;
                         break;
                    }
               }
          }
          elem = elem->NextSiblingElement();
     }
     TiXmlElement *min_angle_elem = sonar_link_elem->FirstChildElement("sensor")->FirstChildElement("ray")->FirstChildElement("scan")->FirstChildElement("horizontal")->FirstChildElement("min_angle");
     TiXmlElement *max_angle_elem = sonar_link_elem->FirstChildElement("sensor")->FirstChildElement("ray")->FirstChildElement("scan")->FirstChildElement("horizontal")->FirstChildElement("max_angle");
     if ( ! (std::istringstream(min_angle_elem->GetText()) >> min_angle_) ) min_angle_ = -1;
     if ( ! (std::istringstream(max_angle_elem->GetText()) >> max_angle_) ) max_angle_ = 1;
     cout << "Min Angle: " << min_angle_ << endl;
     cout << "Max Angle: " << max_angle_ << endl;
     beam_width_ = max_angle_ * 2;

     std::string cloud_topic_name;
     ros::param::get("~cloud_topic_name", cloud_topic_name);

     // Setup sonar cloud subscription
     ros::Subscriber cloud_sub = n_.subscribe<sensor_msgs::PointCloud>(cloud_topic_name, 0, cloudCallback);

     // Setup sonar_image publication
     std::string image_topic_name;
     ros::param::get("~image_topic_name", image_topic_name);

     image_transport::ImageTransport it_(n_);
     img_pub_ = it_.advertise(image_topic_name, 1);

     cout << "=======" << endl;
     cout << "cloud name: " << cloud_topic_name << endl;
     cout << "image name: " << image_topic_name << endl;
     cout << "sonar link name: " << sonar_link_name << endl;

     camera_info_pub_ = n_.advertise<sensor_msgs::CameraInfo>("camera_info", 1);

     ros::Rate loop_rate(10);
     while (ros::ok()) {
          ros::spinOnce();
          loop_rate.sleep();
     }

     return 0;
}


#include <boost/accumulators/statistics/weighted_p_square_cumulative_distribution.hpp>


//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#include <hector_pose_estimation/filter/ekf.h>
#include <hector_pose_estimation/system.h>

#include <boost/pointer_cast.hpp>

#ifdef USE_HECTOR_TIMING
  #include <hector_diagnostics/timing.h>
#endif

namespace hector_pose_estimation {
namespace filter {

EKF::EKF(State &state)
  : Filter(state)
{}

EKF::~EKF()
{}

bool EKF::init(PoseEstimation &estimator)
{
  x_diff = State::Vector(state_.getVectorDimension());
  A = State::SystemMatrix(state_.getCovarianceDimension(), state_.getCovarianceDimension());
  Q = State::Covariance(state_.getCovarianceDimension(), state_.getCovarianceDimension());
  return true;
}

bool EKF::preparePredict(double dt)
{
  x_diff.setZero();
  A.setIdentity();
  Q.setZero();
  return Filter::preparePredict(dt);
}

bool EKF::predict(const SystemPtr& system, double dt)
{
  if (!Filter::predict(system, dt)) return false;
  EKF::Predictor *predictor = boost::dynamic_pointer_cast<EKF::Predictor>(system->predictor());
  x_diff += predictor->x_diff;
  A += predictor->A;
  Q += predictor->Q;
  return true;
}

bool EKF::doPredict(double dt) {
  ROS_DEBUG_NAMED("ekf.prediction", "EKF prediction (dt = %f):", dt);

  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "A      = [" << std::endl << A << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "Q      = [" << std::endl << Q << "]");

#ifdef USE_HECTOR_TIMING
  { hector_diagnostics::TimingSection section("predict.ekf.covariance");
#endif
  state().P() = A * state().P() * A.transpose() + Q;
  state().P().assertSymmetric();

#ifdef USE_HECTOR_TIMING
  }
  { hector_diagnostics::TimingSection section("predict.ekf.state");
#endif
  state().update(x_diff);

#ifdef USE_HECTOR_TIMING
  }
#endif

  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "x_pred = [" << state().getVector().transpose() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "P_pred = [" << std::endl << state().getCovariance() << "]");

  Filter::doPredict(dt);
  return true;
}

} // namespace filter
} // namespace hector_pose_estimation


#ifndef HEADERS_KMEDOIDS_ALGORITHM_HPP_
#define HEADERS_KMEDOIDS_ALGORITHM_HPP_

#include "log_helper.hpp"

#include <carma>
#include <armadillo>
#include <vector>
#include <fstream>
#include <iostream>
#include <chrono>
#include <omp.h>
#include <tuple>
#include <functional>
#include <unordered_map>
#include <string>

typedef std::tuple<size_t, size_t> key_t_bpam;

struct key_hash : public std::unary_function<key_t_bpam, double> {
  std::size_t operator()(const key_t_bpam& k) const;
};

namespace km {
  /**
   *  \brief Class implementation for running KMedoids methods.
   *
   *  KMedoids class. Creates a KMedoids object that can be used to find the medoids
   *  for a particular set of input data.
   *
   *  @param nMedoids Number of medoids/clusters to create
   *  @param algorithm Algorithm used to find medoids; options are "BanditPAM" for
   *  the "Bandit-PAM" algorithm, or "naive" to use the naive method
   *  @param verbosity Verbosity of the algorithm, 0 will have no log file
   *  emitted, 1 will emit a log file
   *  @param maxIter The maximum number of iterations the algorithm runs for
   *  @param buildConfidence Constant that affects the sensitivity of build confidence bounds
   *  @param swapConfidence Constant that affects the sensitiviy of swap confidence bounds
   *  @param logFilename The name of the output log file
   */
class KMedoids {
 public:
      KMedoids(size_t n_medoids = 5, const std::string& algorithm = "BanditPAM", size_t verbosity = 0, size_t max_iter = 1000,
              size_t buildConfidence =  1000, size_t swapConfidence = 10000, std::string logFilename = "KMedoidsLogfile");

      ~KMedoids();

      void fit(const arma::mat& inputData, const std::string& loss);

      // std::map is a RB tree, should use unordered_map
      std::unordered_map<key_t_bpam, double, key_hash> cache;

      // The functions below are "get" functions for read-only attributes

      arma::rowvec getMedoidsFinal();

      arma::rowvec getMedoidsBuild();

      arma::rowvec getLabels();

      size_t getSteps();

      // The functions below are get/set functions for attributes

      size_t getNMedoids();

      void setNMedoids(size_t new_num);

      std::string getAlgorithm();

      void setAlgorithm(const std::string& new_alg); // pass by ref

      size_t getVerbosity();

      void setVerbosity(size_t new_ver);

      size_t getMaxIter();

      void setMaxIter(size_t new_max);

      size_t getbuildConfidence();

      void setbuildConfidence(size_t new_buildConfidence);

      size_t getswapConfidence();

      void setswapConfidence(size_t new_swapConfidence);

      std::string getLogfileName();

      void setLogFilename(const std::string& new_lname);

      void setLossFn(std::string loss);

 protected:
      // The functions below are PAM's constituent functions
      arma::rowvec build_sigma(
        const arma::mat& data,
        arma::rowvec& best_distances,
        arma::uword batch_size,
        bool use_absolute
      );

      void calc_best_distances_swap(
        const arma::mat& data,
        arma::rowvec& medoidIndices,
        arma::rowvec& best_distances,
        arma::rowvec& second_distances,
        arma::rowvec& assignments
      );

      arma::mat swap_sigma(
        const arma::mat& data,
        size_t batch_size,
        arma::rowvec& best_distances,
        arma::rowvec& second_best_distances,
        arma::rowvec& assignments
      );

      void sigma_log(arma::mat& sigma);

      double calc_loss(const arma::mat& data, arma::rowvec& medoidIndices);

      // Loss functions
      double wrappedLossFn(const arma::mat& data, size_t i, size_t j, bool use_cache);

      size_t lp;
      double LP(const arma::mat& data, size_t i, size_t j) const;

      double LINF(const arma::mat& data, size_t i, size_t j) const;

      double cos(const arma::mat& data, size_t i, size_t j) const;

      double manhattan(const arma::mat& data, size_t i, size_t j) const;

      void checkAlgorithm(const std::string& algorithm);

      // Constructor params
      size_t n_medoids; ///< number of medoids identified for a given dataset

      std::string algorithm; ///< options: "naive" and "BanditPAM"

      size_t max_iter; ///< maximum number of iterations during KMedoids::fit

      // Properties of the KMedoids instance
      arma::mat data; ///< input data used during KMedoids::fit

      arma::rowvec labels; ///< assignments of each datapoint to its medoid

      arma::rowvec medoid_indices_build; ///< medoids at the end of build step

      arma::rowvec medoid_indices_final; ///< medoids at the end of the swap step

      double (KMedoids::*lossFn)(const arma::mat& data, size_t i, size_t j) const; ///< loss function used during KMedoids::fit

      LogHelper logHelper; ///< helper object for making formatted logs

      size_t steps; ///< number of actual swap iterations taken by the algorithm

      // Hyperparameters
      size_t buildConfidence; ///< constant that affects the sensitivity of build confidence bounds

      size_t swapConfidence; ///< constant that affects the sensitiviy of swap confidence bounds

      const double precision = 0.001; ///< bound for double comparison precision

      const size_t batchSize = 100; ///< batch size for computation steps

      size_t verbosity; ///< determines whether KMedoids::fit outputs a logfile

      std::string logFilename; ///< name of the logfile output (verbosity permitting)
};
} // namespace km
#endif // HEADERS_KMEDOIDS_ALGORITHM_HPP_


#pragma once

#include <vector>
#include <algorithm>

#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>

#include "cartesian_tree.hpp"

namespace succinct {

    // XXX(ot): implement arbitrary comparator
    template <typename Vector>
    class topk_vector : boost::noncopyable {
    public:
        typedef Vector vector_type;
        typedef typename vector_type::value_type value_type;
        typedef boost::tuple<value_type, uint64_t> entry_type;
        typedef std::vector<entry_type> entry_vector_type;

        topk_vector()
        {}

        template <typename Range>
        topk_vector(Range const& v)
        {
            cartesian_tree(v, std::greater<typename boost::range_value<Range>::type>())
                .swap(m_cartesian_tree);
            vector_type(v).swap(m_v);
        }

        value_type const
        operator[](uint64_t idx) const
        {
            return m_v[idx];
        }

        uint64_t size() const
        {
            return m_v.size();
        }

        class enumerator
        {
        public:
            enumerator()
                : m_topkv(0)
            {}

            bool next()
            {
                using boost::tie;
                if (m_q.empty()) return false;

                value_type cur_mid_val;
                uint64_t cur_mid, cur_a, cur_b;

                std::pop_heap(m_q.begin(), m_q.end(), value_index_comparator());
                tie(cur_mid_val, cur_mid, cur_a, cur_b) = m_q.back();
                m_q.pop_back();

                m_cur = entry_type(cur_mid_val, cur_mid);

                if (cur_mid != cur_a) {
                    uint64_t m = m_topkv->m_cartesian_tree.rmq(cur_a, cur_mid - 1);
                    m_q.push_back(queue_element_type(m_topkv->m_v[m], m, cur_a, cur_mid - 1));
                    std::push_heap(m_q.begin(), m_q.end(), value_index_comparator());
                }

                if (cur_mid != cur_b) {
                    uint64_t m = m_topkv->m_cartesian_tree.rmq(cur_mid + 1, cur_b);
                    m_q.push_back(queue_element_type(m_topkv->m_v[m], m, cur_mid + 1, cur_b));
                    std::push_heap(m_q.begin(), m_q.end(), value_index_comparator());
                }

                return true;
            }

            entry_type const& value() const
            {
                return m_cur;
            }

            friend class topk_vector;

            void swap(enumerator& other)
            {
                using std::swap;
                swap(m_topkv, other.m_topkv);
                swap(m_q, other.m_q);
                swap(m_cur, other.m_cur);
            }

        private:

            void set(topk_vector const* topkv, uint64_t a, uint64_t b)
            {
                assert(a <= b);
                clear();
                m_topkv = topkv;

                uint64_t m = m_topkv->m_cartesian_tree.rmq(a, b);
                m_q.push_back(queue_element_type(m_topkv->m_v[m], m, a, b));
            }

            typedef boost::tuple<value_type, uint64_t, uint64_t, uint64_t> queue_element_type;

            struct value_index_comparator {
                template <typename Tuple>
                bool operator()(Tuple const& a, Tuple const& b) const
                {
                    using boost::get;
                    // lexicographic, increasing on value and decreasing
                    // on index
                    return (get<0>(a) < get<0>(b) ||
                            (get<0>(a) == get<0>(b) &&
                             get<1>(a) > get<1>(b)));
                }
            };

        public:
            void clear()
            {
                m_topkv = 0;
                m_q.clear();
            }

        private:
            topk_vector const* m_topkv;
            std::vector<queue_element_type> m_q;
            entry_type m_cur;
        };

        // NOTE this is b inclusive
        // XXX switch to [a, b) ?
        void get_topk_enumerator(uint64_t a, uint64_t b, enumerator& ret) const
        {
            ret.set(this, a, b);
        }

        enumerator get_topk_enumerator(uint64_t a, uint64_t b) const
        {
            enumerator ret;
            get_topk_enumerator(a, b, ret);
            return ret;
        }

        entry_vector_type
        topk(uint64_t a, uint64_t b, size_t k) const
        {
            entry_vector_type ret(std::min(size_t(b - a + 1), k));
            enumerator it = get_topk_enumerator(a, b);

            bool hasnext;
            for (size_t i = 0; i < ret.size(); ++i) {
                hasnext = it.next();
                assert(hasnext); (void)hasnext;
                ret[i] = it.value();
            }

            assert(ret.size() == k || !it.next());

            return ret;
        }

        template <typename Visitor>
        void map(Visitor& visit)
        {
            visit
                (m_v, "m_v")
                (m_cartesian_tree, "m_cartesian_tree");
        }

        void swap(topk_vector& other)
        {
            other.m_v.swap(m_v);
            other.m_cartesian_tree.swap(m_cartesian_tree);
        }

    protected:

        vector_type m_v;
        cartesian_tree m_cartesian_tree;
    };

}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef AIKIDO_COMMON_SPLINEFWD_HPP_
#define AIKIDO_COMMON_SPLINEFWD_HPP_

#include <Eigen/Core>

namespace aikido {
namespace common {

template <typename Scalar, int Dim, int Degree = Eigen::Dynamic>
class BSpline;

template <typename SplineType, int DerivativeOrder = Eigen::Dynamic>
struct SplineTraits
{
};

/**
 * \ingroup Splines_Module
 * \brief Compile-time attributes of the Spline class for Dynamic degree.
 **/
template <typename _Scalar, int _Dim, int _Degree>
struct SplineTraits<BSpline<_Scalar, _Dim, _Degree>, Eigen::Dynamic>
{
  typedef _Scalar Scalar; /*!< The spline curve's scalar type. */
  enum
  {
    Dimension = _Dim /*!< The spline curve's dimension. */
  };
  enum
  {
    Degree = _Degree /*!< The spline curve's degree. */
  };

  enum
  {
    OrderAtCompileTime
    = _Degree == Eigen::Dynamic
          ? Eigen::Dynamic
          : _Degree + 1 /*!< The spline curve's order at compile-time. */
  };
  enum
  {
    NumOfDerivativesAtCompileTime = OrderAtCompileTime /*!< The number of
                                                          derivatives defined
                                                          for the current
                                                          spline. */
  };

  enum
  {
    DerivativeMemoryLayout
    = Dimension == 1
          ? Eigen::RowMajor
          : Eigen::ColMajor /*!< The derivative type's memory layout. */
  };

  /** \brief The data type used to store non-zero basis functions. */
  typedef Eigen::Array<Scalar, 1, OrderAtCompileTime> BasisVectorType;

  /** \brief The data type used to store the values of the basis function
   * derivatives. */
  typedef Eigen::Array<
      Scalar,
      Eigen::Dynamic,
      Eigen::Dynamic,
      Eigen::RowMajor,
      NumOfDerivativesAtCompileTime,
      OrderAtCompileTime>
      BasisDerivativeType;

  /** \brief The data type used to store the spline's derivative values. */
  typedef Eigen::Array<
      Scalar,
      Dimension,
      Eigen::Dynamic,
      DerivativeMemoryLayout,
      Dimension,
      NumOfDerivativesAtCompileTime>
      DerivativeType;

  /** \brief The point type the spline is representing. */
  typedef Eigen::Array<Scalar, Dimension, 1> PointType;

  /** \brief The data type used to store knot vectors. */
  typedef Eigen::Array<Scalar, 1, Eigen::Dynamic> KnotVectorType;

  /** \brief The data type used to store parameter vectors. */
  typedef Eigen::Array<Scalar, 1, Eigen::Dynamic> ParameterVectorType;

  /** \brief The data type representing the spline's control points. */
  typedef Eigen::Array<Scalar, Dimension, Eigen::Dynamic>
      ControlPointVectorType;
};

/**
 * \ingroup Splines_Module
 * \brief Compile-time attributes of the Spline class for fixed degree.
 *
 * The traits class inherits all attributes from the SplineTraits of Dynamic
 *degree.
 **/
template <typename _Scalar, int _Dim, int _Degree, int _DerivativeOrder>
struct SplineTraits<BSpline<_Scalar, _Dim, _Degree>, _DerivativeOrder>
  : public SplineTraits<BSpline<_Scalar, _Dim, _Degree> >
{
  enum
  {
    OrderAtCompileTime
    = _Degree == Eigen::Dynamic
          ? Eigen::Dynamic
          : _Degree + 1 /*!< The spline curve's order at compile-time. */
  };
  enum
  {
    NumOfDerivativesAtCompileTime
    = _DerivativeOrder == Eigen::Dynamic
          ? Eigen::Dynamic
          : _DerivativeOrder + 1 /*!< The number of
                                    derivatives defined for
                                    the current spline. */
  };

  enum
  {
    DerivativeMemoryLayout
    = _Dim == 1 ? Eigen::RowMajor
                : Eigen::ColMajor /*!< The derivative type's memory layout. */
  };

  /** \brief The data type used to store the values of the basis function
   * derivatives. */
  typedef Eigen::Array<
      _Scalar,
      Eigen::Dynamic,
      Eigen::Dynamic,
      Eigen::RowMajor,
      NumOfDerivativesAtCompileTime,
      OrderAtCompileTime>
      BasisDerivativeType;

  /** \brief The data type used to store the spline's derivative values. */
  typedef Eigen::Array<
      _Scalar,
      _Dim,
      Eigen::Dynamic,
      DerivativeMemoryLayout,
      _Dim,
      NumOfDerivativesAtCompileTime>
      DerivativeType;
};

/** \brief 2D float B-spline with dynamic degree. */
typedef BSpline<float, 2> BSpline2f;

/** \brief 3D float B-spline with dynamic degree. */
typedef BSpline<float, 3> BSpline3f;

/** \brief 2D double B-spline with dynamic degree. */
typedef BSpline<double, 2> BSpline2d;

/** \brief 3D double B-spline with dynamic degree. */
typedef BSpline<double, 3> BSpline3d;

} // namespace common
} // namespace aikido

#endif // AIKIDO_COMMON_SPLINEFWD_HPP_


#include "../Function_State.hxx"

#include "../../../../Boost_Float.hxx"
#include "../../../../ostream_vector.hxx"

#include <boost/math/constants/constants.hpp>

void Function_State::json_end_object()
{
  if(parsing_max_delta)
    {
      throw std::runtime_error(
        "Invalid input file.  Unexpected object end inside '" + name + "."
        + max_delta_state.name + "'.");
    }
  else if(parsing_epsilon_value)
    {
      throw std::runtime_error(
        "Invalid input file.  Unexpected object end inside '" + name + "."
        + epsilon_value_state.name + "'.");
    }
  else if(parsing_infinity_value)
    {
      throw std::runtime_error(
        "Invalid input file.  Unexpected object end inside '" + name + "."
        + infinity_value_state.name + "'.");
    }
  else if(parsing_chebyshev_values)
    {
      throw std::runtime_error(
        "Invalid input file.  Unexpected object end inside '" + name + "."
        + chebyshev_values_state.name + "'.");
    }

  // Convert from sampled values to chebyshev coefficients.
  const Boost_Float pi(boost::math::constants::pi<Boost_Float>());
  auto &values(chebyshev_values_state.value);
  const size_t N(values.size());
  value.chebyshev_coeffs.resize(0);
  value.chebyshev_coeffs.reserve(N);
  std::vector<Boost_Float> coeffs(N, Boost_Float(0));
  for(size_t n(0); n < N; ++n)
    {
      Boost_Float coeff(0);
      for(size_t k(0); k < N; ++k)
        {
          coeff += 2 * cos((n * pi * (2 * (N - 1 - k) + 1)) / (2 * N))
                   * values[k] / N;
        }
      value.chebyshev_coeffs.emplace_back(to_string(coeff));
    }
  inside = false;
}


#ifndef _RYSQ_FOCK_HPP_
#define _RYSQ_FOCK_HPP_

#include <boost/ref.hpp>
#include "rysq/core.hpp"

namespace rysq {

    struct block_matrix_layout {
	block_matrix_layout() {}
	block_matrix_layout(size_t size1,size_t size2,
			    size_t block1, size_t block2) {
	    if ( size1% block1 || size2% block2) throw;
	    this->block1 = block1; this->block2 = block2;
	    this->distance1 = block1*block2;
	    this->distance2 = distance1*(size1/block1);
	}
    	size_t element_at(int i,int j) const {
	    int a = i/block1, b = j/block2;
	    return block_at(a,b) + block_element_at(i - a* block1, j - b*block2);
	}
	size_t block_at(int a,int b) const { return a*distance1 + b*distance2; }
	size_t block_element_at(int i, int j) const { return i + j*block1; }
	size_t block1, block2;
	size_t distance1, distance2;
    };

    template<typename T>
    struct block_matrix_base {
	block_matrix_base(){}
	block_matrix_base(size_t size1, size_t size2, size_t block1, size_t block2)
	    : size1_(size1), size2_( size2),
	      layout_(size1, size2, block1, block2) {}
	size_t size1() const { return size1_; }
	size_t size2() const { return size2_; }
	size_t size()const { return size1_*size2_; }
	size_t block1() const { return layout_.block1; }
	size_t block2() const { return layout_.block2; }
    protected:
	template<class M>
	void check_size(const M &m) const {
	    if (size1() != m.size1() || size2() != m.size2()) throw;
	}
	size_t size1_,  size2_;
	block_matrix_layout layout_;
    };

    template<typename T, class A>
    struct block_matrix : block_matrix_base<T> {
	typedef block_matrix_base<T> base;
	block_matrix(size_t size1, size_t size2,
		     size_t block1, size_t block2)
	    : base(size1, size2, block1, block2),
	      data_(new T[this->size()]) {}
	template<class M>
	void assign(const M &m) {
	    base:: check_size(m);
	    for (size_t j = 0; j < m.size2(); ++j) {
		for (size_t i = 0; i <  m.size1(); ++i) {
		    (*this)(i,j) = m(i,j);
		}
	    }
	}
	T& operator()(int i,int j) {
	    return data_[base::layout_.element_at(i,j)];
	}
	const T& operator()(int i,int j) const {
	    return data_[base::layout_.element_at(i,j)];
	}
	T* data() { return data_.get(); }
	const T* data() const { return data_.get(); }
    protected:
	A data_;
    };

    template<typename T>
    struct block_matrix_adapter : block_matrix_base<T> {
	typedef block_matrix_base<T> base;
	using base::size1;
	using base::size2;
	block_matrix_adapter() : data_(NULL) {}
	block_matrix_adapter(size_t size1, size_t size2, T *data,
			     size_t block1, size_t block2)
	    : base(size1, size2, block1, block2),
	      data_(data) {}
	virtual ~block_matrix_adapter() {}
	template<class M>
	void assign(const M &A) {
	    if (A.size1() != size1() || A.size2() != size2()) throw;
	    for (size_t j = 0; j < size2(); ++j) {
		for (size_t i = 0; i < size1(); ++i) {
		    (*this)(i,j) = A(i,j);
		}
	    }
	}

	T& operator()(int i,int j) {
	    return data_[base::layout_.element_at(i,j)];
	}
	const T& operator()(int i,int j) const {
	    return data_[base::layout_.element_at(i,j)];
	}

	virtual T* block(int i, int j) {
	    return data_ + base::layout_.block_at(i,j);
	}
	virtual	const T* block(int i, int j) const {
	    return data_ + base::layout_.block_at(i,j);
	}

	void bind(T * origin) { data_ = origin; }
	T* data() { return data_; }
	const T* data() const { return data_; }
    protected:
	T* data_;
	int base_[2];
    };

    namespace hf {

	template<class M>
	struct matrix_set : index_set<M> {
	    matrix_set(const int (&base)[4]) {
		std::copy(base, base + 4, base_index.begin());
	    }
	    boost::array<int,4> base_index;
	};

	template<class M>
	struct matrix_ptr_set : matrix_set<M*> {
	    typedef matrix_set<M*> base_type;
	    matrix_ptr_set(const int (&base)[4]):base_type(base){}
	    template<class M2>
	    matrix_ptr_set(matrix_set<M2> &set)
		: base_type(set.base_index.elems)
	    {
		for (int i = 0; i < 12; ++i)
		    (*this)[i] = &set[i];
	    }
	};

    }

    class Fock {
    public:

	typedef block_matrix_adapter<const double> density_matrix;
	typedef block_matrix_adapter<double> fock_matrix;

	typedef hf::matrix_ptr_set<density_matrix> density_matrix_set;
	typedef hf::matrix_ptr_set<fock_matrix> fock_matrix_set;

	typedef index_set<double*> fock_set;
	typedef index_set<const double*> density_set;

	class Impl;
	Fock(const Quartet<Shell> &shells);
	~Fock();

	void operator()(const Quartet<Center> &centers,
			density_set D, fock_set F,
			const Parameters &parameters);

	void operator()(const std::vector<Center> &centers,
			const std::vector<Int4> &quartets,
			density_matrix_set D, fock_matrix_set F,
			const Parameters &parameters);

	static void eval(const Quartet<type> &quartet,
			 const std::vector<Int4> &quartets,
			density_matrix_set D, fock_matrix_set F,
			 const double *Eri, const Parameters &parameters);

	static void eval(const Quartet<type> &quartet,
			 density_set D, fock_set F,
			 const double *Eri, const Parameters &parameters);

	// static void eval(const Quartet<type> &quartet,
	// 		 const matrix::Adapter &D,
	// 		 matrix::Adapter &F,
	// const std::vector<Int4> &quartets,
	// double *Eri, const Parameters &parameters);
    private:
	Transpose transpose_;
	void *kernel_;
    };

    // namespace parallel {

    // 	class Fock {
    // 	    const Quartet<Shell> &shells;
    // 	public:
    // 	    Fock(const Quartet<Shell> &shells);
    // 	    ~Fock();
    // 	    void operator()(const std::vector<Center> &centers,
    // 			    const std::vector<Int4> &quartets,
    // 			    density_matrix_set D, fock_matrix_set F,
    // 			    const Parameters &parameters);
    // 	};

    // }

}

#endif /* _RYSQ_FOCK_HPP_ */