/*
 * File:   Simulation.cpp
 * Author: ben
 *
 * Created on September 21, 2012, 11:18 AM
 */

#include "Simulation.h"
#include "Utils.h"
#include "moves/MoveFactory.h"
#include "Boltzmann.h"
#include "observables/Observable.h"

#include <boost/assign/std.hpp>
#include <boost/unordered_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/array.hpp>
#include <fstream>
#include <sstream>
#include <ctime>

using namespace boost::assign;

Simulation::Simulation() :
k(READ_CONF("mc.k", 10)),
z(READ_CONF("mc.z", 20)),
measureAtVolume(READ_CONF("general.measureAtVolume", true)),
drift(READ_CONF("volume.drift", false)),
volumeStart(READ_CONF("volume.start", 500)),
volumeEnd(READ_CONF("volume.end", 10000)),
skip(READ_CONF("volume.skip", 100)),
drop(READ_CONF("general.drop", 0)) {
    /* Initialize random number generator */
    setSeed(std::time(0));
    moveFactory = new MoveFactory(*this);
}

Simulation::Simulation(const Simulation& orig) {
}

Simulation::~Simulation() {
    clearTriangulation();
    delete moveFactory;

    foreach(Observable* obs, observables) {
        delete obs;
    }
}

void Simulation::clearTriangulation() {
    TriSet deleted;

    foreach(Vertex* v, vertices) {

        foreach(Triangle* t, v->getTriangles()) {
            if (deleted.find(t) == deleted.end()) {
                delete t;
                deleted.insert(t);
            }
        }
        delete v;
    }

    TTSCount = 0;
    SSTCount = 0;
    vertices.clear();
}

void Simulation::readFromFile(const char* filename) {
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cout << "Unable to open file '" << filename << "'" << std::endl;
        return;
    }

    char type;
    int vCount, vertA, vertB, vertC;

    file >> vCount;
    Vertex * vertex_array[vCount];

    clearTriangulation(); // clear old grid

    for (int i = 0; i < vCount; i++) {
        vertex_array[i] = new Vertex();
        vertices.push_back(vertex_array[i]);
    }

    // TODO: check if the triangulation is valid?
    while (file >> type >> vertA >> vertB >> vertC) {
        new Triangle(type == 'T' ? Triangle::TTS : Triangle::SST, vertex_array[vertA],
                vertex_array[vertB], vertex_array[vertC]);

        if (type == 'T') {
            TTSCount++;
        } else {
            SSTCount++;
        }
    }

    file.close();
}

void Simulation::writeToFile(const char* filename) {
    std::ofstream file(filename);
    if (!file.is_open()) {
        std::cout << "Unable to open file '" << filename << "'" << std::endl;
        return;
    }

    int vCount = 0;
    typedef boost::unordered_map<Vertex*, int> vertex_map;
    vertex_map vertexMap;

    foreach(Vertex* v, vertices) {
        vertexMap.insert(vertex_map::value_type(v, vCount));
        vCount++;
    }

    file << vCount << std::endl;

    TriSet visitedTri;

    foreach(Vertex* v, vertices) {

        foreach(Triangle* t, v->getTriangles()) {
            if (visitedTri.find(t) != visitedTri.end()) {
                continue;
            }
            visitedTri.insert(t);

            if (t->getType() == Triangle::TTS) {
                file << "T";
            } else {
                file << "S";
            }

            for (int i = 0; i < 3; i++) {
                file << " " << vertexMap[t->getVertex(i)];
            }

            file << std::endl;
        }
    }

    file.close();
}

void Simulation::generateInitialTriangulation(int N, int T) {
    Vertex * vertices[N * T];
    Triangle * triangles[N * T * 2]; // TODO: remove, unnecessary

    /* Create vertices */
    for (int t = 0; t < T * N; t++) {
        vertices[t] = new Vertex();
        this->vertices.push_back(vertices[t]);
    }

    /* Create foliation */
    for (int t = 0; t < T; t++) {
        for (int s = 0; s < N; s++) {
            Triangle* u = new Triangle(Triangle::TTS, vertices[t * N + s], vertices[((t + 1) % T) * N + s],
                    vertices[t * N + (s + 1) % N]);
            Triangle* v = new Triangle(Triangle::TTS, vertices[((t + 1) % T) * N + (s + 1) % N], vertices[t * N + (s + 1) % N],
                    vertices[((t + 1) % T) * N + s]);
            triangles[t * 2 * N + 2 * s] = u;
            triangles[t * 2 * N + 2 * s + 1] = v;
        }
    }

    TTSCount = N * T * 2;
    SSTCount = 0;
}

Vertex* Simulation::getRandomVertex(const std::vector<Vertex*>& vertices) {
    BOOST_ASSERT(vertices.size() > 0);
    boost::uniform_int<> uint(0, vertices.size() - 1);
    return vertices[uint(rng)];
}

void Simulation::collectTriangles(TriSet& triSet, Vertex* v, int depth) {
    if (depth <= 0) {
        triSet += v->getTriangles();
        return;
    }

    triSet += v->getTriangles();

    foreach(Vertex* u, v->getNeighbouringVertices()) {
        collectTriangles(triSet, u, depth - 1);
    }
}

void Simulation::drawPartialTriangulation(const char* filename, Vertex* v, const TriSet& tri) {
    std::ofstream dotFile(filename);
    if (!dotFile.is_open()) {
        std::cout << "Unable to open file '" << filename << "'" << std::endl;
        return;
    }

    dotFile << "strict graph G {" << std::endl << "node[shape=point];" << std::endl;
    dotFile << "n" << v << "[color=green];" << std::endl;

    foreach(Triangle* t, tri) {
        for (int i = 0; i < 3; i++) {
            dotFile << "n" << t->getVertex(i) << "--" << "n" << t->getVertex((i + 1) % 3);
            if (t->isTimelike(i)) {
                dotFile << " [color=red];" << std::endl;
            } else {
                dotFile << " [color=blue];" << std::endl;
            }
        }
    }

    dotFile << "}";
    dotFile.close();
}

// FIXME
void Simulation::checkLinkOverlap() {
  /*  for (unsigned int i = 0; i < vertices.size() - 1; i++) {
        for (unsigned int j = i + 1; j < vertices.size(); j++) {
            TriSet t;

            TriSet t = vertices[i]->getTriangles() & vertices[j]->getTriangles();

            if (t.size() != 2 && t.size() != 0) {
                std::cerr << "Link duplicates: " << i << " " << j << " " << t.size() << std::endl;
                BOOST_ASSERT(false);
            }
        }
    }*/
}

void Simulation::printTriangleConnectivity(Triangle* t) {
    typedef boost::tuple<Triangle*, Vertex*, Vertex*> curpos;

    int newId = 0;
    boost::unordered_map<Triangle*, int> tri;
    std::queue<curpos> neighbours;

    // add current triangle and first neighbour
    neighbours.push(
            boost::make_tuple(t, t->getVertex(0), t->getVertex(1)));
    neighbours.push(
            boost::make_tuple(t->getNeighbour(0), t->getVertex(0), t->getVertex(1)));

    while (!neighbours.empty()) {
        curpos cur = neighbours.front();
        neighbours.pop();

        Triangle* t = cur.get < 0 > ();

        boost::unordered_map<Triangle*, int>::iterator res = tri.find(t);
        if (res != tri.end()) {
            std::cout << res->second << std::endl;
            continue;
        }

        // create character of tri a - b - c
        Vertex* a = cur.get < 1 > ();
        Vertex* b = cur.get < 2 > ();
        Vertex* c = t->getThirdVertex(a, b);

        std::cout << (t->isTimelike(a, b) ? "T" : "S") <<
                (t->isTimelike(b, c) ? "T" : "S") <<
                (t->isTimelike(c, a) ? "T" : "S") << ": " << newId << std::endl;

        tri[t] = newId;
        newId++;

        // add the two other neighbours to the stack
        neighbours.push(boost::make_tuple(t->getNeighbour(b, c),
                b, c));
        neighbours.push(boost::make_tuple(t->getNeighbour(c, a),
                c, a));
    }
}

std::vector<int> Simulation::createID(Triangle* t) {
    typedef boost::tuple<Triangle*, Vertex*, Vertex*> curpos;
    std::vector<int> id;

    int newId = 8; // start at 8, 8 > character
    boost::unordered_map<Triangle*, int> tri;
    std::queue<curpos> neighbours;

    // add current triangle and first neighbour
    neighbours.push(
            boost::make_tuple(t, t->getVertex(0), t->getVertex(1)));
    neighbours.push(
            boost::make_tuple(t->getNeighbour(0), t->getVertex(0), t->getVertex(1)));

    while (!neighbours.empty()) {
        curpos cur = neighbours.front();
        neighbours.pop();

        Triangle* t = cur.get < 0 > ();

        boost::unordered_map<Triangle*, int>::iterator res = tri.find(t);
        if (res != tri.end()) {
            id.push_back(res->second);
            continue;
        }

        // create character of tri a - b - c
        Vertex* a = cur.get < 1 > ();
        Vertex* b = cur.get < 2 > ();
        Vertex* c = t->getThirdVertex(a, b);

        int character = t->isTimelike(a, b) * 4 + t->isTimelike(b, c) * 2 +
                t->isTimelike(c, a);

        id.push_back(character);
        tri[t] = newId;
        newId++;

        // add the two other neighbours to the stack
        neighbours.push(boost::make_tuple(t->getNeighbour(b, c),
                b, c));
        neighbours.push(boost::make_tuple(t->getNeighbour(c, a),
                c, a));
    }

    return id;
}

void Simulation::Metropolis(double alpha, unsigned int volume, double
        deltaVolume, unsigned int numSweeps, unsigned int sweepLength) {
    unsigned long long moveRejectedBecauseImpossible = 0, moveRejectedBecauseDetBal = 0;
    double bias = 0; // count the bias of the system size
    double lambda = 5;
    bool measured = false;

    /*BoltzmannTester boltzmannTester;
    Triangle* fixed = *vertices[0]->getTriangles().begin();
    moveFactory->setFixedTriangle(fixed);
    std::vector<int> id = createID(fixed);*/

    //std::ofstream ratio("tri_ratio.dat"); // TODO: make observable
    //std::ofstream lambda_measure("lambda.dat");

    if (drift) {
        volume = volumeStart;
    }

    for (unsigned long sweep = 0; sweep < numSweeps; sweep++) {
        if (drift && sweep % skip == 0) {
            volume += (volumeEnd - volumeStart) / (numSweeps / skip);
        }

        /*
        if (sweep % 10 == 0) { // for testing
            boltzmannTester.printFrequencies(lambda, alpha);
        }*/

        // for testing
        //ratio << TTSCount / (double) (SSTCount + TTSCount) << " " << TTSCount << " " << SSTCount << " "
        //        << 2 * vertices.size() << std::endl;

        measured = false;
        for (unsigned int i = 0; i < sweepLength; i++) {
            /* Measure observables when the volume is right */
            if (((!measureAtVolume && i == 0) ||
                    (measureAtVolume && vertices.size() * 2 == volume && !measured
                    && i > 0.4 * sweepLength && i < 0.6 * sweepLength)) &&
                    ((drift && (sweep % skip) > drop) || (!drift && sweep > drop))) {

                foreach(Observable* o, observables) {
                    o->measure(vertices);
                }

                measured = true;
            }

            Move* move = moveFactory->createRandomMove(*this);

            // some random moves can be impossible and to simplify the
            // probability checks, we can do this explicit check
            if (!move->isMovePossible(vertices)) {
                //boltzmannTester.addStateId(id);
                moveFactory->setMoveImpossible();
                moveRejectedBecauseImpossible++;
                continue;
            }

            /* acceptance = P(x') / P(x) * Q(x | x') / Q(x' | x) */
            double acceptance = move->getMoveProbability(lambda, alpha) *
                    move->getInverseTransitionProbability(vertices) /
                    move->getTransitionProbability(vertices);

            /* add quadratic volume fixing term */
            double delta = move->getDeltaSST() + move->getDeltaTTS();
            acceptance *= exp(-deltaVolume * delta * (4.0 *
                    (double) vertices.size() + delta - 2.0 * (double) volume));

            if (acceptance > 1 || getRandomNumber() < acceptance) {
                move->execute(vertices);
                moveFactory->setMoveAccepted();

                TTSCount += move->getDeltaTTS();
                SSTCount += move->getDeltaSST();
                //id = createID(fixed);
            } else {
                moveRejectedBecauseDetBal++;
                moveFactory->setMoveRejected();
            }

            bias += 2.0 * (double) vertices.size() - (double) volume;
            //boltzmannTester.addStateId(id);
        }

        /* Update lambda. Lower lambda means more growth, so the signs are
         * reversed. */
        bias *= z / (double) sweepLength / (double) volume;
        lambda += k * (bias * bias * bias + bias);
        lambda = lambda < 0 ? 0 : lambda;

        std::cout << "Real run " << sweep << ", lambda: " << lambda << ", delta: " << k * (bias * bias * bias + bias)
                << ", bias: " << bias / z * 100 << "%" << std::endl;
        //lambda_measure << lambda << std::endl;
        bias = 0;
    }

    /* Write some statistics*/
    std::cout << "Rejected impossible: " << moveRejectedBecauseImpossible
            << ", " << 100 * moveRejectedBecauseImpossible /
            ((float) sweepLength * (float) numSweeps) << "%" << std::endl;
    std::cout << "Rejected detailed balance: " << moveRejectedBecauseDetBal
            << ", " << 100 * moveRejectedBecauseDetBal /
            ((float) sweepLength * (float) numSweeps - (float) moveRejectedBecauseImpossible)
            << "%" << std::endl;
};


// Cannot "add" boost units quantities when they are "absolute"!

#include <iostream>  // for std::cout

#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>  // for quantities to be streamed to std::cout

// #include <boost/units/systems/si.hpp>
// #include <boost/units/systems/si/prefixes.hpp>
// #include <boost/units/systems/si/temperature.hpp>
#include <boost/units/systems/temperature/celsius.hpp>

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

/// Helper unit constants.
// vvv FAILS vvv
// static const auto celsius = boost::units::absolute<boost::units::celsius::temperature>();
// vvv WORKS vvv
static const auto celsius = boost::units::celsius::temperature();

BOOST_AUTO_TEST_CASE(temperatureAddition)
{
    auto temperature0 = 20.0 * celsius;
    auto temperature1 = 1.0 * celsius;
    auto temperature2 = temperature0 + temperature1;
    auto temperature3 = 21.0 * celsius;

    std::cout << "temperature2: " << temperature2 << std::endl;

    // vvv FAILS vvv
    // BOOST_CHECK_CLOSE(temperature2, temperature3, 1.0e-8);
    // vvv WORKS vvv
    BOOST_CHECK_CLOSE(temperature2 / celsius, temperature3 / celsius, 1.0e-4);

    // test increment
    auto temperature5 = temperature0 + 1 * celsius;
    BOOST_CHECK_CLOSE(temperature5 / celsius, 21.0, 1.0e-4);

    // test increment in loop
    double temperature_double = temperature0 / celsius;
    for (auto temperature = temperature0; temperature < 30.0 * celsius; temperature += 1.0 * celsius)
    {
        BOOST_CHECK_CLOSE(temperature / celsius, temperature_double, 1.0e-4);
        temperature_double += 1.0;
    }
}


#include <boost/geometry/algorithms/within.hpp>


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file all_long_tests.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-11-18
 */

#define BOOST_TEST_MODULE all_long_tests

#include "paal/lp/glp.hpp"

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <iomanip>

namespace {
struct Config {
    Config() { std::cout << std::setprecision(20); }
    ~Config() { paal::lp::glp::free_env(); }
};
}

BOOST_GLOBAL_FIXTURE(Config);


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_PERMUTATION_INCLUDE
#define MTL_MATRIX_PERMUTATION_INCLUDE

#include <boost/numeric/mtl/operation/size.hpp>
#include <boost/numeric/mtl/matrix/reorder.hpp>

namespace mtl { namespace mat {

namespace traits {

    //\ Return type of mtl::mat::permutation
    // Only for completeness
    template <typename Value= short>
    struct permutation
    {
	typedef typename reorder<Value>::type  type;
    };
}

template <typename Value, typename PermutationVector>
typename traits::permutation<Value>::type
inline permutation(const PermutationVector& v)
{
    using mtl::size;
    return reorder(v, size(v));
}

/// Computes permutation matrix from corresponding vector
template <typename PermutationVector>
typename traits::permutation<>::type
inline permutation(const PermutationVector& v)
{
    return permutation<short>(v);
}

}} // namespace mtl::matrix

namespace mtl { namespace vec {

    /// Import into vector namespace; see \ref mtl::mat::permutation
    using mtl::mat::permutation;

}} // namespace mtl::vector

#endif // MTL_MATRIX_PERMUTATION_INCLUDE


// ---------------------------------------------------------------------
//
// Copyright (c) 2015 - 2020 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

/////////////////////////////// INCLUDES /////////////////////////////////////

#include "ibtk/FECache.h"
#include "ibtk/FEDataInterpolation.h"
#include "ibtk/libmesh_utilities.h"

#include "libmesh/compare_types.h"
#include "libmesh/dof_map.h"
#include "libmesh/equation_systems.h"
#include "libmesh/quadrature.h"
#include "libmesh/system.h"
#include "libmesh/type_vector.h"

#include "ibtk/namespaces.h"

IBTK_DISABLE_EXTRA_WARNINGS
#include <boost/multi_array.hpp>
IBTK_ENABLE_EXTRA_WARNINGS

#include <algorithm>
#include <iterator>
#include <set>

namespace libMesh
{
class Elem;
class Point;
template <typename T>
class NumericVector;
} // namespace libMesh

/////////////////////////////// NAMESPACE ////////////////////////////////////

namespace IBTK
{
/////////////////////////////// STATIC ///////////////////////////////////////

/////////////////////////////// PUBLIC ///////////////////////////////////////

FEDataInterpolation::FEDataInterpolation(const unsigned int dim, std::shared_ptr<FEData> fe_data)
    : d_dim(dim), d_fe_data(std::move(fe_data))
{
    return;
}

void
FEDataInterpolation::registerSystem(const System& system,
                                    const std::vector<int>& phi_vars,
                                    const std::vector<int>& dphi_vars)
{
    TBOX_ASSERT(!d_initialized && (!phi_vars.empty() || !dphi_vars.empty()));
    const unsigned int sys_num = system.number();
    for (auto noninterp_system : d_noninterp_systems)
    {
        if (noninterp_system->number() == sys_num)
        {
            // This system has already been registered.  If so, just merge the collection of variables.
            const size_t system_idx =
                std::distance(d_noninterp_systems.begin(),
                              std::find(d_noninterp_systems.begin(), d_noninterp_systems.end(), noninterp_system));

            std::vector<int>& orig_all_vars = d_noninterp_system_all_vars[system_idx];
            std::set<int> all_vars_set(orig_all_vars.begin(), orig_all_vars.end());
            all_vars_set.insert(phi_vars.begin(), phi_vars.end());
            all_vars_set.insert(dphi_vars.begin(), dphi_vars.end());
            d_noninterp_system_all_vars[system_idx].assign(all_vars_set.begin(), all_vars_set.end());

            std::vector<int>& orig_phi_vars = d_noninterp_system_phi_vars[system_idx];
            std::set<int> phi_vars_set(orig_phi_vars.begin(), orig_phi_vars.end());
            phi_vars_set.insert(phi_vars.begin(), phi_vars.end());
            d_noninterp_system_phi_vars[system_idx].assign(phi_vars_set.begin(), phi_vars_set.end());

            std::vector<int>& orig_dphi_vars = d_noninterp_system_dphi_vars[system_idx];
            std::set<int> dphi_vars_set(orig_dphi_vars.begin(), orig_dphi_vars.end());
            dphi_vars_set.insert(dphi_vars.begin(), dphi_vars.end());
            d_noninterp_system_dphi_vars[system_idx].assign(dphi_vars_set.begin(), dphi_vars_set.end());
            return;
        }
    }

    // We have not previously registered the system, so we need to register it here.
    d_noninterp_systems.push_back(&system);

    std::set<int> all_vars_set;
    all_vars_set.insert(phi_vars.begin(), phi_vars.end());
    all_vars_set.insert(dphi_vars.begin(), dphi_vars.end());
    d_noninterp_system_all_vars.push_back(std::vector<int>(all_vars_set.begin(), all_vars_set.end()));

    std::set<int> phi_vars_set(phi_vars.begin(), phi_vars.end());
    d_noninterp_system_phi_vars.push_back(std::vector<int>(phi_vars_set.begin(), phi_vars_set.end()));

    std::set<int> dphi_vars_set(dphi_vars.begin(), dphi_vars.end());
    d_noninterp_system_dphi_vars.push_back(std::vector<int>(dphi_vars_set.begin(), dphi_vars_set.end()));
    return;
}

size_t
FEDataInterpolation::registerInterpolatedSystem(const System& system,
                                                const std::vector<int>& vars,
                                                const std::vector<int>& grad_vars,
                                                NumericVector<double>* system_vec)
{
    TBOX_ASSERT(!d_initialized && (!vars.empty() || !grad_vars.empty()));
    const unsigned int sys_num = system.number();
    for (auto system : d_systems)
    {
        if (system->number() == sys_num)
        {
            // This system has already been registered.  Check to see if the same collection of variables (etc.)
            // were used in the previous registration action; if so, do not re-register the system.
            //
            // NOTE: The same system may be registered multiple times with different collections of variables,
            // gradient variables, system data, etc.  We want to make sure we check all of the registered systems to
            // see if any of them match the function arguments.
            const size_t system_idx =
                std::distance(d_systems.begin(), std::find(d_systems.begin(), d_systems.end(), system));
            bool same_data = true;
            same_data = same_data && (vars == d_system_vars[system_idx]);
            same_data = same_data && (grad_vars == d_system_grad_vars[system_idx]);
            same_data = same_data && (system_vec == d_system_vecs[system_idx]);
            if (same_data) return system_idx;
        }
    }

    // Either we have not previously registered this system, or we are registering it here with a different
    // collection of variables/data.  In either case, we need to register it here.
    const size_t system_idx = d_systems.size();
    d_systems.push_back(&system);
    d_system_dof_map_caches.push_back(d_fe_data->getDofMapCache(system.name()));
    std::set<int> all_vars_set;
    all_vars_set.insert(vars.begin(), vars.end());
    all_vars_set.insert(grad_vars.begin(), grad_vars.end());
    std::vector<int> all_vars(all_vars_set.begin(), all_vars_set.end());
    d_system_all_vars.push_back(all_vars);
    d_system_vars.push_back(vars);
    std::vector<size_t> var_idx(vars.size());
    for (size_t k = 0; k < vars.size(); ++k)
    {
        var_idx[k] = std::distance(all_vars.begin(), std::find(all_vars.begin(), all_vars.end(), vars[k]));
    }
    d_system_var_idx.push_back(var_idx);
    d_system_grad_vars.push_back(grad_vars);
    std::vector<size_t> grad_var_idx(grad_vars.size());
    for (size_t k = 0; k < grad_vars.size(); ++k)
    {
        grad_var_idx[k] = std::distance(all_vars.begin(), std::find(all_vars.begin(), all_vars.end(), grad_vars[k]));
    }
    d_system_grad_var_idx.push_back(grad_var_idx);
    d_system_vecs.push_back(system_vec);
    return system_idx;
}

void
FEDataInterpolation::setupInterpolatedSystemDataIndexes(std::vector<size_t>& system_idxs,
                                                        const std::vector<SystemData>& system_data,
                                                        const EquationSystems* const equation_systems)
{
    TBOX_ASSERT(!d_initialized);
    const size_t n_systems = system_data.size();
    system_idxs.resize(n_systems);
    for (unsigned int k = 0; k < system_idxs.size(); ++k)
    {
        const System& system = equation_systems->get_system(system_data[k].system_name);
        const std::vector<int>& vars = system_data[k].vars;
        const std::vector<int>& grad_vars = system_data[k].grad_vars;
        NumericVector<double>* const system_vec = system_data[k].system_vec;
        system_idxs[k] = registerInterpolatedSystem(system, vars, grad_vars, system_vec);
    }
    return;
}

void
FEDataInterpolation::setInterpolatedDataPointers(std::vector<const std::vector<double>*>& var_data,
                                                 std::vector<const std::vector<VectorValue<double> >*>& grad_var_data,
                                                 const std::vector<size_t>& system_idxs,
                                                 const Elem* const elem,
                                                 const unsigned int qp)
{
    TBOX_ASSERT(d_initialized);
    TBOX_ASSERT(elem == d_current_elem);
    TBOX_ASSERT(qp < d_n_qp);
    var_data.resize(system_idxs.size());
    grad_var_data.resize(system_idxs.size());
    for (unsigned k = 0; k < system_idxs.size(); ++k)
    {
        var_data[k] = &d_system_var_data[qp][system_idxs[k]];
        grad_var_data[k] = &d_system_grad_var_data[qp][system_idxs[k]];
    }
    return;
}

void
FEDataInterpolation::init()
{
    TBOX_ASSERT(!d_initialized);

    // Collect the distinct FETypes to be used.
    std::set<FEType> fe_type_set;
    const size_t num_systems = d_systems.size();
    for (size_t system_idx = 0; system_idx < num_systems; ++system_idx)
    {
        const System& system = *d_systems[system_idx];
        const DofMap& system_dof_map = system.get_dof_map();
        const std::vector<int>& all_vars = d_system_all_vars[system_idx];
        for (const auto& var : all_vars)
        {
            fe_type_set.insert(system_dof_map.variable_type(var));
        }
    }
    const size_t num_noninterp_systems = d_noninterp_systems.size();
    for (size_t system_idx = 0; system_idx < num_noninterp_systems; ++system_idx)
    {
        const System& system = *d_noninterp_systems[system_idx];
        const DofMap& system_dof_map = system.get_dof_map();
        const std::vector<int>& all_vars = d_noninterp_system_all_vars[system_idx];
        for (const auto& var : all_vars)
        {
            fe_type_set.insert(system_dof_map.variable_type(var));
        }
    }
    d_fe_types.assign(fe_type_set.begin(), fe_type_set.end());

    // Determine which FETypes are used for which variables, and which shape functions and shape function
    // derivatives need to be computed.
    const size_t num_fe_types = d_fe_types.size();
    d_eval_phi.resize(num_fe_types, false);
    d_eval_dphi.resize(num_fe_types, false);
    d_system_var_fe_type_idx.resize(num_systems);
    d_system_grad_var_fe_type_idx.resize(num_systems);
    for (size_t system_idx = 0; system_idx < num_systems; ++system_idx)
    {
        const System& system = *d_systems[system_idx];
        const DofMap& system_dof_map = system.get_dof_map();

        NumericVector<double>*& system_vec = d_system_vecs[system_idx];
        if (!system_vec) system_vec = system.current_local_solution.get();

        const std::vector<int>& vars = d_system_vars[system_idx];
        d_system_var_fe_type_idx[system_idx].resize(vars.size(), -1);
        for (unsigned int k = 0; k < vars.size(); ++k)
        {
            const FEType& fe_type = system_dof_map.variable_type(vars[k]);
            const size_t fe_type_idx = getFETypeIndex(fe_type);
            d_eval_phi[fe_type_idx] = true;
            d_system_var_fe_type_idx[system_idx][k] = fe_type_idx;
        }

        const std::vector<int>& grad_vars = d_system_grad_vars[system_idx];
        d_system_grad_var_fe_type_idx[system_idx].resize(grad_vars.size(), -1);
        for (unsigned int k = 0; k < grad_vars.size(); ++k)
        {
            const FEType& fe_type = system_dof_map.variable_type(grad_vars[k]);
            const size_t fe_type_idx = getFETypeIndex(fe_type);
            d_eval_dphi[fe_type_idx] = true;
            d_system_grad_var_fe_type_idx[system_idx][k] = fe_type_idx;
        }
    }
    for (size_t system_idx = 0; system_idx < num_noninterp_systems; ++system_idx)
    {
        const System& system = *d_noninterp_systems[system_idx];
        const DofMap& system_dof_map = system.get_dof_map();

        const std::vector<int>& phi_vars = d_noninterp_system_phi_vars[system_idx];
        for (const auto& phi_var : phi_vars)
        {
            const FEType& fe_type = system_dof_map.variable_type(phi_var);
            const size_t fe_type_idx = getFETypeIndex(fe_type);
            d_eval_phi[fe_type_idx] = true;
        }

        const std::vector<int>& dphi_vars = d_noninterp_system_dphi_vars[system_idx];
        for (const auto& dphi_var : dphi_vars)
        {
            const FEType& fe_type = system_dof_map.variable_type(dphi_var);
            const size_t fe_type_idx = getFETypeIndex(fe_type);
            d_eval_dphi[fe_type_idx] = true;
        }
    }

    // Set up FE objects and request access to shape functions / gradients, as needed.
    d_fe.resize(num_fe_types);
    d_fe_face.resize(num_fe_types);
    d_phi.resize(num_fe_types, nullptr);
    d_dphi.resize(num_fe_types, nullptr);
    d_phi_face.resize(num_fe_types, nullptr);
    d_dphi_face.resize(num_fe_types, nullptr);
    for (unsigned int fe_type_idx = 0; fe_type_idx < num_fe_types; ++fe_type_idx)
    {
        std::unique_ptr<IBTK::FEValuesBase>& fe = d_fe[fe_type_idx];
        if (!fe)
        {
            FEUpdateFlags update_flags = FEUpdateFlags::update_default;
            if (d_eval_q_point) update_flags |= update_quadrature_points;
            if (d_eval_JxW) update_flags |= update_JxW;
            if (d_eval_phi[fe_type_idx]) update_flags |= update_phi;
            if (d_eval_dphi[fe_type_idx]) update_flags |= update_dphi;

            fe = FEValuesBase::build(d_dim, NDIM, d_qrule, d_fe_types[fe_type_idx], update_flags);

            if (d_eval_q_point && !d_q_point) d_q_point = &fe->getQuadraturePoints();
            if (d_eval_JxW && !d_JxW) d_JxW = &fe->getJxW();
            if (d_eval_phi[fe_type_idx]) d_phi[fe_type_idx] = &fe->getShapeValues();
            if (d_eval_dphi[fe_type_idx]) d_dphi[fe_type_idx] = &fe->getShapeGradients();
        }

        std::unique_ptr<FEBase>& fe_face = d_fe_face[fe_type_idx];
        if (!fe_face)
        {
            const FEType& fe_type = d_fe_types[fe_type_idx];
            fe_face = FEBase::build(d_dim, fe_type);
            if (d_qrule_face) fe_face->attach_quadrature_rule(d_qrule_face);
            if (d_eval_q_point_face && !d_q_point_face) d_q_point_face = &fe_face->get_xyz();
            if (d_eval_JxW_face && !d_JxW_face) d_JxW_face = &fe_face->get_JxW();
            if (d_eval_normal_face && !d_normal_face) d_normal_face = &fe_face->get_normals();
            if (d_eval_phi[fe_type_idx]) d_phi_face[fe_type_idx] = &fe_face->get_phi();
            if (d_eval_dphi[fe_type_idx]) d_dphi_face[fe_type_idx] = &fe_face->get_dphi();
        }
    }

    // Indicate that we have initialized the class.
    d_initialized = true;
    return;
}

void
FEDataInterpolation::reinit(const Elem* elem,
                            const std::vector<libMesh::Point>* const points,
                            const std::vector<double>* weights)
{
    TBOX_ASSERT(d_initialized);
    d_current_elem = elem;
    if (d_qrule || points)
    {
        for (const auto& fe : d_fe)
        {
            TBOX_ASSERT(points == nullptr);
            TBOX_ASSERT(weights == nullptr);
            fe->reinit(elem);
        }
    }
    d_n_qp = static_cast<unsigned int>(points ? points->size() : d_qrule ? d_qrule->n_points() : 0);
    return;
}

void
FEDataInterpolation::reinit(const Elem* const elem,
                            const unsigned int side,
                            const double tol,
                            const std::vector<libMesh::Point>* const points,
                            const std::vector<double>* weights)
{
    TBOX_ASSERT(d_initialized);
    d_current_elem = elem;
    d_current_side = side;
    if (d_qrule_face || points)
    {
        for (const auto& fe_face : d_fe_face)
        {
            fe_face->reinit(elem, side, tol, points, weights);
        }
    }
    d_n_qp = static_cast<unsigned int>(points ? points->size() : d_qrule_face ? d_qrule_face->n_points() : 0);
    return;
}

void
FEDataInterpolation::collectDataForInterpolation(const Elem* const elem)
{
    TBOX_ASSERT(d_initialized);
    TBOX_ASSERT(elem == d_current_elem);

    // Collect local DOF data for the element.
    const size_t num_systems = d_systems.size();
    d_system_elem_data.resize(num_systems);
    for (size_t system_idx = 0; system_idx < num_systems; ++system_idx)
    {
        FEDataManager::SystemDofMapCache* system_dof_map_cache = d_system_dof_map_caches[system_idx];
        // Get the DOF mappings and local data for all variables.
        NumericVector<double>* system_vec = d_system_vecs[system_idx];
        const auto& dof_indices = system_dof_map_cache->dof_indices(d_current_elem);
        boost::multi_array<double, 2>& elem_data = d_system_elem_data[system_idx];
        get_values_for_interpolation(elem_data, *system_vec, dof_indices);
    }
    return;
}

const boost::multi_array<double, 2>&
FEDataInterpolation::getElemData(const Elem* const elem, const size_t system_idx)
{
    TBOX_ASSERT(d_initialized);
    TBOX_ASSERT(elem == d_current_elem);
    TBOX_ASSERT(system_idx < d_systems.size());
    return d_system_elem_data[system_idx];
}

void
FEDataInterpolation::interpolate(const Elem* const elem)
{
    TBOX_ASSERT(d_initialized);
    TBOX_ASSERT(elem == d_current_elem);
    interpolateCommon(d_system_var_data, d_system_grad_var_data, d_phi, d_dphi);
    return;
}

void
FEDataInterpolation::interpolate(const Elem* const elem, const unsigned int side)
{
    TBOX_ASSERT(d_initialized);
    TBOX_ASSERT(elem == d_current_elem);
    TBOX_ASSERT(side == d_current_side);
    interpolateCommon(d_system_var_data, d_system_grad_var_data, d_phi_face, d_dphi_face);
    return;
}

/////////////////////////////// PRIVATE //////////////////////////////////////

size_t
FEDataInterpolation::getFETypeIndex(const FEType& fe_type) const
{
    return std::distance(d_fe_types.begin(), std::find(d_fe_types.begin(), d_fe_types.end(), fe_type));
}

void
FEDataInterpolation::interpolateCommon(
    std::vector<std::vector<std::vector<double> > >& system_var_data,
    std::vector<std::vector<std::vector<VectorValue<double> > > >& system_grad_var_data,
    const std::vector<const std::vector<std::vector<double> >*>& phi_data,
    const std::vector<const std::vector<std::vector<VectorValue<double> > >*>& dphi_data)
{
    // Determine the number of quadrature points where the interpolation will be evaluated.
    const unsigned int n_qp = d_n_qp;
    const size_t n_systems = d_systems.size();
    system_var_data.resize(n_qp, std::vector<std::vector<double> >(n_systems));
    system_grad_var_data.resize(n_qp, std::vector<std::vector<VectorValue<double> > >(n_systems));

    // Interpolate data for each system.
    for (size_t system_idx = 0; system_idx < n_systems; ++system_idx)
    {
        const boost::multi_array<double, 2>& elem_data = d_system_elem_data[system_idx];

        // Interpolate regular variables.
        {
            const std::vector<size_t>& var_idxs = d_system_var_idx[system_idx];
            const auto n_vars = static_cast<unsigned int>(var_idxs.size());
            for (unsigned int qp = 0; qp < n_qp; ++qp)
            {
                system_var_data[qp][system_idx].resize(n_vars);
            }
            const std::vector<size_t>& var_fe_type_idxs = d_system_var_fe_type_idx[system_idx];
            for (unsigned int k = 0; k < n_vars; ++k)
            {
                const size_t var_idx = var_idxs[k];
                const size_t fe_type_idx = var_fe_type_idxs[k];
                const std::vector<std::vector<double> >& phi = *phi_data[fe_type_idx];
                for (unsigned int qp = 0; qp < n_qp; ++qp)
                {
                    system_var_data[qp][system_idx][k] = 0.0;
                    for (unsigned int i = 0; i < phi.size(); ++i)
                    {
                        system_var_data[qp][system_idx][k] += elem_data[i][var_idx] * phi[i][qp];
                    }
                }
            }
        }

        // Interpolate gradient variables.
        {
            const std::vector<size_t>& grad_var_idxs = d_system_grad_var_idx[system_idx];
            const auto n_grad_vars = static_cast<unsigned int>(grad_var_idxs.size());
            for (unsigned int qp = 0; qp < n_qp; ++qp)
            {
                system_grad_var_data[qp][system_idx].resize(n_grad_vars);
            }
            const std::vector<size_t>& grad_var_fe_type_idxs = d_system_grad_var_fe_type_idx[system_idx];
            for (unsigned int k = 0; k < n_grad_vars; ++k)
            {
                const size_t var_idx = grad_var_idxs[k];
                const size_t fe_type_idx = grad_var_fe_type_idxs[k];
                const std::vector<std::vector<VectorValue<double> > >& dphi = *dphi_data[fe_type_idx];
                for (unsigned int qp = 0; qp < n_qp; ++qp)
                {
                    system_grad_var_data[qp][system_idx][k].zero();
                    for (unsigned int i = 0; i < dphi.size(); ++i)
                    {
                        system_grad_var_data[qp][system_idx][k] += elem_data[i][var_idx] * dphi[i][qp];
                    }
                }
            }
        }
    }
    return;
}

/////////////////////////////// NAMESPACE ////////////////////////////////////

} // namespace IBTK

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


#include "imagewindowmapper.h"
//#include <boost/math/special_functions/round.hpp>
#include "rectcalc.h"
#include "Geometry/Matrix33.h"
#include "Geometry/Angle.h"
#include "assert.h"
#define FLOAT_ZERO 1.0e-30
//using boost::math::lround;

/*int round(double val)
{
    return (val > 0.0) ? floor(val + 0.5) : ceil(val - 0.5);
}*/
using namespace Geometry;
ImageWindowMapper::ImageWindowMapper()
:m_sizePane (0, 0)
, m_sizeImage(0, 0)
, m_sizeWnd(0, 0)
, _window_offset(0, 0)
{
    m_dAngle = 0.0;
    m_bVFlip = false;
    m_bHFlip = false;
    m_dZoom = 1.0;
    m_dFovX = 0.0;
    m_dFovY = 0.0;
    m_bUpdated = false;
}

ImageWindowMapper::~ImageWindowMapper()
{
}

bool ImageWindowMapper::UpdateMapper()
{
    if (m_bUpdated)
    {
        return true;
    }
    if (m_sizeImage.width() <= 0 || m_sizeImage.height() <= 0 ||
        m_dFovX < FLOAT_ZERO || m_dFovY < FLOAT_ZERO ||
        m_sizeWnd.width() <= 0 || m_sizeWnd.height() <= 0)
    {
        CMatrix33 identity;
        _voxel_to_window = identity;
        _window_to_voxel = identity;
        _physical_to_window = identity;
        _window_to_physical = identity;
        _voxel_to_physical = identity;
        _physical_to_voxel = identity;

        m_bUpdated = true;
        return false;
    }
    _voxel_to_window = CalcVoxelToWindow();
    _voxel_to_window.GetInverse(_window_to_voxel);

    CMatrix33 physical_to_voxel = CalcPhysicalToVoxel();

    _physical_to_window = _voxel_to_window * physical_to_voxel;
    _physical_to_window.GetInverse(_window_to_physical);

    _voxel_to_physical = _window_to_physical * _voxel_to_window;
    _physical_to_voxel = _window_to_voxel * _physical_to_window;

    m_bUpdated = true;
    return true;
}

QPoint ImageWindowMapper::WindowToVoxel(const QPoint& ptWindow) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    Point2D p = _window_to_voxel * Point2D(ptWindow.x(), ptWindow.y());

  //  return QPoint(lround(p.x), lround(p.y));
    return QPoint(p.x, p.y);
}

/*int int_round(double v)
{
    try
    {
        return boost::math::iround(v);
    }
    catch (std::exception& e)
    {
        LOG_ERROR(CString(e.what()));
        if (v < 0)
        {
            return INT_MIN;
        }
        else
        {
            return INT_MAX;
        }
    }
}*/

QPoint ImageWindowMapper::PhysicalToWindow(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    Point2D p = _physical_to_window * ptPhysical;

    //return QPoint(int_round(p.x) - _window_offset.cx, int_round(p.y) - _window_offset.cy);
    return QPoint(p.x - _window_offset.width(), p.y - _window_offset.height());
}

QSize ImageWindowMapper::PhysicalToWindow(const Vector2D& size_physical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    auto size = _physical_to_window * size_physical;

    //return QSize(int_round(size.x), int_round(size.y));
    return QSize(size.cx, size.cy);
}

Point2D ImageWindowMapper::PhysicalToWindow2D(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    Point2D pt = _physical_to_window * ptPhysical;
    pt.x = pt.x - _window_offset.width();
    pt.y = pt.y - _window_offset.height();
    return pt;
}

Vector2D ImageWindowMapper::PhysicalToWindow2D(const Vector2D& vPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    Vector2D size = _physical_to_window * vPhysical;
    return size;
}

Point2D ImageWindowMapper::WindowToPhysical(const QPoint& ptWindow) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    Point2D pt_window(ptWindow.x() + _window_offset.width(),
                      ptWindow.y() + _window_offset.height());
    Point2D pt = _window_to_physical * pt_window;
    return pt;
}

Vector2D ImageWindowMapper::WindowToPhysical(const QSize& szWindow) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    Vector2D size_window(szWindow.width(), szWindow.height());
    Vector2D size2d = _window_to_physical * size_window;
    return size2d;
}

QPoint ImageWindowMapper::PhysicalToVoxel(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    assert(m_dFovX > FLOAT_ZERO && m_dFovY > FLOAT_ZERO);
    Point2D pt = CalcPhysicalToVoxel() * ptPhysical;
  //  QPoint ptVoxel(boost::math::lround(pt.x), boost::math::lround(pt.y));
    QPoint ptVoxel(pt.x, pt.y);
    return ptVoxel;
}

Point2D ImageWindowMapper::PhysicalToDoubleVoxel(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    assert(m_dFovX > FLOAT_ZERO && m_dFovY > FLOAT_ZERO);
    Point2D pt = CalcPhysicalToVoxel() * ptPhysical;
    return pt;
}

Point2D ImageWindowMapper::VoxelToPhysical(const Point2D& voxel) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    return _voxel_to_physical * voxel;
}

QPoint ImageWindowMapper::VoxelToWindow(const Point2D& voxel_point) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    auto window_point = _voxel_to_window * voxel_point;

   // return QPoint(int_round(window_point.x), int_round(window_point.y));
    return QPoint(window_point.x, window_point.y);
}

void ImageWindowMapper::SetFOV(double dFovX, double dFovY)
{
    if (abs(m_dFovX - dFovX) < 0.000001 && abs(m_dFovY - dFovY) < 0.000001)
    {
        return;
    }
    m_dFovX = dFovX;
    m_dFovY = dFovY;
    m_bUpdated = false;
}

void ImageWindowMapper::SetImageSize(QSize size)
{
    if (m_sizeImage == size)
    {
        return;
    }

    m_sizeImage = size;
    m_bUpdated = false;
}

void ImageWindowMapper::SetWindowSize(QSize size)
{
    if (m_sizeWnd == size)
    {
        return;
    }

    m_sizeWnd = size;
    m_bUpdated = false;
}

void ImageWindowMapper::SetWindowOffset(const QSize& offset)
{
    if (_window_offset == offset)
    {
        return;
    }

    _window_offset = offset;
    m_bUpdated = false;
}

CMatrix33 ImageWindowMapper::CalcVoxelToWindow() const
{
    CMatrix33 rotation;
    rotation.MakeRotate(CRadian(-m_dAngle));

    CMatrix33 flip;
    if (m_bHFlip)
    {
        flip = flip * CMatrix33::FromBasisVectors(Vector2D(-1.0, 0.0), Vector2D(0.0, 1.0));
    }
    if (m_bVFlip)
    {
        flip = flip * CMatrix33::FromBasisVectors(Vector2D(1.0, 0.0), Vector2D(0.0, -1.0));
    }

    CMatrix33 pane;
    pane.MakeTranslate(Vector2D(m_sizePane.x, m_sizePane.y));

    QSize image_rect_size = RectCalc::GetFitSize(m_sizeImage, m_sizeWnd);
    double fit_ratio = image_rect_size.width()/ static_cast<double>(m_sizeImage.width());
    CMatrix33 zoom;
    zoom.MakeScale(Vector2D(m_dZoom * fit_ratio, m_dZoom * fit_ratio));

    CMatrix33 image_center_to_first;
    image_center_to_first.MakeTranslate(Vector2D(-(m_sizeImage.width() - 1) / 2.0, -(m_sizeImage.height() - 1) / 2.0));

    CMatrix33 viweport_first_to_center;
    viweport_first_to_center.MakeTranslate(Vector2D((m_sizeWnd.width() - 1) / 2.0, (m_sizeWnd.height() - 1) / 2.0));

    return viweport_first_to_center * zoom * pane * flip * rotation * image_center_to_first;
}

CMatrix33 ImageWindowMapper::CalcPhysicalToVoxel() const
{
    CMatrix33 scale;
    scale.MakeScale(Vector2D(m_sizeImage.width() / m_dFovX, m_sizeImage.height() / m_dFovY));

    CMatrix33 translate;
    translate.MakeTranslate(Vector2D(m_sizeImage.width() / 2.0, m_sizeImage.height() / 2.0));

    return translate * scale;
}

void ImageWindowMapper::SetRotationAngle( double angle )
{
    if (m_dAngle == angle)
    {
        return;
    }

    m_dAngle = angle;
    m_bUpdated = false;
}

double ImageWindowMapper::GetRotationAngle() const
{
    return m_dAngle;
}

bool ImageWindowMapper::IsVerticalFlip() const
{
    return m_bVFlip;
}

void ImageWindowMapper::SetVerticalFlip( bool flip )
{
    if (m_bVFlip == flip)
    {
        return;
    }

    m_bVFlip = flip;
    m_bUpdated = false;
}

bool ImageWindowMapper::IsHorizontalFlip() const
{
    return m_bHFlip;
}

void ImageWindowMapper::SetHorizontalFlip( bool flip )
{
    if (m_bHFlip == flip)
    {
        return;
    }

    m_bHFlip = flip;
    m_bUpdated = false;
}

void ImageWindowMapper::SetZoom( double dZoom )
{
    if (m_dZoom == dZoom)
    {
        return;
    }

    m_dZoom = dZoom;
    m_bUpdated = false;
}

double ImageWindowMapper::GetZoom() const
{
    return m_dZoom;
}

void ImageWindowMapper::SetPane(const Vector2D& pane)
{
    if (m_sizePane.x == pane.cx && m_sizePane.y == pane.cy)
    {
        return;
    }

    m_sizePane = Point2D(pane.cx, pane.cy);
    m_bUpdated = false;
}

Vector2D ImageWindowMapper::GetPane() const
{
    return Vector2D(m_sizePane.x, m_sizePane.y);
}

void ImageWindowMapper::HorizontalFlip()
{
    m_bHFlip = !m_bHFlip;
    m_sizePane.x *=  -1;
    m_bUpdated = false;
}

void ImageWindowMapper::VerticalFlip()
{
    m_bVFlip = ! m_bVFlip;
    m_sizePane.y *= -1;
    m_bUpdated = false;
}

void ImageWindowMapper::Rotate( double angle )
{
    Vector2D old_pane(m_sizePane.x, m_sizePane.y);
    m_sizePane = Point2D(old_pane.cx * cos(angle) + old_pane.cy * sin(angle),
        -old_pane.cx * sin(angle) + old_pane.cy * cos(angle));

    angle = (m_bHFlip == m_bVFlip) ? angle : - angle;
    m_dAngle += angle;

    m_bUpdated = false;
}

void ImageWindowMapper::Zoom( double multiple )
{
    SetZoom(m_dZoom * multiple);
}

void ImageWindowMapper::Pane( const Vector2D& offset )
{
    if (m_sizeImage.width() == 0 || m_sizeImage.height() == 0)
    {
        return ;
    }

    QSize image_rect_size = RectCalc::GetFitSize(m_sizeImage, m_sizeWnd);
    double effective_zoom = m_sizeImage.width() / (m_dZoom * image_rect_size.width());

    m_sizePane = Point2D(m_sizePane.x + offset.cx * effective_zoom, m_sizePane.y + offset.cy * effective_zoom);
    Point2D max_pane ((m_sizeImage.width() * 4.0) / 5, (m_sizeImage.height() * 4.0) / 5);
    if (m_sizePane.x > max_pane.x)
    {
        m_sizePane.x = max_pane.x;
    }
    else if (m_sizePane.x < -max_pane.x)
    {
        m_sizePane.x = -max_pane.x;
    }
    if (m_sizePane.y > max_pane.y)
    {
        m_sizePane.y = max_pane.y;
    }
    else if (m_sizePane.y < -max_pane.y)
    {
        m_sizePane.y = -max_pane.y;
    }
    m_bUpdated = false;
}

bool ImageWindowMapper::operator==( const ImageWindowMapper& rhs )
{
    return ((fabs(m_dAngle - rhs.m_dAngle) <= 1.0e-5) &&
        m_bHFlip == rhs.m_bHFlip &&
        m_bVFlip == rhs.m_bVFlip &&
        (fabs(m_dZoom - rhs.m_dZoom) <= 1.0e-5) &&
        m_sizePane == rhs.m_sizePane);
}

void ImageWindowMapper::Reset()
{
    m_dZoom = 1.0;
    m_dAngle = 0.0;
    m_sizePane = Point2D(0.0, 0.0);
    m_bHFlip = false;
    m_bVFlip = false;

    m_bUpdated = false;
}


#if GOOGLE_CUDA

#define EIGEN_USE_GPU

#include <stdio.h>
#include <cfloat>
#include <time.h>
#include <thrust/extrema.h>
#include <Eigen/Geometry>
#include <cublas_v2.h>
#include "hough_voting_gpu_op.h"

#define VERTEX_CHANNELS 3
#define MAX_ROI 128

#define CUDA_1D_KERNEL_LOOP(i, n)                            \
  for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < n; \
       i += blockDim.x * gridDim.x)

// namespace tensorflow {
using namespace tensorflow;

__device__ inline float point2line(int cx, int cy, int x, int y, float u, float v)
{
  float n1 = -v;
  float n2 = u;

  return fabs(n1 * (cx - x) + n2 * (cy - y)) / sqrt(n1 * n1 + n2 * n2);
}

__device__ inline float angle_distance(int cx, int cy, int x, int y, float u, float v)
{
  float dx = cx - x;
  float dy = cy - y;
  float n1 = sqrt(u * u + v * v);
  float n2 = sqrt(dx * dx + dy * dy);
  float dot = u * dx + v * dy;
  float distance = dot / (n1 * n2);

  return distance;
}

__device__ inline float angle_distance_label(int cx, int cy, int x, int y, float u, float v,
  int cls, const int height, const int width, const int* labelmap)
{
  float dx = cx - x;
  float dy = cy - y;
  float n1 = sqrt(u * u + v * v);
  float n2 = sqrt(dx * dx + dy * dy);
  float dot = u * dx + v * dy;
  float distance = dot / (n1 * n2);

  int num = 20;
  int count = 0;
  for (int i = 1; i <= num; i++)
  {
    float step = float(i) / float(num);
    int px = int(x + step * dx);
    int py = int(y + step * dy);
    if (px >= 0 && px < width && py >= 0 && py < height)
    {
      if (labelmap[py * width + px] == cls)
        count++;
    }
  }
  if ((float)count / float(num) < 0.8)
    distance = 0;

  return distance;
}

__device__ inline float IoU(float* a, float* b)
{
  float left = fmax(a[0], b[0]), right = fmin(a[2], b[2]);
  float top = fmax(a[1], b[1]), bottom = fmin(a[3], b[3]);
  float width = fmax(right - left + 1, 0.f), height = fmax(bottom - top + 1, 0.f);
  float interS = width * height;
  float Sa = (a[2] - a[0] + 1) * (a[3] - a[1] + 1);
  float Sb = (b[2] - b[0] + 1) * (b[3] - b[1] + 1);
  return interS / (Sa + Sb - interS);
}

__device__ inline void project_box(int cls, const float* extents, const float* meta_data, float distance, float factor, float* threshold)
{
  float xHalf = extents[cls * 3 + 0] * 0.5;
  float yHalf = extents[cls * 3 + 1] * 0.5;
  float zHalf = extents[cls * 3 + 2] * 0.5;
  float bb3D[24];

  bb3D[0] = xHalf; bb3D[1] = yHalf; bb3D[2] = zHalf + distance;
  bb3D[3] = -xHalf; bb3D[4] = yHalf; bb3D[5] = zHalf + distance;
  bb3D[6] = xHalf; bb3D[7] = -yHalf; bb3D[8] = zHalf + distance;
  bb3D[9] = -xHalf; bb3D[10] = -yHalf; bb3D[11] = zHalf + distance;
  bb3D[12] = xHalf; bb3D[13] = yHalf; bb3D[14] = -zHalf + distance;
  bb3D[15] = -xHalf; bb3D[16] = yHalf; bb3D[17] = -zHalf + distance;
  bb3D[18] = xHalf; bb3D[19] = -yHalf; bb3D[20] = -zHalf + distance;
  bb3D[21] = -xHalf; bb3D[22] = -yHalf; bb3D[23] = -zHalf + distance;

  float fx = meta_data[0];
  float fy = meta_data[4];
  float px = meta_data[2];
  float py = meta_data[5];
  float minX = 1e8;
  float maxX = -1e8;
  float minY = 1e8;
  float maxY = -1e8;
  for (int i = 0; i < 8; i++)
  {
    float x = fx * (bb3D[i * 3] / bb3D[i * 3 + 2])  + px;
    float y = fy * (bb3D[i * 3 + 1] / bb3D[i * 3 + 2])  + py;
    minX = fmin(minX, x);
    minY = fmin(minY, y);
    maxX = fmax(maxX, x);
    maxY = fmax(maxY, y);
  }
  float width = maxX - minX + 1;
  float height = maxY - minY + 1;
  *threshold = fmax(width, height) * factor;
}

__device__ inline float compute_box_overlap(int cls, const float* extents, const float* meta_data, const float* pose, float* box)
{
  float xHalf = extents[cls * 3 + 0] * 0.5;
  float yHalf = extents[cls * 3 + 1] * 0.5;
  float zHalf = extents[cls * 3 + 2] * 0.5;

  Eigen::Matrix<float,8,3,Eigen::DontAlign> bb3D;
  bb3D(0, 0) = xHalf; bb3D(0, 1) = yHalf; bb3D(0, 2) = zHalf;
  bb3D(1, 0) = -xHalf; bb3D(1, 1) = yHalf; bb3D(1, 2) = zHalf;
  bb3D(2, 0) = xHalf; bb3D(2, 1) = -yHalf; bb3D(2, 2) = zHalf;
  bb3D(3, 0) = -xHalf; bb3D(3, 1) = -yHalf; bb3D(3, 2) = zHalf;
  bb3D(4, 0) = xHalf; bb3D(4, 1) = yHalf; bb3D(4, 2) = -zHalf;
  bb3D(5, 0) = -xHalf; bb3D(5, 1) = yHalf; bb3D(5, 2) = -zHalf;
  bb3D(6, 0) = xHalf; bb3D(6, 1)= -yHalf; bb3D(6, 2) = -zHalf;
  bb3D(7, 0) = -xHalf; bb3D(7, 1) = -yHalf; bb3D(7, 2) = -zHalf;

  // rotation
  Eigen::Quaternionf quaternion(pose[6], pose[7], pose[8], pose[9]);
  Eigen::Matrix3f rmatrix = quaternion.toRotationMatrix();
  Eigen::Matrix<float,3,8,Eigen::DontAlign> bb3D_new = rmatrix * bb3D.transpose();

  // projection
  float fx = meta_data[0];
  float fy = meta_data[4];
  float px = meta_data[2];
  float py = meta_data[5];
  float x1 = 1e8;
  float x2 = -1e8;
  float y1 = 1e8;
  float y2 = -1e8;
  for (int i = 0; i < 8; i++)
  {
    float X = bb3D_new(0, i) + pose[10];
    float Y = bb3D_new(1, i) + pose[11];
    float Z = bb3D_new(2, i) + pose[12];
    float x = fx * (X / Z)  + px;
    float y = fy * (Y / Z)  + py;
    x1 = fmin(x1, x);
    y1 = fmin(y1, y);
    x2 = fmax(x2, x);
    y2 = fmax(y2, y);
  }

  float box_gt[4];
  box_gt[0] = x1;
  box_gt[1] = y1;
  box_gt[2] = x2;
  box_gt[3] = y2;
  return IoU(box, box_gt);
}

__global__ void compute_arrays_kernel(const int nthreads, const int* labelmap,
    int* arrays, int* array_size, const int height, const int width)
{
  CUDA_1D_KERNEL_LOOP(index, nthreads)
  {
    int cls = labelmap[index];
    if (cls > 0)
    {
      int size = atomicAdd(array_size + cls, 1);
      int offset = cls * height * width + size;
      arrays[offset] = index;
    }
  }
}

/*
__global__ void compute_hough_kernel(const int nthreads, float* hough_space, float* hough_data, const int* labelmap,
    const float* vertmap, const float* extents, const float* meta_data, int* arrays, int* array_size,
    int* class_indexes, const int height, const int width, const int num_classes, const int count, const float inlierThreshold, const int skip_pixels)
{
  CUDA_1D_KERNEL_LOOP(index, nthreads)
  {
    // (cls, cx, cy) is an element in the hough space
    int ind = index / (height * width);
    int cls = class_indexes[ind];
    int n = index % (height * width);
    int cx = n % width;
    int cy = n / width;

    int size = array_size[cls];
    float distance = 0;
    float bb_width = -1;
    float bb_height = -1;
    float threshold;
    for (int i = 0; i < size; i += skip_pixels)
    {
      int offset = cls * height * width + i;
      int location = arrays[offset];
      int x = location % width;
      int y = location / width;

      // read the direction
      offset = VERTEX_CHANNELS * cls + VERTEX_CHANNELS * num_classes * (y * width + x);
      float u = vertmap[offset];
      float v = vertmap[offset + 1];
      float d = exp(vertmap[offset + 2]);

      // vote
      if (angle_distance(cx, cy, x, y, u, v) > inlierThreshold)
      // if (point2line(cx, cy, x, y, u, v) < 1 && angle_distance_label(cx, cy, x, y, u, v, cls, height, width, labelmap) > 0)
      {
        project_box(cls, extents, meta_data, d, 0.6, &threshold);
        float dx = fabsf(x - cx);
        float dy = fabsf(y - cy);
        if (dx < threshold && dy < threshold)
        {
          hough_space[index]++;
          distance += d;
        }
        if (dx > bb_width && dx < threshold && dy < threshold)
          bb_width = dx;
        if (dy > bb_height && dx < threshold && dy < threshold)
          bb_height = dy;
      }
    }

    if (hough_space[index] > 0)
    {
      distance /= hough_space[index];
      int offset = ind * height * width * 3 + 3 * (cy * width + cx);
      hough_data[offset] = distance;
      hough_data[offset + 1] = 2 * bb_height;
      hough_data[offset + 2] = 2 * bb_width;
    }
  }
}

*/

__global__ void compute_hough_kernel(const int nthreads, float* hough_space, float* hough_data, const int* labelmap,
    const float* vertmap, const float* extents, const float* meta_data, int* arrays, int* array_size,
    int* class_indexes, const int height, const int width, const int num_classes, const int count, const float inlierThreshold, const int skip_pixels)
{
  CUDA_1D_KERNEL_LOOP(index, nthreads)
  {
    // (cls, cx, cy) is an element in the hough space
    int ind = index / (height * width);
    int cls = class_indexes[ind];
    int n = index % (height * width);
    int cx = n % width;
    int cy = n / width;
    int size = array_size[cls];
    float distance = 0;
    float threshold;

    for (int i = 0; i < size; i += skip_pixels)
    {
      int offset = cls * height * width + i;
      int location = arrays[offset];
      int x = location % width;
      int y = location / width;

      // read the direction
      offset = VERTEX_CHANNELS * cls + VERTEX_CHANNELS * num_classes * (y * width + x);
      float u = vertmap[offset];
      float v = vertmap[offset + 1];
      float d = exp(vertmap[offset + 2]);

      // vote
      if (angle_distance(cx, cy, x, y, u, v) > inlierThreshold)
      {
        project_box(cls, extents, meta_data, d, 0.6, &threshold);
        float dx = fabsf(x - cx);
        float dy = fabsf(y - cy);
        if (dx < threshold && dy < threshold)
        {
          hough_space[index]++;
          distance += d;
        }
      }
    }

    if (hough_space[index] > 0)
    {
      distance /= hough_space[index];

      float bb_width = -1;
      float bb_height = -1;
      for (int i = 0; i < size; i += skip_pixels)
      {
        int offset = cls * height * width + i;
        int location = arrays[offset];
        int x = location % width;
        int y = location / width;

        // read the direction
        offset = VERTEX_CHANNELS * cls + VERTEX_CHANNELS * num_classes * (y * width + x);
        float u = vertmap[offset];
        float v = vertmap[offset + 1];

        // vote
        if (angle_distance(cx, cy, x, y, u, v) > inlierThreshold)
        {
          project_box(cls, extents, meta_data, distance, 0.6, &threshold);
          float dx = fabsf(x - cx);
          float dy = fabsf(y - cy);
          if (dx > bb_width && dx < threshold && dy < threshold)
            bb_width = dx;
          if (dy > bb_height && dx < threshold && dy < threshold)
            bb_height = dy;
        }
      }

      int offset = ind * height * width * 3 + 3 * (cy * width + cx);
      hough_data[offset] = distance;
      hough_data[offset + 1] = 2 * bb_height;
      hough_data[offset + 2] = 2 * bb_width;
    }
  }
}

__global__ void compute_max_indexes_kernel(const int nthreads, int* max_indexes, int index_size, int* num_max, float* hough_space,
  float* hough_data, int height, int width, float threshold, float perThreshold)
{
  CUDA_1D_KERNEL_LOOP(index, nthreads)
  {
    // (ind, cx, cy) is an element in the hough space
    int ind = index / (height * width);
    int n = index % (height * width);
    int cx = n % width;
    int cy = n / width;
    int kernel_size = 3;

    int offset = ind * height * width * 3 + 3 * (cy * width + cx);
    float bb_height = hough_data[offset + 1];
    float bb_width = hough_data[offset + 2];

    if (hough_space[index] > threshold && bb_height > 0 && bb_width > 0)
    {
      // check if the location is local maximum
      int flag = 0;
      for (int x = cx - kernel_size; x <= cx + kernel_size; x++)
      {
        for (int y = cy - kernel_size; y <= cy + kernel_size; y++)
        {
          if (x >= 0 && x < width && y >= 0 && y < height)
          {
            if (hough_space[ind * height * width + y * width + x] > hough_space[index])
            {
              flag = 1;
              break;
            }
          }
        }

        // check the percentage of voting
        if (hough_space[index] / (bb_height * bb_width) < perThreshold)
          flag = 1;
      }

      if (flag == 0)
      {
        // add the location to max_indexes
        int max_index = atomicAdd(num_max, 1);
        if (max_index < index_size)
          max_indexes[max_index] = index;
      }
    }
  }
}

__global__ void compute_rois_kernel(const int nthreads, float* top_box, float* top_pose, float* top_target, float* top_weight, int* top_domain,
    const float* extents, const float* meta_data, const float* gt, float* hough_space, float* hough_data, int* max_indexes, int* class_indexes,
    int is_train, int batch_index, const int height, const int width, const int num_classes, const int num_gt, int* num_rois)
{
  CUDA_1D_KERNEL_LOOP(index, nthreads)
  {
    float scale = 0.05;
    int max_index = max_indexes[index];
    int ind = max_index / (height * width);
    int cls = class_indexes[ind];
    int n = max_index % (height * width);
    int x = n % width;
    int y = n / width;

    float fx = meta_data[0];
    float fy = meta_data[4];
    float px = meta_data[2];
    float py = meta_data[5];
    float rx = (x - px) / fx;
    float ry = (y - py) / fy;

    int offset = ind * height * width * 3 + 3 * (y * width + x);
    float bb_distance = hough_data[offset];
    float bb_height = hough_data[offset + 1];
    float bb_width = hough_data[offset + 2];

    if (is_train)
    {
      int roi_index = atomicAdd(num_rois, 9);
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x - bb_width * (0.5 + scale);
      top_box[roi_index * 7 + 3] = y - bb_height * (0.5 + scale);
      top_box[roi_index * 7 + 4] = x + bb_width * (0.5 + scale);
      top_box[roi_index * 7 + 5] = y + bb_height * (0.5 + scale);
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      for (int i = 0; i < 9; i++)
      {
        top_pose[(roi_index + i) * 7 + 0] = 1;
        top_pose[(roi_index + i) * 7 + 1] = 0;
        top_pose[(roi_index + i) * 7 + 2] = 0;
        top_pose[(roi_index + i) * 7 + 3] = 0;
        top_pose[(roi_index + i) * 7 + 4] = rx * bb_distance;
        top_pose[(roi_index + i) * 7 + 5] = ry * bb_distance;
        top_pose[(roi_index + i) * 7 + 6] = bb_distance;

        if (num_gt == 0)
          top_domain[roi_index + i] = 1;
        else
          top_domain[roi_index + i] = 0;
      }

      // compute pose target
      for (int i = 0; i < num_gt; i++)
      {
        int gt_batch = int(gt[i * 13 + 0]);
        int gt_id = int(gt[i * 13 + 1]);
        if(cls == gt_id && batch_index == gt_batch)
        {
          int gt_ind = i;

          float overlap = compute_box_overlap(cls, extents, meta_data, gt + gt_ind * 13, top_box + roi_index * 7 + 2);
          if (overlap > 0.2)
          {
            for (int j = 0; j < 9; j++)
            {
              top_target[(roi_index + j) * 4 * num_classes + 4 * cls + 0] = gt[gt_ind * 13 + 6];
              top_target[(roi_index + j) * 4 * num_classes + 4 * cls + 1] = gt[gt_ind * 13 + 7];
              top_target[(roi_index + j) * 4 * num_classes + 4 * cls + 2] = gt[gt_ind * 13 + 8];
              top_target[(roi_index + j) * 4 * num_classes + 4 * cls + 3] = gt[gt_ind * 13 + 9];

              top_weight[(roi_index + j) * 4 * num_classes + 4 * cls + 0] = 1;
              top_weight[(roi_index + j) * 4 * num_classes + 4 * cls + 1] = 1;
              top_weight[(roi_index + j) * 4 * num_classes + 4 * cls + 2] = 1;
              top_weight[(roi_index + j) * 4 * num_classes + 4 * cls + 3] = 1;
            }
            break;
          }
        }
      }

      // add jittering boxes
      float x1 = top_box[roi_index * 7 + 2];
      float y1 = top_box[roi_index * 7 + 3];
      float x2 = top_box[roi_index * 7 + 4];
      float y2 = top_box[roi_index * 7 + 5];
      float ww = x2 - x1;
      float hh = y2 - y1;

      // (-1, -1)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1 - 0.05 * ww;
      top_box[roi_index * 7 + 3] = y1 - 0.05 * hh;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      // (+1, -1)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1 + 0.05 * ww;
      top_box[roi_index * 7 + 3] = y1 - 0.05 * hh;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      // (-1, +1)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1 - 0.05 * ww;
      top_box[roi_index * 7 + 3] = y1 + 0.05 * hh;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      // (+1, +1)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1 + 0.05 * ww;
      top_box[roi_index * 7 + 3] = y1 + 0.05 * hh;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      // (0, -1)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1;
      top_box[roi_index * 7 + 3] = y1 - 0.05 * hh;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      // (-1, 0)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1 - 0.05 * ww;
      top_box[roi_index * 7 + 3] = y1;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      // (0, +1)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1;
      top_box[roi_index * 7 + 3] = y1 + 0.05 * hh;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      // (+1, 0)
      roi_index++;
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x1 + 0.05 * ww;
      top_box[roi_index * 7 + 3] = y1;
      top_box[roi_index * 7 + 4] = top_box[roi_index * 7 + 2] + ww;
      top_box[roi_index * 7 + 5] = top_box[roi_index * 7 + 3] + hh;
      top_box[roi_index * 7 + 6] = hough_space[max_index];
    }
    else
    {
      int roi_index = atomicAdd(num_rois, 1);
      top_box[roi_index * 7 + 0] = batch_index;
      top_box[roi_index * 7 + 1] = cls;
      top_box[roi_index * 7 + 2] = x - bb_width * (0.5 + scale);
      top_box[roi_index * 7 + 3] = y - bb_height * (0.5 + scale);
      top_box[roi_index * 7 + 4] = x + bb_width * (0.5 + scale);
      top_box[roi_index * 7 + 5] = y + bb_height * (0.5 + scale);
      top_box[roi_index * 7 + 6] = hough_space[max_index];

      top_pose[roi_index * 7 + 0] = 1;
      top_pose[roi_index * 7 + 1] = 0;
      top_pose[roi_index * 7 + 2] = 0;
      top_pose[roi_index * 7 + 3] = 0;
      top_pose[roi_index * 7 + 4] = rx * bb_distance;
      top_pose[roi_index * 7 + 5] = ry * bb_distance;
      top_pose[roi_index * 7 + 6] = bb_distance;
    }
  }
}

void reset_outputs(float* top_box, float* top_pose, float* top_target, float* top_weight, int* top_domain, int* num_rois, int num_classes)
{
  int num = MAX_ROI * 9;
  cudaMemset(top_box, 0, num * 7 * sizeof(float));
  cudaMemset(top_pose, 0, num * 7 * sizeof(float));
  cudaMemset(top_target, 0, num * 4 *num_classes * sizeof(float));
  cudaMemset(top_weight, 0, num * 4 * num_classes * sizeof(float));
  cudaMemset(top_domain, 0, num * sizeof(int));
  cudaMemset(num_rois, 0, sizeof(int));
}

void copy_num_rois(int* num_rois, int* num_rois_device)
{
  cudaMemcpy(num_rois, num_rois_device, sizeof(int), cudaMemcpyDeviceToHost);
}

void copy_outputs(float* top_box, float* top_pose, float* top_target, float* top_weight, int* top_domain,
  float* top_box_final, float* top_pose_final, float* top_target_final, float* top_weight_final, int* top_domain_final, int num_classes, int num_rois)
{
  cudaMemcpy(top_box_final, top_box, num_rois * 7 * sizeof(float), cudaMemcpyDeviceToDevice);
  cudaMemcpy(top_pose_final, top_pose, num_rois * 7 * sizeof(float), cudaMemcpyDeviceToDevice);
  cudaMemcpy(top_target_final, top_target, num_rois * 4 * num_classes * sizeof(float), cudaMemcpyDeviceToDevice);
  cudaMemcpy(top_weight_final, top_weight, num_rois * 4 * num_classes * sizeof(float), cudaMemcpyDeviceToDevice);
  cudaMemcpy(top_domain_final, top_domain, num_rois * sizeof(int), cudaMemcpyDeviceToDevice);
}

void set_gradients(float* top_label, float* top_vertex, int batch_size, int height, int width, int num_classes)
{
  cudaMemset(top_label, 0, batch_size * height * width * sizeof(float));
  cudaMemset(top_vertex, 0, batch_size * height * width * 3 * num_classes * sizeof(float));
}

void HoughVotingLaucher(OpKernelContext* context,
    const int* labelmap, const float* vertmap, const float* extents, const float* meta_data, const float* gt,
    const int batch_index, const int batch_size, const int height, const int width, const int num_classes, const int num_gt,
    const int is_train, const float inlierThreshold, const int labelThreshold, const float votingThreshold, const float perThreshold,
    const int skip_pixels,
    float* top_box, float* top_pose, float* top_target, float* top_weight, int* top_domain, int* num_rois, const Eigen::GpuDevice& d)
{
  const int kThreadsPerBlock = 1024;
  int output_size;
  cudaError_t err;

  // step 1: compute a label index array for each class
  int dims[2];
  dims[0] = num_classes;
  dims[1] = height * width;
  TensorShape output_shape_arrays;
  TensorShapeUtils::MakeShape(dims, 2, &output_shape_arrays);
  Tensor arrays_tensor;
  OP_REQUIRES_OK(context, context->allocate_temp(DT_INT32, output_shape_arrays, &arrays_tensor));
  int* arrays = arrays_tensor.flat<int>().data();

  TensorShape output_shape_array_sizes;
  TensorShapeUtils::MakeShape(&num_classes, 1, &output_shape_array_sizes);
  Tensor array_sizes_tensor;
  OP_REQUIRES_OK(context, context->allocate_temp(DT_INT32, output_shape_array_sizes, &array_sizes_tensor));
  int* array_sizes = array_sizes_tensor.flat<int>().data();
  cudaMemset(array_sizes, 0, num_classes * sizeof(int));

  output_size = height * width;
  compute_arrays_kernel<<<(output_size + kThreadsPerBlock - 1) / kThreadsPerBlock,
                       kThreadsPerBlock, 0, d.stream()>>>(
      output_size, labelmap, arrays, array_sizes, height, width);
  cudaThreadSynchronize();

  // compute class indexes
  int* array_sizes_host = (int*)malloc(num_classes * sizeof(int));
  int* class_indexes_host = (int*)malloc(num_classes * sizeof(int));
  cudaMemcpy(array_sizes_host, array_sizes, num_classes * sizeof(int), cudaMemcpyDeviceToHost);
  int count = 0;
  for (int c = 1; c < num_classes; c++)
  {
    if (array_sizes_host[c] > labelThreshold)
    {
      class_indexes_host[count] = c;
      count++;
    }
    // else
    //  printf("class %d with only pixels %d\n", c, array_sizes_host[c]);
  }

  if (count == 0)
  {
    free(array_sizes_host);
    free(class_indexes_host);
    return;
  }

  TensorShape output_shape_class_indexes;
  TensorShapeUtils::MakeShape(&count, 1, &output_shape_class_indexes);
  Tensor class_indexes_tensor;
  OP_REQUIRES_OK(context, context->allocate_temp(DT_INT32, output_shape_class_indexes, &class_indexes_tensor));
  int* class_indexes = class_indexes_tensor.flat<int>().data();
  cudaMemcpy(class_indexes, class_indexes_host, count * sizeof(int), cudaMemcpyHostToDevice);

  err = cudaGetLastError();
  if(cudaSuccess != err)
  {
    fprintf( stderr, "cudaCheckError() failed compute label index: %s\n", cudaGetErrorString( err ) );
    exit( -1 );
  }

  // step 2: compute the hough space
  int hdims[4];
  hdims[0] = count;
  hdims[1] = height;
  hdims[2] = width;
  hdims[3] = 1;
  TensorShape output_shape_hough_space;
  TensorShapeUtils::MakeShape(hdims, 4, &output_shape_hough_space);
  Tensor hough_space_tensor;
  OP_REQUIRES_OK(context, context->allocate_temp(DT_FLOAT, output_shape_hough_space, &hough_space_tensor));
  float* hough_space = hough_space_tensor.flat<float>().data();
  if (cudaMemset(hough_space, 0, count * height * width * sizeof(float)) != cudaSuccess)
    fprintf(stderr, "reset error\n");

  hdims[3] = 3;
  TensorShape output_shape_hough_data;
  TensorShapeUtils::MakeShape(hdims, 4, &output_shape_hough_data);
  Tensor hough_data_tensor;
  OP_REQUIRES_OK(context, context->allocate_temp(DT_FLOAT, output_shape_hough_data, &hough_data_tensor));
  float* hough_data = hough_data_tensor.flat<float>().data();
  if (cudaMemset(hough_data, 0, count * height * width * 3 * sizeof(float)) != cudaSuccess)
    fprintf(stderr, "reset error\n");

  output_size = count * height * width;
  compute_hough_kernel<<<(output_size + kThreadsPerBlock - 1) / kThreadsPerBlock,
                       kThreadsPerBlock, 0, d.stream()>>>(
      output_size, hough_space, hough_data, labelmap, vertmap, extents, meta_data,
      arrays, array_sizes, class_indexes, height, width, num_classes, count, inlierThreshold, skip_pixels);
  cudaThreadSynchronize();

  err = cudaGetLastError();
  if(cudaSuccess != err)
  {
    fprintf( stderr, "cudaCheckError() failed compute hough space: %s\n", cudaGetErrorString( err ) );
    exit( -1 );
  }

  // step 3: find the maximum in hough space
  int dim = 1;
  TensorShape output_shape_num_max;
  TensorShapeUtils::MakeShape(&dim, 1, &output_shape_num_max);
  Tensor num_max_tensor;
  OP_REQUIRES_OK(context, context->allocate_temp(DT_INT32, output_shape_num_max, &num_max_tensor));
  int* num_max = num_max_tensor.flat<int>().data();
  if (cudaMemset(num_max, 0, sizeof(int)) != cudaSuccess)
    fprintf(stderr, "reset error\n");

  int index_size = MAX_ROI / batch_size;
  TensorShape output_shape_max_indexes;
  TensorShapeUtils::MakeShape(&index_size, 1, &output_shape_max_indexes);
  Tensor max_indexes_tensor;
  OP_REQUIRES_OK(context, context->allocate_temp(DT_INT32, output_shape_max_indexes, &max_indexes_tensor));
  int* max_indexes = max_indexes_tensor.flat<int>().data();
  if (cudaMemset(max_indexes, 0, index_size * sizeof(int)) != cudaSuccess)
    fprintf(stderr, "reset error\n");

  if (votingThreshold > 0)
  {
    output_size = count * height * width;
    compute_max_indexes_kernel<<<(output_size + kThreadsPerBlock - 1) / kThreadsPerBlock,
                       kThreadsPerBlock, 0, d.stream()>>>(
      output_size, max_indexes, index_size, num_max, hough_space, hough_data, height, width, votingThreshold, perThreshold);
    cudaThreadSynchronize();
  }
  else
  {
    int* max_indexes_host = (int*)malloc(count * sizeof(int));
    memset(max_indexes_host, 0, count * sizeof(int));
    for (int i = 0; i < count; i++)
    {
      float *hmax = thrust::max_element(thrust::device, hough_space + i * height * width, hough_space + (i+1) * height * width);
      max_indexes_host[i] = hmax - hough_space;
    }
    cudaMemcpy(num_max, &count, sizeof(int), cudaMemcpyHostToDevice);
    cudaMemcpy(max_indexes, max_indexes_host, count * sizeof(int), cudaMemcpyHostToDevice);
    free(max_indexes_host);
  }
  err = cudaGetLastError();
  if(cudaSuccess != err)
  {
    fprintf( stderr, "cudaCheckError() failed compute maximum: %s\n", cudaGetErrorString( err ) );
    exit( -1 );
  }

  // step 4: compute outputs
  int num_max_host;
  cudaMemcpy(&num_max_host, num_max, sizeof(int), cudaMemcpyDeviceToHost);
  if (num_max_host >= index_size)
    num_max_host = index_size;
  // printf("num_max: %d\n", num_max_host);
  if (num_max_host > 0)
  {
    output_size = num_max_host;
    compute_rois_kernel<<<(output_size + kThreadsPerBlock - 1) / kThreadsPerBlock,
                         kThreadsPerBlock, 0, d.stream()>>>(
        output_size, top_box, top_pose, top_target, top_weight, top_domain,
        extents, meta_data, gt, hough_space, hough_data, max_indexes, class_indexes,
        is_train, batch_index, height, width, num_classes, num_gt, num_rois);
    cudaThreadSynchronize();
  }

  // clean up
  free(array_sizes_host);
  free(class_indexes_host);

  err = cudaGetLastError();
  if(cudaSuccess != err)
  {
    fprintf( stderr, "cudaCheckError() failed compute outputs: %s\n", cudaGetErrorString( err ) );
    exit( -1 );
  }
}

// }  // namespace tensorflow

#endif  // GOOGLE_CUDA


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_SHIFT_LEFT_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_SHIFT_LEFT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/operator/functions/shift_left.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/constants/int_splat.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/assert.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( shift_left_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::make_dependent<int64_t,A0>::type int_t;

    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef simd::native<int_t,boost::simd::tag::sse_> gen_type;
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_left sse2: a shift is out of range");
      result_type const
      Mask1 = bitwise_cast<result_type>( boost::simd::integral_constant
                                           < gen_type, int_t(0x00ff00ff00ff00ffll)>()
                                       );
      result_type const
      Mask2 = bitwise_cast<result_type>( boost::simd::integral_constant
                                           < gen_type, int_t(0xff00ff00ff00ff00ll)>()
                                       );
      result_type tmp  = b_and(a0, Mask1);
      result_type tmp1 = _mm_slli_epi16(tmp, int(a1));
      tmp1 = b_and(tmp1, Mask1);
      tmp = b_and(a0, Mask2);
      result_type tmp3 = _mm_slli_epi16(tmp, int(a1));
      return tmp1 | b_and(tmp3, Mask2);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( shift_left_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_left sse2: a shift is out of range");
      return _mm_slli_epi16(a0, int(a1));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( shift_left_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "a shift is out of range");
      return _mm_slli_epi32(a0, int(a1));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( shift_left_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "a shift is out of range");
      return _mm_slli_epi64(a0, int(a1));
    }
  };
} } }

#endif
#endif