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

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

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

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

namespace {

template <typename DataType>
std::vector<DataType> cpp_model(std::vector<DataType> acts, int numK) {
  std::make_heap(acts.begin(), acts.end());
  std::vector<DataType> output(numK);

  for (int i = 0; i < numK; ++i) {
    std::pop_heap(acts.begin(), acts.end());
    output[i] = acts.back();
    acts.pop_back();
  }

  return output;
}

} // namespace

template <typename DataType>
static bool doTest(const DeviceType &deviceType, const Type &activationType,
                   unsigned size, unsigned numK, bool sort) {
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();
  Graph graph(target);
  popnn::addCodelets(graph);

  auto activations = graph.addVariable(activationType, {size}, "activations");
  auto labels = graph.addVariable(UNSIGNED_INT, {size}, "labels");

  auto maxAct = graph.addVariable(activationType, {numK}, "maxValues");
  auto maxValueIndices =
      graph.addVariable(UNSIGNED_INT, {numK}, "maxValuesIndices");

  graph.setTileMapping(activations, 0);
  graph.setTileMapping(maxAct, 0);
  graph.setTileMapping(maxValueIndices, 0);
  graph.setTileMapping(labels, 0);

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  auto rawHostActivations = allocateHostMemoryForTensor(
      activations, "activations", graph, uploadProg, downloadProg, tmap);

  auto rawHostMaxAct = allocateHostMemoryForTensor(
      maxAct, "maxValues", graph, uploadProg, downloadProg, tmap);

  auto rawHostLabels = allocateHostMemoryForTensor(
      labels, "labels", graph, uploadProg, downloadProg, tmap);

  auto rawHostMaxValueIndices =
      allocateHostMemoryForTensor(maxValueIndices, "maxValuesIndices", graph,
                                  uploadProg, downloadProg, tmap);

  std::mt19937 randomEngine;
  std::vector<DataType> hostActivations(size);

  {
    boost::random::uniform_int_distribution<unsigned> randDist(
        std::numeric_limits<unsigned>::lowest(),
        std::numeric_limits<unsigned>::max());
    for (auto &a : hostActivations) {
      unsigned tmp = randDist(randomEngine);
      std::memcpy(&a, &tmp, sizeof(DataType));

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

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

  // Create the index vector for the indices. Should just be sequential 0 to
  // size.
  std::vector<unsigned> hostLabels(size);
  {
    for (unsigned i = 0; i < size; ++i) {
      hostLabels[i] = i;
    }
  }
  copy(target, hostLabels.data(), size, UNSIGNED_INT, rawHostLabels.get());

  auto cs = graph.addComputeSet();

  std::string vertexName =
      templateVertex("popnn::ReduceMaxNClassSparse", activationType, sort);

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

  graph.connect(v["activations"], activations);
  graph.connect(v["maxValuesIndices"], maxValueIndices);
  graph.connect(v["maxValues"], maxAct);
  graph.connect(v["labels"], labels);
  graph.setInitialValue(v["numK"], numK);
  graph.setInitialValue(v["shouldSort"], sort);

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

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

  std::vector<DataType> deviceOut(numK);
  copy(target, activationType, rawHostMaxAct.get(), deviceOut.data(), numK);

  std::vector<unsigned> deviceOutIndices(numK);
  copy(target, UNSIGNED_INT, rawHostMaxValueIndices.get(),
       deviceOutIndices.data(), numK);

  std::vector<DataType> cppOut = cpp_model(hostActivations, numK);
  bool success = true;

  // Check that the indices returned match up to the activations. We do this
  // first so we don't have to deal with the sort later.
  for (unsigned i = 0; i < numK; ++i) {
    int ind = deviceOutIndices[i];

    success &= deviceOut[i] == hostActivations[ind];
  }

  // If we didn't expect the output to be sorted originally we have to sort it
  // as the raw array view of a binary heap is not sorted.
  if (!sort) {
    std::sort(deviceOut.begin(), deviceOut.end(), std::greater<DataType>());
  }

  // Check that it matches the C++ version.
  for (unsigned i = 0; i < numK; ++i) {
    success &= deviceOut[i] == cppOut[i];
  }

  return success;
}

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

  DeviceType deviceType;
  Type activationType;
  unsigned topK;
  unsigned size;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()("help", "Print help")(
      "device-type", po::value<DeviceType>(&deviceType)->required(),
      "Device Type")("size", po::value<unsigned>(&size)->required(),
                     "Total size to process with vertex")(
      "k", po::value<unsigned>(&topK)->required(),
      "Return 'k' number of elements.")(
      "activation-type", po::value<Type>(&activationType)->required(),
      "Element type for input activations");
  // clang-format on

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

  if (activationType == FLOAT) {
    // Check against unsorted.
    if (!doTest<float>(deviceType, activationType, size, topK, false))
      return 1;

    // Check against sorted.
    if (!doTest<float>(deviceType, activationType, size, topK, true))
      return 1;
  } else if (activationType == INT) {
    if (!doTest<int>(deviceType, activationType, size, topK, false) ||
        !doTest<int>(deviceType, activationType, size, topK, true)) {
      return 1;
    }
  } else if (activationType == UNSIGNED_INT) {
    if (!doTest<unsigned>(deviceType, activationType, size, topK, false) ||
        !doTest<unsigned>(deviceType, activationType, size, topK, true)) {
      return 1;
    }
  }
  return 0;
}


#include <unordered_map>

#include <boost/filesystem.hpp>
#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

#include "GraphOptimizer/OptApp.h"
#include "GraphOptimizer/vertigo/vertex_switchLinear.h"
#include "GraphOptimizer/vertigo/edge_switchPrior.h"
#include "GraphOptimizer/vertigo/edge_se3Switchable.h"

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType>
    SlamLinearCSparseSolver;
typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType>
    SlamLinearPCGSolver;
typedef std::unordered_map<int, g2o::HyperGraph::Vertex *> VertexIDMap;
typedef std::pair<int, g2o::HyperGraph::Vertex *> VertexIDPair;
typedef std::set<g2o::HyperGraph::Edge *> EdgeSet;

COptApp::COptApp(void) {}

COptApp::~COptApp(void) {}

bool COptApp::Init() {
    namespace fs = boost::filesystem;
    if (fs::exists(fs::path(odometry_log_file_))) {
        odometry_traj_.LoadFromFile(odometry_log_file_);
        if (fs::exists(fs::path(odometry_info_file_))) {
            odometry_info_.LoadFromFile(odometry_info_file_);
        }
    }
    if (fs::exists(fs::path(loop_log_file_))) {
        loop_traj_.LoadFromFile(loop_log_file_);
        if (fs::exists(fs::path(loop_info_file_))) {
            loop_info_.LoadFromFile(loop_info_file_);
        }
    }
    pose_traj_.data_.clear();
    pose_traj_.data_.push_back(
        FramedTransformation(0, 0, 1, Eigen::Matrix4d::Identity()));
    for (int i = 0; i < (int)odometry_traj_.data_.size(); i++) {
        pose_traj_.data_.push_back(
            FramedTransformation(i + 1, i + 1, i + 2,
                                 pose_traj_.data_[i].transformation_ *
                                     odometry_traj_.data_[i].transformation_));
    }
    return (odometry_traj_.data_.size() > 0);
}

void COptApp::OptimizeSwitchable() {
    struct SwitchableEdge {
      public:
        VertexSwitchLinear *v_;
        EdgeSwitchPrior *ep_;
        EdgeSE3Switchable *e_;
        FramedTransformation *t_;
    };

    g2o::SparseOptimizer *optimizer;
    optimizer = new g2o::SparseOptimizer();
    optimizer->setVerbose(true);
    SlamBlockSolver *solver = NULL;
    SlamLinearCSparseSolver *linearSolver = new SlamLinearCSparseSolver();
    linearSolver->setBlockOrdering(false);
    solver = new SlamBlockSolver(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *algo =
        new g2o::OptimizationAlgorithmLevenberg(solver);
    optimizer->setAlgorithm(algo);

    std::vector<SwitchableEdge> switch_edge;

    Eigen::Matrix<double, 6, 6> default_information;
    default_information = Eigen::Matrix<double, 6, 6>::Identity();

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId(i);
        v->setEstimate(Eigen2G2O(pose_traj_.data_[i].transformation_));
        if (i == 0) {
            v->setFixed(true);
        }
        optimizer->addVertex(v);

        if (i > 0) {
            g2o::EdgeSE3 *g2o_edge = new g2o::EdgeSE3();
            g2o_edge->vertices()[0] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i - 1));
            g2o_edge->vertices()[1] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
            g2o_edge->setMeasurement(g2o::internal::fromSE3Quat(
                Eigen2G2O(odometry_traj_.data_[i - 1].transformation_)));
            if (odometry_info_.data_.size() > 0) {
                g2o_edge->setInformation(
                    odometry_info_.data_[i - 1].information_);
            } else {
                g2o_edge->setInformation(default_information);
            }
            optimizer->addEdge(g2o_edge);
        }
    }

    for (int i = 0; i < (int)loop_traj_.data_.size(); i++) {
        FramedTransformation &t = loop_traj_.data_[i];

        SwitchableEdge edge;
        edge.t_ = &t;

        edge.v_ = new VertexSwitchLinear();
        edge.v_->setId(optimizer->vertices().size());
        edge.v_->setEstimate(1.0);
        optimizer->addVertex(edge.v_);

        edge.ep_ = new EdgeSwitchPrior();
        edge.ep_->vertices()[0] = edge.v_;
        edge.ep_->setMeasurement(1.0);
        edge.ep_->setInformation(Eigen::Matrix<double, 1, 1>::Identity() *
                                 weight_);
        optimizer->addEdge(edge.ep_);

        edge.e_ = new EdgeSE3Switchable();
        edge.e_->vertices()[0] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id1_));
        edge.e_->vertices()[1] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id2_));
        edge.e_->vertices()[2] = edge.v_;
        edge.e_->setMeasurement(
            g2o::internal::fromSE3Quat(Eigen2G2O(t.transformation_)));
        if (loop_info_.data_.size() > 0) {
            edge.e_->setInformation(loop_info_.data_[i].information_);
        } else {
            edge.e_->setInformation(default_information);
        }
        optimizer->addEdge(edge.e_);
        switch_edge.push_back(edge);
    }

    optimizer->initializeOptimization();
    optimizer->optimize(max_iteration_);

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
        pose_traj_.data_[i].transformation_ =
            G2O2Matrix4d(v->estimateAsSE3Quat());
    }
    pose_traj_.SaveToFile(pose_log_file_);

    loop_remain_traj_.data_.clear();
    for (int i = 0; i < (int)switch_edge.size(); i++) {
        SwitchableEdge &edge = switch_edge[i];
        if (edge.v_->estimate() > 0.5) {
            loop_remain_traj_.data_.push_back(loop_traj_.data_[i]);
        }
    }
    loop_remain_traj_.SaveToFile(loop_remain_log_file_);

    refine_traj_.data_.clear();
    for (int i = 0; i < (int)odometry_traj_.data_.size(); i++) {
        refine_traj_.data_.push_back(odometry_traj_.data_[i]);
    }
    for (int i = 0; i < (int)switch_edge.size(); i++) {
        SwitchableEdge &edge = switch_edge[i];
        if (edge.v_->estimate() > 0.5 &&
            loop_traj_.data_[i].id1_ + 1 < loop_traj_.data_[i].id2_) {
            refine_traj_.data_.push_back(loop_traj_.data_[i]);
        }
    }
    refine_traj_.SaveToFile(refine_log_file_);
}

void COptApp::OptimizeEM() {
    struct SwitchableEdge {
      public:
        double weight_;
        g2o::EdgeSE3 *e_;
        FramedTransformation *t_;
    };

    g2o::SparseOptimizer *optimizer;
    optimizer = new g2o::SparseOptimizer();
    optimizer->setVerbose(true);
    SlamBlockSolver *solver = NULL;
    SlamLinearCSparseSolver *linearSolver = new SlamLinearCSparseSolver();
    linearSolver->setBlockOrdering(false);
    solver = new SlamBlockSolver(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *algo =
        new g2o::OptimizationAlgorithmLevenberg(solver);
    optimizer->setAlgorithm(algo);

    std::vector<SwitchableEdge> switch_edge;

    Eigen::Matrix<double, 6, 6> default_information;
    default_information = Eigen::Matrix<double, 6, 6>::Identity();

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId(i);
        v->setEstimate(Eigen2G2O(pose_traj_.data_[i].transformation_));
        if (i == 0) {
            v->setFixed(true);
        }
        optimizer->addVertex(v);

        if (i > 0) {
            g2o::EdgeSE3 *g2o_edge = new g2o::EdgeSE3();
            g2o_edge->vertices()[0] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i - 1));
            g2o_edge->vertices()[1] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
            g2o_edge->setMeasurement(g2o::internal::fromSE3Quat(
                Eigen2G2O(odometry_traj_.data_[i - 1].transformation_)));
            if (odometry_info_.data_.size() > 0) {
                g2o_edge->setInformation(
                    odometry_info_.data_[i - 1].information_);
            } else {
                g2o_edge->setInformation(default_information);
            }
            optimizer->addEdge(g2o_edge);
        }
    }

    for (int i = 0; i < (int)loop_traj_.data_.size(); i++) {
        FramedTransformation &t = loop_traj_.data_[i];
        SwitchableEdge edge;
        edge.t_ = &t;
        edge.weight_ = 0.0;

        edge.e_ = new g2o::EdgeSE3();
        edge.e_->vertices()[0] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id1_));
        edge.e_->vertices()[1] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id2_));
        edge.e_->setMeasurement(
            g2o::internal::fromSE3Quat(Eigen2G2O(t.transformation_)));
        if (loop_info_.data_.size() > 0) {
            edge.e_->setInformation(loop_info_.data_[i].information_);
        } else {
            edge.e_->setInformation(default_information);
        }
        optimizer->addEdge(edge.e_);
        switch_edge.push_back(edge);
    }

    for (int itr = 0; itr < max_iteration_; itr++) {
        // E step
        for (int i = 0; i < (int)switch_edge.size(); i++) {
            SwitchableEdge &edge = switch_edge[i];
            if (loop_info_.data_.size() > 0) {
                edge.e_->setInformation(loop_info_.data_[i].information_);
            } else {
                edge.e_->setInformation(default_information);
            }
            edge.e_->computeError();
            edge.weight_ = (weight_ * weight_) /
                           (weight_ * weight_ + switch_edge[i].e_->chi2());

            if (loop_info_.data_.size() > 0) {
                edge.e_->setInformation(loop_info_.data_[i].information_ *
                                        sqrt(edge.weight_));
            } else {
                edge.e_->setInformation(default_information *
                                        sqrt(edge.weight_));
            }
        }

        // M step
        optimizer->initializeOptimization();
        optimizer->optimize(1);
    }

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
        pose_traj_.data_[i].transformation_ =
            G2O2Matrix4d(v->estimateAsSE3Quat());
    }
    pose_traj_.SaveToFile(pose_log_file_);

    loop_remain_traj_.data_.clear();
    for (int i = 0; i < (int)switch_edge.size(); i++) {
        SwitchableEdge &edge = switch_edge[i];
        if (edge.weight_ > 0.25) {
            loop_remain_traj_.data_.push_back(loop_traj_.data_[i]);
        }
    }
    loop_remain_traj_.SaveToFile(loop_remain_log_file_);
}


#include <Eigen/SparseCore>

#include <RIMACS/Forward.hpp>
#include <RIMACS/Functors/LineGraph.hpp>

#include <RIMACS/Functors/GraphCachingFunctor.hpp>

namespace RIMACS {

// Experimental evaluation for optimal node order suggestion as performed by
// Schmidt et.al.: Disconnected Maximum Common Substructures Under Constraints
// https://dx.doi.org/10.1021/acs.jcim.0c00741
static constexpr double NODE_ORDER_RELATIVE_POSITION = 1.0;
static constexpr double EDGE_ORDER_RELATIVE_POSITION = 0.75;

// Con- Destructors have to be defined in cpp because the SparseMatrixCache struct is undefined in the
// hpp. But the unique_ptr con- destructors require, that the struct is defined.
CachingAdjacencyFunctor::CachingAdjacencyFunctor(
    const CachingRequiredMinimalAdjacencyFunctor& realInstance)
  : m_instance(realInstance)
{}

CachingAdjacencyFunctor::~CachingAdjacencyFunctor() = default;
CachingAdjacencyFunctor::CachingAdjacencyFunctor(
    CachingAdjacencyFunctor&& other,
    const CachingRequiredMinimalAdjacencyFunctor& newBaseRef) noexcept
  : m_instance(newBaseRef)
  , m_cache(std::move(other.m_cache))
  , m_nodeOrder(std::move(other.m_nodeOrder))
{}

struct SparseMatrixCache {
  SparseMatrixCache(
      ComponentIndex nofNodes)
    : m_matrix(nofNodes, nofNodes)
  {}

  Eigen::SparseMatrix<MappingIndex> m_matrix;
};

void CachingAdjacencyFunctor::doInitialize()
{
  m_cache = std::make_unique<SparseMatrixCache>(m_instance.getNofNodes());
  auto& mat = m_cache->m_matrix;
  std::vector<Eigen::Triplet<unsigned>> edges;
  edges.reserve(2 * m_instance.getNofEdges());
  for(MappingIndex i = 0, last = m_instance.getNofEdges(); i < last; ++i) {
    MappingIndex from = m_instance.edgeGetFromId(i);
    MappingIndex to = m_instance.edgeGetToId(i);
    edges.emplace_back(from, to, i + 1);
    edges.emplace_back(to, from, i + 1);
  }
  std::sort(edges.begin(), edges.end(),
            [](const Eigen::Triplet<unsigned>& t1, const Eigen::Triplet<unsigned>& t2) {
              if(t1.col() != t2.col()) {
                return t1.col() < t2.col();
              }
              return t1.row() < t2.row();
            });
  mat.setFromTriplets(edges.begin(), edges.end());
  mat.makeCompressed();

  if(dynamic_cast<const BasicLineGraph*>(&m_instance)) {
    m_nodeOrder = this->getNodeOrderSuggestionVector(EDGE_ORDER_RELATIVE_POSITION);
  } else {
    m_nodeOrder = this->getNodeOrderSuggestionVector(NODE_ORDER_RELATIVE_POSITION);
  }
}

bool CachingAdjacencyFunctor::adjacent(
    MappingIndex node1,
    MappingIndex node2) const
{
  const auto& mat = m_cache->m_matrix;
  // perform a linear search on the neighbour indices.
  // On such low degree graphs (usually <= 4), linear searches outperform binary searches
  auto begin = mat.innerIndexPtr() + mat.outerIndexPtr()[node1];
  auto end = mat.innerIndexPtr() + mat.outerIndexPtr()[node1 + 1];
  return std::find(begin, end, node2) != end;
}

MappingIndex CachingAdjacencyFunctor::getEdgeId(
    MappingIndex nodeFromId,
    MappingIndex nodeToId) const
{
  static_assert(NO_EDGE == std::numeric_limits<MappingIndex>::max(),
                "No edge indicator must be the -1 value");
  MappingIndex edgeId = m_cache->m_matrix.coeff(nodeFromId, nodeToId);
  return edgeId - 1;
}

MappingIndex CachingAdjacencyFunctor::edgeGetFromId(
    MappingIndex edgeId) const
{
  return m_instance.edgeGetFromId(edgeId);
}

MappingIndex CachingAdjacencyFunctor::edgeGetToId(
    MappingIndex edgeId) const
{
  return m_instance.edgeGetToId(edgeId);
}

std::vector<MappingIndex> CachingAdjacencyFunctor::nodeGetEdges(
    MappingIndex nodeIdx) const
{
  const Eigen::SparseMatrix<unsigned>& mat = m_cache->m_matrix;
  Eigen::SparseMatrix<unsigned>::InnerIterator it(mat, nodeIdx);
  std::vector<MappingIndex> result;
  result.reserve(mat.outerIndexPtr()[nodeIdx + 1] - mat.outerIndexPtr()[nodeIdx]);
  for(; it; ++it) {
    result.push_back(it.value() - 1);
  }
  return result;
}

std::vector<MappingIndex> CachingAdjacencyFunctor::nodeGetNeighbours(
    MappingIndex nodeIdx) const
{
  const Eigen::SparseMatrix<unsigned>& mat = m_cache->m_matrix;
  Eigen::SparseMatrix<unsigned>::InnerIterator it(mat, nodeIdx);
  std::vector<MappingIndex> result;
  result.reserve(mat.outerIndexPtr()[nodeIdx + 1] - mat.outerIndexPtr()[nodeIdx]);
  for(; it; ++it) {
    result.push_back(it.row());
  }
  return result;
}

MappingIndex CachingAdjacencyFunctor::getNofNodes() const
{
  return m_cache->m_matrix.rows();
}

MappingIndex CachingAdjacencyFunctor::nodeGetNofEdges(
    MappingIndex nodeIdx) const
{
  return m_cache->m_matrix.outerIndexPtr()[nodeIdx + 1]
         - m_cache->m_matrix.outerIndexPtr()[nodeIdx];
}

MappingIndex CachingAdjacencyFunctor::getNofEdges() const
{
  return m_instance.getNofEdges();
}

} // namespace RIMACS


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

  Aggregate SIMD numerical and type limits for X86 SSE2

  @copyright 2012 - 2015 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_LIMITS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_LIMITS_HPP_INCLUDED

#include <boost/simd/arch/x86/tags.hpp>
#include <boost/simd/arch/common/limits.hpp>
#include <boost/dispatch/meta/make_integer.hpp>

namespace boost { namespace simd
{
  template<> struct limits<boost::simd::sse2_>
  {
    using parent = boost::simd::simd_;

    struct largest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<8,Sign> {};
    };

    struct smallest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<1,Sign> {};
    };
    using smallest_real = float;
    using largest_real  = double;

    enum { bits = 128, bytes = 16 };
  };
} }

#endif


#include "gauss_newton_solver.h"
#include "prior_sparse_features.h"
#include "util.h"
#include "device_util.h"
#include "device_array.h"

#include <Eigen/Dense>
#include <chrono>

GaussNewtonSolver::GaussNewtonSolver()
	: m_face_bb(1)
	, m_sh_coefficients_gpu(9)
{
	cublasCreate(&m_cublas);
}

GaussNewtonSolver::~GaussNewtonSolver()
{
	cublasDestroy(m_cublas);
	destroyTextures();
}

void GaussNewtonSolver::solve(const std::vector<glm::vec2>& sparse_features, Face& face, cv::Mat& frame, glm::mat4& projection, const Pyramid& pyramid)
{
	if (sparse_features.empty()) //no tracking -> cublas doesnt like a getting matrix/vector of size 0
	{
		return;
	}

	auto number_of_levels = pyramid.getNumberOfLevels();
	if (sizeof(m_params.num_gn_iterations) / sizeof(int) != number_of_levels)
	{
		throw std::runtime_error("Please specify number of GN iteration per pyramid level!");
	}

	const int nFeatures = sparse_features.size();
	const int nShapeCoeffs = m_params.num_shape_coefficients;
	const int nExpressionCoeffs = m_params.num_expression_coefficients;
	const int nAlbedoCoeffs = m_params.num_albedo_coefficients;
	const int nFaceCoeffs = nShapeCoeffs + nExpressionCoeffs + nAlbedoCoeffs;
	const int nUnknowns = 7 + nFaceCoeffs + 9; //3+3+1 = 7 DoF for rotation, translation and intrinsics. Plus nFaceCoeffs for face parameters and 9 for lighting.

	const float wSparse = std::powf(10, m_params.sparse_weight_exponent);
	const float wDense = std::powf(10, m_params.dense_weight_exponent);
	const float wReg = std::powf(10, m_params.regularisation_weight_exponent);

	for (int pyramid_level = number_of_levels - 1; pyramid_level >= 0; pyramid_level--)
	{
		pyramid.setGraphicsSettings(pyramid_level, face.getGraphicsSettings());

		const int frameWidth = face.m_graphics_settings.texture_width;
		const int frameHeight = face.m_graphics_settings.texture_height;
		const int nPixels = frameWidth * frameHeight;
		const int nResiduals = 2 * nFeatures + 3 * nPixels + nFaceCoeffs; //nFaceCoeffs -> regularizer

		const auto& prior_local_ids = PriorSparseFeatures::get().getPriorIds();

		//TODO: Allocate all of the objects below once. So, move them out of here.
		auto jacobian_gpu = util::DeviceArray<float>(nResiduals * nUnknowns);
		auto residuals_gpu = util::DeviceArray<float>(nResiduals);
		auto result_gpu = util::DeviceArray<float>(nUnknowns);
		std::vector<float> result(nUnknowns);
		auto ids_gpu = util::DeviceArray<int>(prior_local_ids);
		auto key_pts_gpu = util::DeviceArray<glm::vec2>(sparse_features);

		cv::Mat processed_frame;
		cv::resize(frame, processed_frame, cv::Size(frameWidth, frameHeight));
		cv::cvtColor(processed_frame, processed_frame, cv::COLOR_BGR2RGB);
		util::DeviceArray<uchar> frame_gpu = util::DeviceArray<uchar>(3 * nPixels);
		util::copy(frame_gpu, processed_frame.data, 3 * nPixels);

		for (int iteration = 0; iteration < m_params.num_gn_iterations[pyramid_level]; ++iteration)
		{
			jacobian_gpu.memset(0);
			residuals_gpu.memset(0);
			face.computeFace();
			face.updateVertexBuffer();
			face.draw();

			auto face_pose = face.computeModelMatrix();
			Eigen::Matrix<float, 3, 3> jacobian_local;
			jacobian_local <<
				face_pose[0][0], face_pose[1][0], face_pose[2][0],
				face_pose[0][1], face_pose[1][1], face_pose[2][1],
				face_pose[0][2], face_pose[1][2], face_pose[2][2];

			glm::mat3 drx, dry, drz;
			face.computeRotationDerivatives(drx, dry, drz);

			mapRenderTargets(face);
			FaceBoundingBox face_bb = computeFaceBoundingBox(face.m_graphics_settings.texture_width, face.m_graphics_settings.texture_height);

			int n_current_residuals = 2 * nFeatures + nFaceCoeffs + 3 * face_bb.width * face_bb.height;

			util::copy(m_sh_coefficients_gpu, face.m_sh_coefficients, 9);

			//CUDA
			computeJacobian(
				//shared memory
				face_bb,
				nFeatures, frameWidth, frameHeight,
				nShapeCoeffs, nExpressionCoeffs, nAlbedoCoeffs, nUnknowns, n_current_residuals,
				face.m_number_of_vertices * 3,
				face.m_shape_coefficients.size(),
				face.m_expression_coefficients.size(),
				face.m_albedo_coefficients.size(),
				face.m_sh_coefficients.size(),
				wSparse, wDense, wReg,

				frame_gpu.getPtr(),

				face_pose, drx, dry, drz, projection, jacobian_local,

				//device memory input
				ids_gpu.getPtr(), face.m_current_face_gpu.getPtr(), key_pts_gpu.getPtr(),

				face.m_shape_basis_gpu.getPtr(),
				face.m_expression_basis_gpu.getPtr(),
				face.m_albedo_basis_gpu.getPtr(),

				face.m_shape_coefficients_gpu.getPtr(),
				face.m_expression_coefficients_gpu.getPtr(),
				face.m_albedo_coefficients_gpu.getPtr(),
				m_sh_coefficients_gpu.getPtr(),

				//device memory output
				jacobian_gpu.getPtr(), residuals_gpu.getPtr()
			);

			unmapRenderTargets(face);

			//Apply step and update poses GPU
			solveUpdatePCG(m_cublas, nUnknowns, n_current_residuals, nResiduals, jacobian_gpu, residuals_gpu, result_gpu, 1.0f, -1.0f);
			util::copy(result, result_gpu, nUnknowns);

			updateParameters(result, projection, frame.cols / static_cast<float>(frame.rows), face, nShapeCoeffs, nExpressionCoeffs, nAlbedoCoeffs);

			std::vector<float> residuals_loss_test(n_current_residuals);
			util::copy(residuals_loss_test, residuals_gpu, n_current_residuals);
			Eigen::Map<Eigen::VectorXf> residuals_loss_test_eigen(residuals_loss_test.data(), n_current_residuals);
			std::cout << "Unknowns: " << nUnknowns << ", Residuals: " << nResiduals << std::endl;
			std::cout << "Iteration: " << iteration << " , Loss: " << glm::sqrt(residuals_loss_test_eigen.dot(residuals_loss_test_eigen)) << std::endl;
		}
	}
}

void GaussNewtonSolver::solveUpdatePCG(const cublasHandle_t& cublas, const int nUnknowns, const int nCurrentResiduals, const int nResiduals, util::DeviceArray<float>& jacobian,
	util::DeviceArray<float>& residuals, util::DeviceArray<float>& x, const float alphaLHS, const float alphaRHS)
{
	const float alpha = 1, beta = 0;
	x.memset(0);

	auto r = util::DeviceArray<float>(nUnknowns);	//current residual
	auto p = util::DeviceArray<float>(nUnknowns);	//gradient
	auto M = util::DeviceArray<float>(nUnknowns);	//preconditioner
	M.memset(0);
	auto z = util::DeviceArray<float>(nUnknowns);	//preconditioned residual
	auto Jp = util::DeviceArray<float>(nCurrentResiduals);
	auto JTJp = util::DeviceArray<float>(nUnknowns);

	//M=inv(diag(JTJ))
	computeJacobiPreconditioner(nUnknowns, nCurrentResiduals, nResiduals, jacobian.getPtr(), M.getPtr());

	//r = -JTf;
	cublasSgemv(cublas, CUBLAS_OP_T, nCurrentResiduals, nUnknowns, &alphaRHS, jacobian.getPtr(), nCurrentResiduals, residuals.getPtr(), 1, &beta, r.getPtr(), 1);

	//z = Mr
	elementwiseMultiplication(nUnknowns, M.getPtr(), r.getPtr(), z.getPtr());

	//p=z;
	cublasScopy(cublas, nUnknowns, z.getPtr(), 1, p.getPtr(), 1);

	float zTr_old = 0, zTr = 0;
	float pTJTJp;
	//zTr
	cublasSdot(cublas, nUnknowns, z.getPtr(), 1, r.getPtr(), 1, &zTr_old);
	int i = 0;
	for (; i < std::min(nUnknowns, m_params.num_pcg_iterations); ++i)
	{
		//apply JTJ
		cublasSgemv(cublas, CUBLAS_OP_N, nCurrentResiduals, nUnknowns, &alphaLHS, jacobian.getPtr(), nCurrentResiduals, p.getPtr(), 1, &beta, Jp.getPtr(), 1);
		cublasSgemv(cublas, CUBLAS_OP_T, nCurrentResiduals, nUnknowns, &alpha, jacobian.getPtr(), nCurrentResiduals, Jp.getPtr(), 1, &beta, JTJp.getPtr(), 1);

		cublasSdot(cublas, nUnknowns, p.getPtr(), 1, JTJp.getPtr(), 1, &pTJTJp);

		float ak = zTr_old / std::max(pTJTJp, m_params.kNearZero);
		//x = ak*p + x
		cublasSaxpy(cublas, nUnknowns, &ak, p.getPtr(), 1, x.getPtr(), 1);

		//r = r - ak* JTJp
		ak *= -1;
		cublasSaxpy(cublas, nUnknowns, &ak, JTJp.getPtr(), 1, r.getPtr(), 1);

		//z=Mr
		elementwiseMultiplication(nUnknowns, M.getPtr(), r.getPtr(), z.getPtr());

		//zTr
		cublasSdot(cublas, nUnknowns, z.getPtr(), 1, r.getPtr(), 1, &zTr);

		if (zTr < m_params.kTolerance)
		{
			break;
		}

		float bk = zTr / std::max(zTr_old, m_params.kNearZero);

		//p = z + bk*p
		cublasSscal(cublas, nUnknowns, &bk, p.getPtr(), 1);
		cublasSaxpy(cublas, nUnknowns, &alpha, z.getPtr(), 1, p.getPtr(), 1);

		zTr_old = zTr;
	}
	//	std::cout << "PCG iters: " << i << std::endl;
}

void GaussNewtonSolver::solveUpdateCG(const cublasHandle_t& cublas, const int nUnknowns, const int nResiduals, util::DeviceArray<float>& jacobian,
	util::DeviceArray<float>& residuals, util::DeviceArray<float>& x, const float alphaLHS, const float alphaRHS)
{
	const float alpha = 1, beta = 0;
	x.memset(0);

	auto r = util::DeviceArray<float>(nUnknowns);	//current residual
	auto p = util::DeviceArray<float>(nUnknowns);	//gradient
	auto Jp = util::DeviceArray<float>(nResiduals);
	auto JTJp = util::DeviceArray<float>(nUnknowns);

	//r = -JTf;
	cublasSgemv(cublas, CUBLAS_OP_T, nResiduals, nUnknowns, &alphaRHS, jacobian.getPtr(), nResiduals, residuals.getPtr(), 1, &beta, r.getPtr(), 1);

	//p=r;
	cublasScopy(cublas, nUnknowns, r.getPtr(), 1, p.getPtr(), 1);

	float rTr_old = 0, rTr;
	float pTJTJp;
	//rTr
	cublasSdot(cublas, nUnknowns, r.getPtr(), 1, r.getPtr(), 1, &rTr);
	int i = 0;
	auto num_of_iterations = std::min(nUnknowns, m_params.num_pcg_iterations);
	for (; i < num_of_iterations; ++i)
	{
		//apply JTJ
		cublasSgemv(cublas, CUBLAS_OP_N, nResiduals, nUnknowns, &alphaLHS, jacobian.getPtr(), nResiduals, p.getPtr(), 1, &beta, Jp.getPtr(), 1);
		cublasSgemv(cublas, CUBLAS_OP_T, nResiduals, nUnknowns, &alpha, jacobian.getPtr(), nResiduals, Jp.getPtr(), 1, &beta, JTJp.getPtr(), 1);

		rTr_old = rTr;

		cublasSdot(cublas, nUnknowns, p.getPtr(), 1, JTJp.getPtr(), 1, &pTJTJp);

		float ak = rTr / std::max(pTJTJp, m_params.kNearZero);
		//x = ak*p + x
		cublasSaxpy(cublas, nUnknowns, &ak, p.getPtr(), 1, x.getPtr(), 1);

		//r = r - ak* JTJp
		ak *= -1;
		cublasSaxpy(cublas, nUnknowns, &ak, JTJp.getPtr(), 1, r.getPtr(), 1);

		//rTr
		cublasSdot(cublas, nUnknowns, r.getPtr(), 1, r.getPtr(), 1, &rTr);

		if (rTr < m_params.kTolerance)
		{
			break;
		}

		float bk = rTr / std::max(rTr_old, m_params.kNearZero);

		//p = r + bk*p
		cublasSscal(cublas, nUnknowns, &bk, p.getPtr(), 1);
		cublasSaxpy(cublas, nUnknowns, &alpha, r.getPtr(), 1, p.getPtr(), 1);
	}
}

void GaussNewtonSolver::updateParameters(const std::vector<float>& result, glm::mat4& projection, float aspect_ratio, Face& face,
	const int nShapeCoeffs, const int nExpressionCoeffs, const int nAlbedoCoeffs)
{
	projection[0][0] += result[0];
	projection[1][1] = projection[0][0] * aspect_ratio;

	face.m_rotation_coefficients.x += result[1];
	face.m_rotation_coefficients.y += result[2];
	face.m_rotation_coefficients.z += result[3];

	face.m_translation_coefficients.x += result[4];
	face.m_translation_coefficients.y += result[5];
	face.m_translation_coefficients.z += result[6];

#pragma omp parallel num_threads(4)
	{
#pragma omp single
		{
			for (int i = 0; i < nShapeCoeffs; ++i)
			{
				face.m_shape_coefficients[i] += result[7 + i];
			}
		}

#pragma omp single
		{
			for (int i = 0; i < nExpressionCoeffs; ++i)
			{
				auto c = face.m_expression_coefficients[i] + result[7 + nShapeCoeffs + i];
				face.m_expression_coefficients[i] = glm::clamp(c, -0.5f, 0.5f);
			}
		}

#pragma omp single
		{
			for (int i = 0; i < nAlbedoCoeffs; ++i)
			{
				face.m_albedo_coefficients[i] += result[7 + nShapeCoeffs + nExpressionCoeffs + i];
			}
		}

#pragma omp single
		{
			for (int i = 0; i < 9; ++i)
			{
				face.m_sh_coefficients[i] += result[7 + nShapeCoeffs + nExpressionCoeffs + nAlbedoCoeffs + i];
			}
		}
	}
}

void GaussNewtonSolver::mapRenderTargets(Face& face)
{
	if (face.m_graphics_settings.mapped_to_cuda)
	{
		std::cout << "Warning: mapRenderTargets is called while rts is already mapped!" << std::endl;
		return;
	}

	cudaGraphicsResource* resources[] = { face.m_graphics_settings.rt_rgb_cuda_resource,
		face.m_graphics_settings.rt_barycentrics_cuda_resource,
		face.m_graphics_settings.rt_vertex_ids_cuda_resource };
	CHECK_CUDA_ERROR(cudaGraphicsMapResources(3, resources, 0));

	cudaArray* array_rgb{ nullptr };
	cudaArray* array_barycentrics{ nullptr };
	cudaArray* array_vertex_ids{ nullptr };

	CHECK_CUDA_ERROR(cudaGraphicsSubResourceGetMappedArray(&array_rgb, resources[0], 0, 0));
	CHECK_CUDA_ERROR(cudaGraphicsSubResourceGetMappedArray(&array_barycentrics, resources[1], 0, 0));
	CHECK_CUDA_ERROR(cudaGraphicsSubResourceGetMappedArray(&array_vertex_ids, resources[2], 0, 0));

	//RGB texture
	cudaResourceDesc res_desc;
	memset(&res_desc, 0, sizeof(res_desc));
	res_desc.resType = cudaResourceTypeArray;
	res_desc.res.array.array = array_rgb;

	cudaTextureDesc tex_desc;
	memset(&tex_desc, 0, sizeof(tex_desc));
	tex_desc.addressMode[0] = cudaTextureAddressMode(cudaAddressModeWrap);
	tex_desc.addressMode[1] = cudaTextureAddressMode(cudaAddressModeWrap);
	tex_desc.filterMode = cudaTextureFilterMode(cudaFilterModeLinear);
	tex_desc.readMode = cudaReadModeNormalizedFloat;
	tex_desc.normalizedCoords = 0;
	CHECK_CUDA_ERROR(cudaCreateTextureObject(&m_texture_rgb, &res_desc, &tex_desc, nullptr));

	//Barycentrics texture
	res_desc.res.array.array = array_barycentrics;
	tex_desc.filterMode = cudaTextureFilterMode(cudaFilterModePoint);
	tex_desc.readMode = cudaReadModeElementType;
	CHECK_CUDA_ERROR(cudaCreateTextureObject(&m_texture_barycentrics, &res_desc, &tex_desc, nullptr));

	//Vertex ids texture
	res_desc.res.array.array = array_vertex_ids;
	CHECK_CUDA_ERROR(cudaCreateTextureObject(&m_texture_vertex_ids, &res_desc, &tex_desc, nullptr));

	face.m_graphics_settings.mapped_to_cuda = true;
}

void GaussNewtonSolver::unmapRenderTargets(Face& face)
{
	if (!face.m_graphics_settings.mapped_to_cuda)
	{
		std::cout << "Warning: unmapRenderTargets is called while rts is already unmapped!" << std::endl;
		return;
	}

	destroyTextures();

	cudaGraphicsResource* resources[] = { face.m_graphics_settings.rt_rgb_cuda_resource,
		face.m_graphics_settings.rt_barycentrics_cuda_resource,
		face.m_graphics_settings.rt_vertex_ids_cuda_resource };
	CHECK_CUDA_ERROR(cudaGraphicsUnmapResources(3, resources, 0));

	face.m_graphics_settings.mapped_to_cuda = false;
}

void GaussNewtonSolver::destroyTextures()
{
	if (m_texture_rgb)
	{
		CHECK_CUDA_ERROR(cudaDestroyTextureObject(m_texture_rgb));
		m_texture_rgb = 0;
	}
	if (m_texture_barycentrics)
	{
		m_texture_barycentrics = 0;
		CHECK_CUDA_ERROR(cudaDestroyTextureObject(m_texture_barycentrics));
	}
	if (m_texture_vertex_ids)
	{
		CHECK_CUDA_ERROR(cudaDestroyTextureObject(m_texture_vertex_ids));
		m_texture_vertex_ids = 0;
	}
}


//==============================================================================
//         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_BITWISE_FUNCTIONS_SIMD_COMMON_FFS_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_FFS_HPP_INCLUDED

#include <boost/simd/bitwise/functions/ffs.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/genmask.hpp>
#include <boost/simd/include/functions/simd/firstbitset.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/shift_left.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/simd/sdk/simd/meta/biggest_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( ffs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<type8_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0,unsigned>::type rtype;
      typedef typename A0::extension_type ext;
      typedef simd::native<typename simd::meta::biggest_integer<ext>::type, ext> ltype;
      typedef typename boost::dispatch::meta::scalar_of<ltype>::type sltype;
      rtype v = firstbitset(a0);
      return b_and(genmask(v), b_or(b_or((-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xAAAAAAAAAAAAAAAAull)>()))))
                                        ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xCCCCCCCCCCCCCCCCull)>()))), 1))
                                   ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xF0F0F0F0F0F0F0F0ull)>()))), 2))+One<rtype>());
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( ffs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<type64_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0,unsigned>::type rtype;
      return  simd::bitwise_cast<rtype>(map(dispatch::functor<boost::simd::tag::ffs_>(), simd::bitwise_cast<rtype>(a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( ffs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<type16_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0,unsigned>::type rtype;
      typedef typename A0::extension_type ext;
      typedef simd::native<typename simd::meta::biggest_integer<ext>::type, ext> ltype;
      typedef typename boost::dispatch::meta::scalar_of<ltype>::type sltype;
      rtype v = firstbitset(a0);
      return  b_and(genmask(v), b_or(b_or(b_or((-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xAAAAAAAAAAAAAAAAull)>()))))
                                              ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xCCCCCCCCCCCCCCCCull)>()))), 1))
                                         ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xF0F0F0F0F0F0F0F0ull)>()))), 2))
                                    ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xFF00FF00FF00FF00ull)>()))), 3))+One<rtype>());
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( ffs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<type32_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0,unsigned>::type rtype;
      typedef typename A0::extension_type ext;
      typedef simd::native<typename simd::meta::biggest_integer<ext>::type, ext> ltype;
      typedef typename boost::dispatch::meta::scalar_of<ltype>::type sltype;
      rtype v = firstbitset(a0);
      return  b_and(genmask(v), b_or(b_or(b_or(b_or((-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xAAAAAAAAAAAAAAAAull)>()))))
                                                   ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xCCCCCCCCCCCCCCCCull)>()))), 1))
                                              ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xF0F0F0F0F0F0F0F0ull)>()))), 2))
                                         ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xFF00FF00FF00FF00ull)>()))), 3))
                                    ,  shli(-( genmask(b_and(v, boost::simd::integral_constant<ltype,sltype(0xFFFF0000FFFF0000ull)>()))), 4))+One<rtype>());
    }
  };
} } }

#endif


//==============================================================================
//         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 NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMALN_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMALN_HPP_INCLUDED

#include <nt2/euler/functions/gammaln.hpp>
#include <nt2/euler/functions/details/gammaln_kernel.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/simd/dec.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/floor.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/is_flint.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/is_greater_equal.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/is_lez.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/log.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/logical_andnot.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/rec.hpp>
#include <nt2/include/functions/simd/seladd.hpp>
#include <nt2/include/functions/simd/seldec.hpp>
#include <nt2/include/functions/simd/selinc.hpp>
#include <nt2/include/functions/simd/selsub.hpp>
#include <nt2/include/functions/simd/sinpi.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/include/constants/false.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/invpi.hpp>
#include <nt2/include/constants/logpi.hpp>
#include <nt2/include/constants/logsqrt2pi.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITES
#include <nt2/include/functions/simd/logical_or.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( gammaln_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<double_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    typedef typename meta::as_logical<A0>::type bA0;
    NT2_FUNCTOR_CALL(1)
    {
      bA0 inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = nt2::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = eq(q, Inf<A0>());
      #endif
      bA0 test = lt(a0, nt2::splat<A0>(-34.0));
      size_t nb = nt2::inbtrue(test);
      A0 r =  Nan<A0>();
      if(nb > 0)
      {
        //treat negative large with reflection
        r = large_negative(q);
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(inf_result, Nan<A0>(), r);
      }
      A0 r1 = other(a0);
      A0 r2 = if_else(test, r, r1);
      return nt2::if_else(eq(a0, Minf<A0>()),
                          Nan<A0>(),
                          nt2::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    static inline A0 large_negative(const A0& q)
    {
      A0 w = gammaln(q);
      A0 p = nt2::floor(q);
      A0 z = q - p;
      bA0 test2 = lt(z, nt2::Half<A0>() );
      z = nt2::selsub(test2, z, nt2::One<A0>());
      z = q*nt2::sinpi(z);
      z =  nt2::abs(z);
      return Logpi<A0>()-log(z)-w;
    }
    static inline A0 other(const A0& xx)
    {
      A0 x =  xx;
      bA0 test = lt(x, nt2::splat<A0>(13.0) );
      size_t nb = inbtrue(test);
      A0 r1 = Zero<A0>();
      if (nb > 0)
      {
        A0 z = One<A0>();
        A0 p = Zero<A0>();
        A0 u = x;
        bA0 test1 = ge(u,Three<A0>());
        while(nt2::any(test1))
        {
          p = seldec(test1, p);
          u = if_else(test1, x+p, u);
          z = if_else(test1, z*u, z);
          test1 = ge(u,Three<A0>());
        }
        //all u are less than 3
        bA0 test2 = lt(u,Two<A0>());

        while(nt2::any(test2))
        {
          z = if_else(test2, z/u, z);
          p = selinc(test2, p);
          u = if_else(test2, x+p, u);
          test2 = lt(u,Two<A0>());
        }
        z = nt2::abs(z);
        x +=  p-Two<A0>();
        r1 = x * details::gammaln_kernel<A0>::gammaln1(x)+nt2::log(z);
        if (nb >= meta::cardinal_of<A0>::value) return r1;
      }
      A0 r2 = fma(xx-Half<A0>(),nt2::log(xx), Logsqrt2pi<A0>()-xx);
      A0 p = rec(sqr(xx));
      r2 += details::gammaln_kernel<A0>::gammalnA(p)/xx;
      return if_else(test, r1, r2);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( gammaln_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<single_<A0>,X>))
                            )
  {

    typedef A0 result_type;
    typedef typename meta::as_logical<A0>::type bA0;
    typedef typename meta::scalar_of<A0>::type sA0;
    NT2_FUNCTOR_CALL(1)
    {
      bA0 inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = nt2::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = logical_or(eq(x, Inf<A0>()), inf_result);
      #endif
      bA0 ltza0 = is_ltz(a0);
      size_t nb = nt2::inbtrue(ltza0);
      A0 r, r1 =  other(q);
      if(nb > 0)
      {
        //treat negative
        r = nt2::if_else(inf_result, Inf<A0>(), negative(q, r1));
        if (nb >= meta::cardinal_of<A0>::value) return r;
      }
      A0 r2 = if_else(ltza0, r, r1);
      return nt2::if_else(eq(a0, Minf<A0>()),
                          Nan<A0>(),
                          nt2::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    static inline A0 negative(const A0& q,  const A0& w)
    {
      A0 p = nt2::floor(q);
      A0 z = q - p;
      bA0 test2 = lt(z, nt2::Half<A0>() );
      z = nt2::selsub(test2, z, nt2::One<A0>());
      z = q*nt2::sinpi(z);
      z =  nt2::abs(z);
      return -log(Invpi<A0>()*nt2::abs(z))-w;
    }
    static inline A0 other(const A0& x)
    {
      bA0 xlt650 = lt(x, nt2::splat<A0>(6.50) );
      size_t nb = inbtrue(xlt650);
      A0 r0x = x;
      A0 r0z = x;
      A0 r0s = One<A0>();
      A0 r1 = Zero<A0>();
      A0 p =  Nan<A0>();
      if (nb > 0)
      {
        bA0 kernelC = False<bA0>();
        A0 z = One<A0>();
        A0 tx = x;
        A0 nx = Zero<A0>();

        const A0 _075 = nt2::splat<A0>(0.75);
        const A0 _150 = Two<A0>()*_075;
        const A0 _125 = nt2::splat<A0>(1.25);
        const A0 _250 = Two<A0>()*_125;
        bA0 xge150 = ge(x, _150);
        bA0 txgt250= gt(tx,_250);

        // x >= 1.5
        while (nt2::any(logical_and(xge150, txgt250)))
        {
          nx = seldec(txgt250, nx);
          tx = if_else(txgt250, x + nx, tx);
          z = if_else(txgt250, z*tx, z);
          txgt250= gt(tx,_250);
        }
        r0x = seladd(xge150, x, nx - Two<A0>());
        r0z = if_else(xge150, z, r0z);
        r0s = if_else(xge150,One<A0>(), r0s);

        // x >= 1.25 && x < 1.5
        bA0 xge125 = ge(x, _125);
        bA0 xge125t = l_andnot(xge125, xge150);
        if (nt2::any(xge125))
        {
          r0x =  if_else(xge125t, dec(x)    , r0x);
          r0z =  if_else(xge125t, z*x       , r0z);
          r0s =  if_else(xge125t, Mone<A0>(), r0s);
        }
        // x >= 0.75&& x < 1.5
        bA0 xge075  = ge(x, _075);
        bA0 xge075t = l_andnot(xge075, xge125);
        if (nt2::any(xge075t))
        {
          kernelC =  xge075t;
          r0x =  if_else(xge075t, dec(x)    , r0x);
          r0z =  if_else(xge075t, One<A0>() , r0z);
          r0s =  if_else(xge075t, Mone<A0>(), r0s);
          p = details::gammaln_kernel<A0>::gammalnC(r0x);
        }
        // tx < 1.5 && x < 0.75
        bA0 txlt150 = l_andnot(lt(tx,_150), xge075);
        if (nt2::any(txlt150))
        {
          bA0 orig = txlt150;
          while( nt2::any(txlt150) )
          {
            z  = if_else(txlt150, z*tx, z);
            nx = selinc(txlt150, nx);
            tx = if_else(txlt150, x + nx, tx);
            txlt150= l_andnot(lt(tx,_150), xge075);
          }
          r0x =  seladd(orig, r0x, nx - Two<A0>());
          r0z =  if_else(orig,z         , r0z);
          r0s =  if_else(orig,Mone<A0>(), r0s);
        }
        p =  if_else(kernelC, p, details::gammaln_kernel<A0>::gammalnB(r0x));
        if (nb >= meta::cardinal_of<A0>::value)
          return fma(r0x, p, r0s*nt2::log(nt2::abs(r0z)));
      }
      r0z = if_else(xlt650, nt2::abs(r0z), x);
      A0 m = nt2::log(r0z);
      r1 = fma(r0x, p, r0s*m);
      A0 r2 = fma(x-Half<A0>(),m,Logsqrt2pi<A0>()-x);
      r2 += details::gammaln_kernel<A0>::gammaln2(rec(sqr(x)))/x;
      return if_else(xlt650, r1, r2);
    }
  };
} }

#endif


/*!
@file
Adapts `std::ratio` for use with Hana.

@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_EXT_STD_RATIO_HPP
#define BOOST_HANA_EXT_STD_RATIO_HPP

#include <boost/hana/bool.hpp>
#include <boost/hana/concept/integral_constant.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/fwd/core/convert.hpp>
#include <boost/hana/fwd/core/tag_of.hpp>
#include <boost/hana/fwd/div.hpp>
#include <boost/hana/fwd/equal.hpp>
#include <boost/hana/fwd/less.hpp>
#include <boost/hana/fwd/minus.hpp>
#include <boost/hana/fwd/mod.hpp>
#include <boost/hana/fwd/mult.hpp>
#include <boost/hana/fwd/one.hpp>
#include <boost/hana/fwd/plus.hpp>
#include <boost/hana/fwd/zero.hpp>

#include <cstdint>
#include <ratio>
#include <type_traits>

namespace boost { namespace hana {
    namespace ext { namespace std { struct ratio_tag; }}

    template <std::intmax_t num, std::intmax_t den>
    struct tag_of<std::ratio<num, den>> {
        using type = ext::std::ratio_tag;
    };

    //////////////////////////////////////////////////////////////////////////
    // Conversion from IntegralConstants
    //////////////////////////////////////////////////////////////////////////
    template <typename C>
    struct to_impl<ext::std::ratio_tag, C, when<
        hana::IntegralConstant<C>::value
    >> {
        template <typename N>
        static constexpr auto apply(N const&) {
            return std::ratio<N::value>{};
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr auto apply(R1 const&, R2 const&)
        { return hana::bool_c<std::ratio_equal<R1, R2>::value>; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct less_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr auto apply(R1 const&, R2 const&)
        { return hana::bool_c<std::ratio_less<R1, R2>::value>; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct plus_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_add<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct zero_impl<ext::std::ratio_tag> {
        static constexpr std::ratio<0> apply()
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Group
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct minus_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_subtract<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct mult_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_multiply<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct one_impl<ext::std::ratio_tag> {
        static constexpr std::ratio<1> apply()
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // EuclideanRing
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct div_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_divide<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct mod_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio<0> apply(R1 const&, R2 const&)
        { return {}; }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_EXT_STD_RATIO_HPP


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include "application.h"

#include <exception>
#include <stdexcept>
#include <string>
#include <sstream>

#include <boost/assign/list_of.hpp>

#include <scm/log.h>
#include <scm/core/math.h>

#include <scm/gl_core.h>
#include <scm/gl_core/math.h>

#include <scm/gl_util/data/imaging/texture_loader.h>
#include <scm/gl_util/data/imaging/texture_loader_dds.h>
#include <scm/gl_util/primitives/wavefront_obj.h>

namespace {

const scm::math::vec3f diffuse(0.7f, 0.7f, 0.7f);
const scm::math::vec3f specular(0.2f, 0.7f, 0.9f);
const scm::math::vec3f ambient(0.1f, 0.1f, 0.1f);
const scm::math::vec3f position(1, 1, 1);

struct scm_debug_output : public scm::gl::render_context::debug_output
{
    void operator()(scm::gl::debug_source   src,
                    scm::gl::debug_type     t,
                    scm::gl::debug_severity sev,
                    const std::string&      msg) const
    {
        using namespace scm;
        using namespace scm::gl;
        out() << log::error
              << "gl error: <source: " << debug_source_string(src)
              << ", type: "            << debug_type_string(t)
              << ", severity: "        << debug_severity_string(sev) << "> "
              << msg << log::end;
    }
};

} // namespace

namespace scm {
namespace gl {
namespace gui {

application_window::application_window(const math::vec2ui&                    vp_size,
                                       const gl::viewer::viewer_attributes&   view_attrib,
                                       const gl::wm::context::attribute_desc& ctx_attrib,
                                       const gl::wm::surface::format_desc&    win_fmt)
  : viewer_window(vp_size, view_attrib, ctx_attrib, win_fmt)
{
    if (!init_renderer()) {
        std::stringstream msg;
        msg << "application_window::application_window(): error initializing multi large image rendering stystem.";
        err() << msg.str() << log::end;
        throw (std::runtime_error(msg.str()));
    }
}

application_window::~application_window()
{
    shutdown();
    std::cout << "application_window::~application_window(): bye, bye..." << std::endl;
}

bool
application_window::init_renderer()
{
    using namespace scm;
    using namespace scm::gl;
    using namespace scm::math;
    using boost::assign::list_of;

    const render_device_ptr& device = _viewer->device();

    device->main_context()->register_debug_callback(make_shared<scm_debug_output>());

    if (!reload_shaders(device)) {
        scm::err() << log::error
                   << "application_window::init_renderer(): "
                   << "error loading program" << log::end;
        return false;
    }

    _bstate = device->create_blend_state(false, FUNC_ONE, FUNC_ZERO, FUNC_ONE, FUNC_ZERO);
    _dstate = device->create_depth_stencil_state(true, true, COMPARISON_LESS);
    _rstate = device->create_rasterizer_state(FILL_SOLID, CULL_BACK, ORIENT_CCW, true);
    _sstate = device->create_sampler_state(FILTER_MIN_MAG_LINEAR, WRAP_CLAMP_TO_EDGE);

    try {
        texture_loader tl;
        _env_map      = tl.load_texture_2d(*device, "e:/data/textures/ibl_hdr/mobile_ave_env.hdr", false);
        _env_map_diff = tl.load_texture_2d(*device, "e:/data/textures/ibl_hdr/mobile_ave_diff.hdr", false);
        //_model      = make_shared<wavefront_obj_geometry>(device, "../../../res/geometry/sphere.obj");
        //_model      = make_shared<wavefront_obj_geometry>(device, "../../../res/geometry/box.obj");
        _model      = make_shared<wavefront_obj_geometry>(device, "../../../res/geometry/guardian.obj");
        //_model      = make_shared<wavefront_obj_geometry>(device, "e:/data/geometry/beetle_felge.obj");
        //_model      = make_shared<wavefront_obj_geometry>(device, "g:/geometry/01_renault_clio.obj");
        //_model        = make_shared<wavefront_obj_geometry>(device, "e:/data/geometry/high_res_sphere3.obj");
        //_model        = make_shared<wavefront_obj_geometry>(device, "e:/data/geometry/happy_budda.obj");

        texture_loader_dds tldds;
        _model_normal_map  = tl.load_texture_2d(*device, "../../../res/geometry/guardian_normals.tif", false);
        _model_diffuse_map = tl.load_texture_2d(*device, "../../../res/geometry/guardian_diffuse.tif", false);
    }
    catch(std::exception& e) {
        err() << log::error
              << "application_window::init_renderer(): "
              << "error loading model geometry: " << e.what() << log::end;
        return false;
    }

    if (   !_env_map
        || !_env_map
        || !_model_normal_map) {
        err() << log::error
              << "application_window::init_renderer(): "
              << "error loading textures." << log::end;
        return false;
    }

    _time_query    = device->create_timer_query();
    _time_query_00 = device->create_timer_query();
    _time_query_01 = device->create_timer_query();

    _frame_number      = 0;
    _timer_start_frame = 0;

    _timer_available   = true;
    _fence_available   = true;

    _animation_enabled = false;
    _rotation_angle    = 0.0f;

    _model_grid_res     = vec3ui(3);
    _model_grid_queries = false;
    _model_grid_queries_available = true;
    _model_grid_queries_start_frame = 0;
    for (unsigned gz = 0; gz < _model_grid_res.z; ++gz) {
        for (unsigned gy = 0; gy < _model_grid_res.y; ++gy) {
            for (unsigned gx = 0; gx < _model_grid_res.x; ++gx) {
                _time_queries.push_back(device->create_timer_query());
            }
        }
    }

    _enable_output = false;

    return true;
}

bool
application_window::reload_shaders(const gl::render_device_ptr& device)
{
    using namespace scm;
    using namespace scm::gl;
    using namespace scm::math;
    using boost::assign::list_of;

    program_ptr p  = device->create_program(list_of(device->create_shader_from_file(STAGE_VERTEX_SHADER,   "../../../src/shaders/simple.glslv"))
                                                   (device->create_shader_from_file(STAGE_GEOMETRY_SHADER, "../../../src/shaders/simple.glslg"))
                                                   (device->create_shader_from_file(STAGE_FRAGMENT_SHADER, "../../../src/shaders/simple.glslf")),
                                            "shader_prog");

    if (!p) {
        scm::err() << log::error
                   << "application_window::reload_shaders(): "
                   << "error loading program" << log::end;
        return false;
    }
    else {
        _shader_prog = p;
        return true;
    }
}

void
application_window::shutdown()
{
    _shader_prog.reset();

    _rstate.reset();
    _bstate.reset();
    _dstate.reset();
    _sstate.reset();

    _model.reset();
    _model_normal_map.reset();
    _model_diffuse_map.reset();
    _env_map.reset();
    _env_map_diff.reset();

    _time_query.reset();
    _time_query_00.reset();
    _time_query_01.reset();

    _fence.reset();
    _fence_00.reset();
    _fence_01.reset();
}

void
application_window::update(const gl::render_device_ptr& device,
                           const gl::render_context_ptr& context)
{
}

void
application_window::display(const gl::render_context_ptr& context)
{
    ++_frame_number;

    using namespace scm::gl;
    using namespace scm::math;

    float deg_per_sec = 180.0f;

    if (_animation_enabled) {
        _rotation_angle += deg_per_sec * (_viewer->frame_time_us() / 1e6f);
    }

    const mat4f rotation_matrix  = make_rotation(_rotation_angle, vec3f(0.0f, 1.0f, 0.0f));

#if 1 // check overhead of time queries
    const vec3i grid_res = vec3i(_model_grid_res);

    std::vector<mat4f>  view_matrices;

    const int   max_grid = max(grid_res.x, max(grid_res.y, grid_res.z));
    const float scale    = 0.4f / max_grid;
    const vec3f spacing  = vec3f(1.0f) / vec3f(grid_res);
    const vec3f offset   = vec3f(grid_res - 1) / 2.0f;
    const mat4f scale_mat = make_scale(vec3f(scale));
    for (int gz = 0; gz < grid_res.z; ++gz) {
        for (int gy = 0; gy < grid_res.y; ++gy) {
            for (int gx = 0; gx < grid_res.x; ++gx) {
                const vec3f o = vec3f(gx, gy, gz);
                const mat4f t = make_translation((o - offset) * spacing);
                const mat4f v = _viewer->main_camera().view_matrix() * t * rotation_matrix * scale_mat;
                view_matrices.push_back(v);
            }
        }
    }

    const mat4f& proj_matrix         = _viewer->main_camera().projection_matrix();

    gl::uniform_mat4f_ptr view_mat_uniform = _shader_prog->uniform_mat4f("model_view_matrix");
    {
        context_program_guard       cpg(context);
        context_state_objects_guard csg(context);
        context_texture_units_guard tsg(context);

        _shader_prog->uniform("projection_matrix", proj_matrix);

        context->bind_program(_shader_prog);

        context->bind_texture(_env_map,           _sstate, 0);
        context->bind_texture(_env_map_diff,      _sstate, 1);
        context->bind_texture(_model_normal_map,  _sstate, 2);
        context->bind_texture(_model_diffuse_map, _sstate, 3);

        context->set_rasterizer_state(_rstate);
        context->set_blend_state(_bstate);
        context->set_depth_stencil_state(_dstate);

        for (size_t vs = 0; vs < view_matrices.size(); ++vs) {
            const mat4f& v = view_matrices[vs];
            view_mat_uniform->set_value(0, v);

            if (_model_grid_queries && _model_grid_queries_available) {
                context->begin_query(_time_queries[vs]);
            }
            _model->draw(context);
            //_model->draw_raw(context);
            if (_model_grid_queries && _model_grid_queries_available) {
                context->end_query(_time_queries[vs]);

                if (vs == view_matrices.size() - 1) {
                    _model_grid_queries_available   = false;
                    _model_grid_queries_start_frame = _frame_number;
                    _model_grid_last_query = _time_queries[vs];
                }
            }
        }
    }

    if (_model_grid_last_query && _model_grid_last_query != _time_queries.back()) {
        out() << _model_grid_last_query.get() << " " << _time_queries.back().get() << log::end;
    }
    //if (_model_grid_queries && context->query_result_available(_time_queries.back())) {
    if (_model_grid_last_query && context->query_result_available(_model_grid_last_query)) {
        scm::uint64 frame_delay = _frame_number - _model_grid_queries_start_frame;
        double combined_draw_time = 0.0;
        for (size_t vs = 0; vs < _time_queries.size(); ++vs) {
            context->collect_query_results(_time_queries[vs]);
            combined_draw_time += static_cast<double>(_time_queries[vs]->result()) / 1e3;
        }
        double avg_draw_time = combined_draw_time / static_cast<double>(_time_queries.size());
        if (_enable_output) {
            out() << "grid timer available with " << frame_delay << " frames delay, avg draw time: "
                  << std::fixed << std::setprecision(3) << avg_draw_time << "us." << log::end;
        }
        _model_grid_queries_available = true;
        _model_grid_last_query.reset();
    }

    //context->collect_query_results(_time_query_00);
    //context->collect_query_results(_time_query_01);
    //double draw_time_00 = time::to_milliseconds(time::nanosec(_time_query_00->result()));
    //double draw_time_01 = time::to_milliseconds(time::nanosec(_time_query_01->result()));
    //out() << std::fixed << std::setprecision(3)
    //      << "draw time: " << draw_time_00 << "ms, " << draw_time_01 << "ms, " << log::end;

#endif

#if 0 // check timing of overlapped drawcalls

    const mat4f  model_matrix_00     = make_translation(-0.9f, 0.0f, 0.0f);// * make_scale(vec3f(0.2f));
    const mat4f  model_matrix_01     = make_translation( 0.9f, 0.0f, 0.0f);// * make_scale(vec3f(0.2f));
    const mat4f& view_matrix_00      = _viewer->main_camera().view_matrix() * model_matrix_00;
    const mat4f& view_matrix_01      = _viewer->main_camera().view_matrix() * model_matrix_01;
    const mat4f& proj_matrix         = _viewer->main_camera().projection_matrix();

    gl::uniform_mat4f_ptr view_mat_uniform = _shader_prog->uniform_mat4f("model_view_matrix");

    {
        context_program_guard       cpg(context);
        context_state_objects_guard csg(context);
        context_texture_units_guard tsg(context);

        _shader_prog->uniform("projection_matrix", proj_matrix);

        context->bind_program(_shader_prog);

        context->bind_texture(_env_map,      _sstate, 0);
        context->bind_texture(_env_map_diff, _sstate, 1);
        context->bind_texture(_env_map,           _sstate, 0);
        context->bind_texture(_env_map_diff,      _sstate, 1);
        context->bind_texture(_model_normal_map,  _sstate, 2);
        context->bind_texture(_model_diffuse_map, _sstate, 3);

        context->set_rasterizer_state(_rstate);
        context->set_blend_state(_bstate);
        context->set_depth_stencil_state(_dstate);

        { // 00 left
            context->begin_query(_time_query_00);
            view_mat_uniform->value(0, view_matrix_00);
            _model->draw_raw(context);
            context->end_query(_time_query_00);
        }

        { // 01 right
            context->begin_query(_time_query_01);
            view_mat_uniform->value(0, view_matrix_01);
            _model->draw_raw(context);
            context->end_query(_time_query_01);
        }
    }

    context->collect_query_results(_time_query_00);
    context->collect_query_results(_time_query_01);
    double draw_time_00 = time::to_milliseconds(time::nanosec(_time_query_00->result()));
    double draw_time_01 = time::to_milliseconds(time::nanosec(_time_query_01->result()));
    out() << std::fixed << std::setprecision(3)
          << "draw time: " << draw_time_00 << "ms, " << draw_time_01 << "ms, " << log::end;

#endif

#if 0 // check frame latency of timer availability
    const mat4f  model_matrix        = make_scale(vec3f(0.2f));
    const mat4f& view_matrix         = _viewer->main_camera().view_matrix() * model_matrix;
    const mat4f& proj_matrix         = _viewer->main_camera().projection_matrix();

    {
        context_program_guard       cpg(context);
        context_state_objects_guard csg(context);
        context_texture_units_guard tsg(context);

        _shader_prog->uniform("projection_matrix", proj_matrix);
        _shader_prog->uniform("model_view_matrix", view_matrix);

        context->bind_program(_shader_prog);

        context->bind_texture(_env_map,      _sstate, 0);
        context->bind_texture(_env_map_diff, _sstate, 1);
        context->bind_texture(_env_map,           _sstate, 0);
        context->bind_texture(_env_map_diff,      _sstate, 1);
        context->bind_texture(_model_normal_map,  _sstate, 2);
        context->bind_texture(_model_diffuse_map, _sstate, 3);

        context->set_rasterizer_state(_rstate);
        context->set_blend_state(_bstate);
        context->set_depth_stencil_state(_dstate);

        if (_timer_available) {
            context->begin_query(_time_query);
        }
        _model->draw_raw(context);

        if (_timer_available) {
            context->end_query(_time_query);
            _timer_start_frame = _frame_number;
            _timer_available   = false;
        }
    }

    // DO NOT DO THIS!!!!!!!
    //context->collect_query_results(_time_query);

    // DO THIS...
    if (context->query_result_available(_time_query)) {
        scm::uint64 frame_delay = _frame_number - _timer_start_frame;
        out() << "timer available with " << frame_delay << " frames delay.";
        _timer_available = true;
    }
#endif

#if 0 // check frame latency of fence availability
    const mat4f  model_matrix        = make_scale(vec3f(0.2f)) * rotation_matrix;
    const mat4f& view_matrix         = _viewer->main_camera().view_matrix() * model_matrix;
    const mat4f& proj_matrix         = _viewer->main_camera().projection_matrix();

    {
        context_program_guard       cpg(context);
        context_state_objects_guard csg(context);
        context_texture_units_guard tsg(context);

        _shader_prog->uniform("projection_matrix", proj_matrix);
        _shader_prog->uniform("model_view_matrix", view_matrix);

        context->bind_program(_shader_prog);

        context->bind_texture(_env_map,      _sstate, 0);
        context->bind_texture(_env_map_diff, _sstate, 1);
        context->bind_texture(_env_map,           _sstate, 0);
        context->bind_texture(_env_map_diff,      _sstate, 1);
        context->bind_texture(_model_normal_map,  _sstate, 2);
        context->bind_texture(_model_diffuse_map, _sstate, 3);

        context->set_rasterizer_state(_rstate);
        context->set_blend_state(_bstate);
        context->set_depth_stencil_state(_dstate);

        _model->draw_raw(context);
        if (_fence_available) {
            _fence                 = context->insert_fence_sync();
            _fence_insertion_frame = _frame_number;
            _fence_available       = false;
        }
    }

    // DO NOT DO THIS!!!!!!!
    //if (context->sync_client_wait(_fence) != SYNC_WAIT_CONDITION_SATISFIED) {
    //    err() << "fence wait failed." << log::end;
    //}

    // DO THIS...
    if (context->sync_signal_status(_fence) == SYNC_SIGNALED) {
        scm::uint64 frame_delay = _frame_number - _fence_insertion_frame;
        out() << "fence available with " << frame_delay << " frames delay.";
        _fence_available = true;
    }
#endif
}

void
application_window::reshape(const gl::render_device_ptr& device,
                            const gl::render_context_ptr& context,
                            int w, int h)
{
    viewer_window::reshape(device, context, w, h);
}

void
application_window::keyboard_input(int k, bool state, scm::uint32 mod)
{
    //out() << "application_window::keyboard_input()" << log::end;
    viewer_window::keyboard_input(k, state, mod);
    if (state) { // only fire on key down
        switch(k) {
            case Qt::Key_Escape:    close_program();break;
            case Qt::Key_Space:     _animation_enabled = !_animation_enabled;break;
            case Qt::Key_G:         _model_grid_queries = !_model_grid_queries;break;
            case Qt::Key_O:         _enable_output = !_enable_output;break;
            case Qt::Key_S:         reload_shaders(_viewer->device());break;
            default:;
        }
    }
    //switch(k) { // key toggles
    //    default:;
    //}
}

void
application_window::mouse_double_click(gl::viewer::mouse_button b, int x, int y)
{
    //out() << "application_window::mouse_double_click()" << log::end;
}

void
application_window::mouse_press(gl::viewer::mouse_button b, int x, int y)
{
    //out() << "application_window::mouse_press()" << log::end;
}

void
application_window::mouse_release(gl::viewer::mouse_button b, int x, int y)
{
    //out() << "application_window::mouse_release()" << log::end;
}

void
application_window::mouse_move(gl::viewer::mouse_button b, int x, int y)
{
    //out() << "application_window::mouse_move()" << log::end;
}

} // namespace gui
} // namespace gl
} // namespace scm


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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_ARITHMETIC_FUNCTIONS_REMAINDER_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_REMAINDER_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
      @brief  remainder generic tag

      Represents the remainder function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct remainder_ : ext::elementwise_<remainder_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<remainder_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_remainder_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::remainder_, Site> dispatching_remainder_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::remainder_, Site>();
   }
   template<class... Args>
   struct impl_remainder_;
  }
  /*!
    Computes the remainder of division.
    The return value is a0-n*a1, where n is the value a0/a1,
    rounded to the nearest integer (using round2even).

    @par semantic:
    For any given value @c x, @c y of type @c T:

    @code
    T r = remainder(x, y);
    @endcode

    For floating point values the code is equivalent to:

    @code
    T r = x-divround2even(x, y)*y;
    @endcode

    @par Note:

    As r can be negative, @c remainder is not defined for unsigned types.

    @see @funcref{mod}, @funcref{rem}
    @par Alias

    @c drem

    @param  a0
    @param  a1

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::remainder_, remainder, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::remainder_, drem, 2)
    BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::remainder_, remround2even, 2)
} }

#endif