// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <cassert>
#include <functional>
#include <iostream>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Convolution.hpp>
#include <poplibs_test/Norms.hpp>
#include <poplibs_test/Util.hpp>
#include <poplibs_test/exceptions.hpp>
#include <poplin/codelets.hpp>
#include <popnn/BatchNorm.hpp>
#include <popnn/GroupNorm.hpp>
#include <popnn/InstanceNorm.hpp>
#include <popnn/LayerNorm.hpp>
#include <popnn/Norms.hpp>
#include <popnn/codelets.hpp>
#include <popops/ElementWise.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <tuple>

// Tolerances used in tests
#define FLOAT_REL_TOL 0.1
#define HALF_REL_TOL 0.2
#define FLOAT_ABS_TOL 1e-5
#define HALF_ABS_TOL 7e-2

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

const OptionFlags engineOptions;

static std::pair<poplibs_test::norm::NormType, unsigned>
parseTestType(const std::string &testTypeString,
              const std::vector<std::size_t> &dims, unsigned numGroups) {
  if (testTypeString == "BN") {
    return std::make_pair(poplibs_test::norm::NormType::BatchNorm, numGroups);
  } else if (testTypeString == "GN") {
    return std::make_pair(poplibs_test::norm::NormType::GroupNorm, numGroups);
  } else if (testTypeString == "IN") {
    return std::make_pair(poplibs_test::norm::NormType::InstanceNorm, dims[1]);
  } else if (testTypeString == "LN") {
    return std::make_pair(poplibs_test::norm::NormType::LayerNorm, 1);
  } else {
    throw poplibs_test::poplibs_test_error("Invalid test type");
  }
}

static std::pair<Tensor, Tensor>
normStatistics(Graph &graph, const Tensor acts, float eps, Sequence &prog,
               bool unbiasedVarEstimate, bool stableAlgo,
               const Type &partialsType, const std::string &debugPrefix,
               unsigned numGroups, poplibs_test::norm::NormType normType,
               const poplar::OptionFlags &options) {
  switch (normType) {
  case poplibs_test::norm::NormType::BatchNorm:
    return bn::batchNormStatistics(graph, acts, eps, prog, unbiasedVarEstimate,
                                   stableAlgo, partialsType, debugPrefix);
  case poplibs_test::norm::NormType::GroupNorm:
    return gn::groupNormStatistics(graph, acts, eps, prog, numGroups,
                                   unbiasedVarEstimate, stableAlgo,
                                   partialsType, debugPrefix, options);
  case poplibs_test::norm::NormType::LayerNorm:
    return ln::layerNormStatistics(graph, acts, eps, prog, unbiasedVarEstimate,
                                   stableAlgo, partialsType, debugPrefix);
  case poplibs_test::norm::NormType::InstanceNorm:
    return in::instanceNormStatistics(graph, acts, eps, prog,
                                      unbiasedVarEstimate, stableAlgo,
                                      partialsType, debugPrefix);
  }
  throw poplibs_test::poplibs_test_error("Invalid normType");
}

std::pair<Tensor, Tensor> normalise(Graph &graph, const Tensor &acts,
                                    const Tensor &gamma, const Tensor &beta,
                                    const Tensor &mean, const Tensor &iStdDev,
                                    Sequence &prog,
                                    const std::string &debugPrefix,
                                    poplibs_test::norm::NormType normType,
                                    const poplar::OptionFlags &options) {
  switch (normType) {
  case poplibs_test::norm::NormType::BatchNorm:
    return bn::batchNormalise(graph, acts, gamma, beta, mean, iStdDev, prog,
                              debugPrefix, options);
  case poplibs_test::norm::NormType::GroupNorm:
    return gn::groupNormalise(graph, acts, gamma, beta, mean, iStdDev, prog,
                              debugPrefix, options);
  case poplibs_test::norm::NormType::LayerNorm:
    return ln::layerNormalise(graph, acts, gamma, beta, mean, iStdDev, prog,
                              debugPrefix, options);
  case poplibs_test::norm::NormType::InstanceNorm:
    return in::instanceNormalise(graph, acts, gamma, beta, mean, iStdDev, prog,
                                 debugPrefix, options);
  }
  throw poplibs_test::poplibs_test_error("Invalid norm type");
}

static std::pair<Tensor, Tensor>
normParamGradients(Graph &graph, const Tensor &acts, const Tensor &gradsIn,
                   const Tensor &mean, const Tensor &iStdDev, Sequence &prog,
                   const Type &partialsType, const std::string &debugPrefix,
                   poplibs_test::norm::NormType normType,
                   const poplar::OptionFlags &options) {
  switch (normType) {
  case poplibs_test::norm::NormType::BatchNorm:
    return bn::batchNormParamGradients(graph, acts, gradsIn, mean, iStdDev,
                                       prog, partialsType, debugPrefix,
                                       options);
  case poplibs_test::norm::NormType::GroupNorm:
    return gn::groupNormParamGradients(graph, acts, gradsIn, mean, iStdDev,
                                       prog, partialsType, debugPrefix,
                                       options);
  case poplibs_test::norm::NormType::LayerNorm:
    return ln::layerNormParamGradients(graph, acts, gradsIn, mean, iStdDev,
                                       prog, partialsType, debugPrefix,
                                       options);
  case poplibs_test::norm::NormType::InstanceNorm:
    return in::instanceNormParamGradients(graph, acts, gradsIn, mean, iStdDev,
                                          prog, partialsType, debugPrefix,
                                          options);
  }
  throw poplibs_test::poplibs_test_error("Invalid normType");
}

static Tensor normGradients(Graph &graph, const Tensor &acts,
                            const Tensor &gradsIn, const Tensor &mean,
                            const Tensor &iStdDev, const Tensor &gamma,
                            Sequence &prog, const Type &partialsType,
                            const std::string &debugPrefix,
                            poplibs_test::norm::NormType normType,
                            const poplar::OptionFlags &options) {
  switch (normType) {
  case poplibs_test::norm::NormType::BatchNorm:
    return bn::batchNormGradients(graph, acts, gradsIn, mean, iStdDev, gamma,
                                  prog, partialsType, debugPrefix, options);
  case poplibs_test::norm::NormType::GroupNorm:
    return gn::groupNormGradients(graph, acts, gradsIn, mean, iStdDev, gamma,
                                  prog, partialsType, debugPrefix, options);
  case poplibs_test::norm::NormType::LayerNorm:
    return ln::layerNormGradients(graph, acts, gradsIn, mean, iStdDev, gamma,
                                  prog, partialsType, debugPrefix, options);
  case poplibs_test::norm::NormType::InstanceNorm:
    return in::instanceNormGradients(graph, acts, gradsIn, mean, iStdDev, gamma,
                                     prog, partialsType, debugPrefix, options);
  }
  throw poplibs_test::poplibs_test_error("Invalid normType");
}

static void normParamUpdate(poplar::Graph &graph,
                            const poplar::Tensor &gammaDelta,
                            const poplar::Tensor &betaDelta, float learningRate,
                            poplar::Tensor &gamma, poplar::Tensor &beta,
                            poplar::program::Sequence &prog,
                            const std::string &debugPrefix,
                            poplibs_test::norm::NormType normType,
                            const poplar::OptionFlags &options) {
  auto scale = graph.addConstant(beta.elementType(), {}, -learningRate);
  graph.setTileMapping(scale, 0);
  switch (normType) {
  case poplibs_test::norm::NormType::BatchNorm:
    bn::batchNormParamUpdate(graph, gammaDelta, betaDelta, scale, gamma, beta,
                             prog, debugPrefix, options);
    return;
  case poplibs_test::norm::NormType::GroupNorm:
    gn::groupNormParamUpdate(graph, gammaDelta, betaDelta, scale, gamma, beta,
                             prog, debugPrefix, options);
    return;
  case poplibs_test::norm::NormType::LayerNorm:
    ln::layerNormParamUpdate(graph, gammaDelta, betaDelta, scale, gamma, beta,
                             prog, debugPrefix, options);
    return;
  case poplibs_test::norm::NormType::InstanceNorm:
    in::instanceNormParamUpdate(graph, gammaDelta, betaDelta, scale, gamma,
                                beta, prog, debugPrefix, options);
    return;
  }
}

static bool normTest(const DeviceType &deviceType,
                     const std::vector<std::size_t> dims, float eps,
                     float learningRate, unsigned tilesPerIPU,
                     const Type &dataType, bool unbiasedVarEstimate,
                     bool stableAlgo, const Type &partialsType,
                     poplibs_test::norm::NormType normType, unsigned numGroups,
                     bool groupNormStridedChannelGrouping, bool dumpProfile,
                     bool compile_only) {
  assert(dims.size() >= 2);
  poplar::OptionFlags options = {
      {"groupNormStridedChannelGrouping",
       groupNormStridedChannelGrouping ? "true" : "false"}};

  const auto batchSize = dims[0];
  const auto numChannels = dims[1];

  auto device = createTestDevice(deviceType, 1, tilesPerIPU);
  const auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);
  popnn::addCodelets(graph);
  poplin::addCodelets(graph);

  const auto fieldSize = std::accumulate(dims.begin() + 2, dims.end(), 1U,
                                         std::multiplies<std::size_t>());

  std::vector<std::size_t> actDims;
  actDims.push_back(batchSize);
  actDims.resize(dims.size() - 1);
  std::copy(dims.begin() + 2, dims.end(), actDims.begin() + 1);
  actDims.push_back(dims[1]);

  auto acts = graph.addVariable(dataType, actDims, "act");
  poputil::mapTensorLinearly(graph, acts);
  acts = acts.dimShufflePartial({acts.rank() - 1}, {1});

  auto prog = Sequence();

  Tensor mean, invStdDev;
  const bool isBatchNorm = normType == poplibs_test::norm::NormType::BatchNorm;

  std::tie(mean, invStdDev) =
      normStatistics(graph, acts, eps, prog, unbiasedVarEstimate, stableAlgo,
                     partialsType, "", numGroups, normType, options);
  Tensor gamma, beta;
  std::tie(gamma, beta) = popnn::createNormParams(graph, acts);

  Tensor actsWhitened, actsBN;
  std::tie(actsBN, actsWhitened) = normalise(
      graph, acts, gamma, beta, mean, invStdDev, prog, "", normType, options);
  Tensor actsBNInf;
  if (isBatchNorm) {
    // create combined parameters for inference
    // The calculations here to obtain combinedScale and addendPart have to be
    // done at a higher precision as would be typical in an actual
    // implementation. Here we keep them at the data precision inorder not
    // to increase the graph size. The alternate approach would be to do the
    // calculation on the host and run a second graph to do inference alone.
    const auto combinedScale = mul(graph, gamma, invStdDev, prog);
    const auto addendPart = mul(graph, mean, combinedScale, prog);
    const auto addend = sub(graph, beta, addendPart, prog);
    actsBNInf = bn::batchNormalise(graph, acts, combinedScale, addend, prog, "",
                                   options);
  } else {
    actsBNInf = actsBN;
  }

  auto gradsIn = graph.clone(actsBN);

  Tensor gammaDelta, betaDelta;
  std::tie(gammaDelta, betaDelta) =
      normParamGradients(graph, acts, gradsIn, mean, invStdDev, prog,
                         partialsType, "", normType, options);
  auto gradsOut = normGradients(graph, acts, gradsIn, mean, invStdDev, gamma,
                                prog, partialsType, "", normType, options);

  normParamUpdate(graph, gammaDelta, betaDelta, learningRate, gamma, beta, prog,
                  "", normType, options);

  std::vector<std::pair<std::string, char *>> tmap;
  Sequence uploadProg, downloadProg;
  auto rawHostActs = allocateHostMemoryForTensor(
      acts, "acts", graph, uploadProg, downloadProg, tmap);
  auto rawHostActsBN = allocateHostMemoryForTensor(
      actsBN, "actsBN", graph, uploadProg, downloadProg, tmap);
  auto rawHostActsBNInf = allocateHostMemoryForTensor(
      actsBNInf, "actsBNInf", graph, uploadProg, downloadProg, tmap);
  auto rawHostGradsIn = allocateHostMemoryForTensor(
      gradsIn, "gradsIn", graph, uploadProg, downloadProg, tmap);
  auto rawHostGradsOut = allocateHostMemoryForTensor(
      gradsOut, "gradsOut", graph, uploadProg, downloadProg, tmap);
  auto rawHostMean = allocateHostMemoryForTensor(
      mean, "mean", graph, uploadProg, downloadProg, tmap);
  auto rawHostInvStdDev = allocateHostMemoryForTensor(
      invStdDev, "invStdDev", graph, uploadProg, downloadProg, tmap);
  auto rawHostGamma = allocateHostMemoryForTensor(
      gamma, "gamma", graph, uploadProg, downloadProg, tmap);
  auto rawHostBeta = allocateHostMemoryForTensor(
      beta, "beta", graph, uploadProg, downloadProg, tmap);
  unsigned numStatsElems = isBatchNorm ? numChannels : numGroups * batchSize;

  boost::multi_array<double, 3> hostActs(
      boost::extents[batchSize][numChannels][fieldSize]);
  boost::multi_array<double, 3> hostActsBN(
      boost::extents[batchSize][numChannels][fieldSize]);
  boost::multi_array<double, 3> hostActsBNInf(
      boost::extents[batchSize][numChannels][fieldSize]);
  boost::multi_array<double, 3> hostGradsIn(
      boost::extents[batchSize][numChannels][fieldSize]);
  boost::multi_array<double, 3> hostGradsOut(
      boost::extents[batchSize][numChannels][fieldSize]);
  boost::multi_array<double, 1> hostMean(boost::extents[numStatsElems]);
  boost::multi_array<double, 1> hostInvStdDev(boost::extents[numStatsElems]);
  boost::multi_array<double, 1> hostGamma(boost::extents[numChannels]);
  boost::multi_array<double, 1> hostBeta(boost::extents[numChannels]);

  std::mt19937 randomEngine;
  writeRandomValues(target, dataType, hostActs, -1.0, +5.0, randomEngine);
  writeRandomValues(target, dataType, hostGamma, 0., +6.0, randomEngine);
  writeRandomValues(target, dataType, hostBeta, -1.0, +5.0, randomEngine);
  writeRandomValues(target, dataType, hostGradsIn, 0., +4.0, randomEngine);
  auto modelGamma = hostGamma;
  auto modelBeta = hostBeta;
  auto modelGradsIn = hostGradsIn;

  copy(target, hostActs, dataType, rawHostActs.get());
  copy(target, hostGamma, dataType, rawHostGamma.get());
  copy(target, hostBeta, dataType, rawHostBeta.get());
  copy(target, hostGradsIn, dataType, rawHostGradsIn.get());

  Engine engine(graph, Sequence(uploadProg, prog, downloadProg), engineOptions);

  if (compile_only)
    return 0;

  device.bind([&](const Device &d) {
    engine.load(d);
    attachStreams(engine, tmap);
    engine.run(0); // Run.
  });

  copy(target, dataType, rawHostMean.get(), hostMean);
  copy(target, dataType, rawHostInvStdDev.get(), hostInvStdDev);
  copy(target, dataType, rawHostActsBN.get(), hostActsBN);
  copy(target, dataType, rawHostActsBNInf.get(), hostActsBNInf);
  copy(target, dataType, rawHostGradsOut.get(), hostGradsOut);
  copy(target, dataType, rawHostBeta.get(), hostBeta);
  copy(target, dataType, rawHostGamma.get(), hostGamma);

  bool matchesModel = true;

  boost::multi_array<double, 3> modelActsWhitened(
      boost::extents[batchSize][numChannels][fieldSize]);
  boost::multi_array<double, 1> modelMean(boost::extents[numStatsElems]);
  boost::multi_array<double, 1> modelInvStdDev(boost::extents[numStatsElems]);

  poplibs_test::norm::normStatistics(hostActs, eps, unbiasedVarEstimate,
                                     stableAlgo, modelMean, modelInvStdDev,
                                     normType, groupNormStridedChannelGrouping);

  boost::multi_array<double, 3> modelActsBN(
      boost::extents[batchSize][numChannels][fieldSize]);
  poplibs_test::norm::normalise(hostActs, modelGamma, modelBeta, modelMean,
                                modelInvStdDev, modelActsBN, modelActsWhitened,
                                normType, groupNormStridedChannelGrouping);
  boost::multi_array<double, 3> modelGradsOut(
      boost::extents[batchSize][numChannels][fieldSize]);

  poplibs_test::norm::normGradients(modelActsWhitened, modelGradsIn,
                                    modelInvStdDev, modelGamma, modelGradsOut,
                                    normType, groupNormStridedChannelGrouping);
  poplibs_test::norm::normParamUpdate(modelActsWhitened, modelGradsIn,
                                      learningRate, modelGamma, modelBeta,
                                      normType);

  const double relativeTolerance =
      dataType == FLOAT ? FLOAT_REL_TOL : HALF_REL_TOL;
  const double absoluteTolerance =
      dataType == FLOAT ? FLOAT_ABS_TOL : HALF_ABS_TOL;

  matchesModel &= checkIsClose("mean", hostMean, modelMean, relativeTolerance,
                               absoluteTolerance);
  matchesModel &= checkIsClose("InvStdDev", hostInvStdDev, modelInvStdDev,
                               relativeTolerance, absoluteTolerance);
  matchesModel &= checkIsClose("actsBN", hostActsBN, modelActsBN,
                               relativeTolerance, absoluteTolerance);
  matchesModel &= checkIsClose("actsBNInf", hostActsBNInf, modelActsBN,
                               relativeTolerance, absoluteTolerance);
  matchesModel &= checkIsClose("gradsOut", hostGradsOut, modelGradsOut,
                               relativeTolerance, absoluteTolerance);
  matchesModel &= checkIsClose("beta", hostBeta, modelBeta, relativeTolerance,
                               absoluteTolerance);
  matchesModel &= checkIsClose("gamma", hostGamma, modelGamma,
                               relativeTolerance, absoluteTolerance);

  if (deviceType != DeviceType::Cpu && dumpProfile) {
    engine.printProfileSummary(std::cout,
                               OptionFlags{{"showExecutionSteps", "true"}});
  }
  return matchesModel;
}

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

  DeviceType deviceType;
  float eps;
  float learningRate;
  Type dataType;
  Type partialsType;
  unsigned tilesPerIPU;
  ShapeOption<std::size_t> dims;
  std::string test;
  bool unbiasedVarEstimate = false;
  bool stableAlgo = false;
  bool groupNormStridedChannelGrouping = true;
  unsigned numGroups = 1;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Print help")
    ("compile-only", "Stop after compilation; don't run the program")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("eps",
     po::value<float>(&eps)->required(),
     "eps")
    ("learning-rate",
     po::value<float>(&learningRate)->required(),
     "Learning Rate")
    ("stable-algo-for-stats",
     po::value<bool>(&stableAlgo)->default_value(stableAlgo),
     "use stable algorithms for computing statistics")
    ("profile", "Output profiling report")
    ("data-type",
     po::value<Type>(&dataType)->required(),
     "Data Type")
    ("partials-type",
     po::value<Type>(&partialsType)->required(),
     "Partials Type")
    ("tiles-per-ipu",
     po::value<unsigned>(&tilesPerIPU)->required(),
     "Tiles per IPU")
    ("dims",
     po::value<ShapeOption<std::size_t>>(&dims)->required(),
     "Dimensions : {batch,channels, ....field....}, where field could be "
     "empty or have any dimension")
    ("unbiased-var-estimate",
     po::value<bool>(&unbiasedVarEstimate)->default_value(unbiasedVarEstimate),
     "Use unbiased variance estimate")
    ("num-groups",
     po::value<unsigned>(&numGroups)->default_value(numGroups),
     "Number of groups in group norm. Ignored for BN, LN and IN")
    ("norm-type",
     po::value<std::string>(&test)->required(),
     "Normalisation type: BN | GN | IN | LN")
    ("strided-channel-grouping",
     po::value<bool>(&groupNormStridedChannelGrouping)->
                    default_value(groupNormStridedChannelGrouping),
     "Use faster but non-standard strided channel grouping (group norm only)");
  // 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;
  }

  bool dumpProfile = vm.count("profile");
  unsigned groups;
  poplibs_test::norm::NormType normType;
  std::tie(normType, groups) = parseTestType(test, dims.val, numGroups);

  std::cerr << "\n Test ";
  std::cerr << test << " groups " << groups << " channels " << dims.val[1];

  if (dims.val.size() < 2) {
    std::cerr << "error: norm test must have tensor dimensions of at least 2";
    return 1;
  }
  auto matchesModel = normTest(
      deviceType, dims.val, eps, learningRate, tilesPerIPU, dataType,
      unbiasedVarEstimate, stableAlgo, partialsType, normType, groups,
      groupNormStridedChannelGrouping, dumpProfile, vm.count("compile-only"));
  return matchesModel ? 0 : 1;
}


#ifndef SKYLARK_QUASI_DENSE_TRANSFORM_DATA_HPP
#define SKYLARK_QUASI_DENSE_TRANSFORM_DATA_HPP

#ifndef SKYLARK_SKETCH_HPP
#error "Include top-level sketch.hpp instead of including individuals headers"
#endif

#include <vector>
#include <boost/math/distributions.hpp>

#include "boost/smart_ptr.hpp"

namespace skylark { namespace sketch {

namespace internal {

template <template <typename, typename> class Distribution,
          template <typename> class QMCSequenceType,
          typename ValueType>
struct quasi_random_samples_array_t {

public:

    typedef ValueType value_type;
    typedef Distribution<value_type, boost::math::policies::policy<> >
    distribution_type;
    typedef QMCSequenceType<value_type> sequence_type;

    quasi_random_samples_array_t() :
        _d(0), _N(0), _distribution(), _sequence(), _skip(0) {

    }

    quasi_random_samples_array_t(int d, int N,
        const distribution_type& distribution,
        const sequence_type& sequence, int skip = 0) :
        _d(d), _N(N), _distribution(distribution),
        _sequence(sequence), _skip(skip) {

    }

    quasi_random_samples_array_t(const quasi_random_samples_array_t& other) :
        _d(other._d), _N(other._N), _distribution(other._distribution),
        _sequence(other._sequence), _skip(other._skip) {

    }

    quasi_random_samples_array_t& operator=(const quasi_random_samples_array_t& other) {
        _d = other._d;
        _N = other._N;
        _distribution = other._distribution;
        _skip = other._skip;
        _sequence = other._sequence;
        return *this;
    }

    value_type operator[](size_t index) const {
        value_type baseval =
            _sequence.coordinate(_skip + (index / _d), index % _d);
        return boost::math::quantile(_distribution, baseval);
    }

private:
    size_t _d;
    size_t _N;
    distribution_type _distribution;
    sequence_type _sequence;
    size_t _skip;
};

}  // namespace internal

template <template <typename, typename> class ValueDistribution,
          template <typename> class QMCSequenceType>
struct quasi_dense_transform_data_t :
        public dense_transform_data_t<
    internal::quasi_random_samples_array_t<ValueDistribution,
                                           QMCSequenceType, double > > {

    // Note: we always generate doubles for array values,
    // but when applying to floats the size can be reduced.
    typedef double value_type;
    typedef QMCSequenceType<value_type> sequence_type;
    typedef ValueDistribution<value_type, boost::math::policies::policy<> >
    distribution_type;
    typedef internal::quasi_random_samples_array_t<ValueDistribution,
                                                   QMCSequenceType,
                                                   value_type> value_accessor_type;

    typedef dense_transform_data_t<value_accessor_type> base_t;

    /**
     * Regular constructor
     */
    quasi_dense_transform_data_t (int N, int S, double scale,
        const sequence_type& sequence, int skip,  base::context_t& context)
        : base_t(N, S, scale, context, "DistributionDenseTransform"),
          _skip(skip), _distribution(), _sequence(sequence) {

        // No scaling in "raw" form
        context = build();
    }

    virtual
    boost::property_tree::ptree to_ptree() const {
        SKYLARK_THROW_EXCEPTION (
          base::sketch_exception()
              << base::error_msg(
                 "Do not yet support serialization of generic dense transform"));

        return boost::property_tree::ptree();
    }

    quasi_dense_transform_data_t(const quasi_dense_transform_data_t& other)
        : base_t(other), _distribution(other._distribution),
          _sequence(other._sequence) {

    }

protected:

    quasi_dense_transform_data_t (int N, int S, double scale,
        const sequence_type& sequence, int skip,
        const base::context_t& context, std::string type)
        : base_t(N, S, scale, context, type),
          _skip(skip), _distribution(), _sequence(sequence) {

    }

    base::context_t build() {
        base::context_t ctx = base_t::build();

        base_t::entries =
            value_accessor_type(base_t::_N, base_t::_S, _distribution,
                _sequence, _skip);

        return ctx;
    }

    int _skip;
    distribution_type _distribution; /**< Distribution for samples */
    sequence_type _sequence;

};

} } /** namespace skylark::sketch */

#endif /** SKYLARK_QUASI_DENSE_TRANSFORM_DATA_HPP */


// HEADER
#include <path_follower/controller/robotcontroller_kinematic_HBZ.h>

// THIRD PARTY
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// PROJECT
#include <path_follower/parameters/path_follower_parameters.h>
#include <path_follower/utils/cubic_spline_interpolation.h>
#include <cslibs_navigation_utilities/MathHelper.h>
#include <path_follower/utils/pose_tracker.h>
#include <path_follower/utils/visualizer.h>
#include <path_follower/utils/obstacle_cloud.h>
#include <path_follower/collision_avoidance/collision_avoider.h>
#include <path_follower/factory/controller_factory.h>

// SYSTEM
#include <cmath>
#include <deque>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <boost/algorithm/clamp.hpp>
#include <pcl_ros/point_cloud.h>
#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_Kinematic_HBZ, kinematic_hbz, default_collision_avoider);

using namespace Eigen;

RobotController_Kinematic_HBZ::RobotController_Kinematic_HBZ():
    RobotController(),
    cmd_(this),
    vn_(0),
    Ts_(0.02),
    ind_(0),
    proj_ind_(0),
    xe_(0),
    ye_(0),
    theta_e_(0),
    x_meas_(0),
    y_meas_(0),
    theta_meas_(0),
    angular_vel_(0),
    Vl_(0),
    Vr_(0),
    delta_(0)
{

    wheel_velocities_ = nh_.subscribe<std_msgs::Float64MultiArray>("wheel_velocities", 10,
                                                                   &RobotController_Kinematic_HBZ::WheelVelocities, this);

}

void RobotController_Kinematic_HBZ::stopMotion()
{

    cmd_.speed = 0;
    cmd_.direction_angle = 0;
    cmd_.rotation = 0;

    MoveCommand mcmd = cmd_;
    publishMoveCommand(mcmd);
}

void RobotController_Kinematic_HBZ::initialize()
{
    RobotController::initialize();

    //reset the index of the current point on the path
    ind_ = 0;

    theta_e_ = 0.0;

    // desired velocity
    vn_ = std::min(PathFollowerParameters::getInstance()->max_velocity(), velocity_);
    ROS_DEBUG_STREAM("velocity_: " << velocity_ << ", vn: " << vn_);

}

void RobotController_Kinematic_HBZ::WheelVelocities(const std_msgs::Float64MultiArray::ConstPtr& array)
{
    double frw = array->data[0];
    double flw = array->data[1];
    double brw = array->data[2];
    double blw = array->data[3];

    Vl_ = (flw + blw)/2.0;
    Vr_ = (frw + brw)/2.0;
}

void RobotController_Kinematic_HBZ::start()
{

}

void RobotController_Kinematic_HBZ::reset()
{
    RobotController::reset();
}

void RobotController_Kinematic_HBZ::setPath(Path::Ptr path)
{
    RobotController::setPath(path);
}

double RobotController_Kinematic_HBZ::computeSpeed()
{
    double v = vn_;

    double V1 = 1.0/2.0*(std::pow(xe_,2) + std::pow(ye_,2) + 1.0/opt_.lambda()*fabs(sin(theta_e_-delta_)));

    if(angular_vel_ > 0){

        if(V1 >= opt_.epsilon()){
            v = (-opt_.alpha_r()*opt_.y_ICR_l()*vn_)/(opt_.y_ICR_r() - opt_.y_ICR_l());
        }
        else if(V1 < opt_.epsilon()){
            v = (opt_.alpha_r()*vn_)/(1 + std::fabs(opt_.y_ICR_r()*path_interpol.curvature(ind_)));
        }
    }
    else if(angular_vel_ <= 0){

        if(V1 >= opt_.epsilon()){
            v = (opt_.alpha_l()*opt_.y_ICR_r()*vn_)/(opt_.y_ICR_r() - opt_.y_ICR_l());
        }
        else if(V1 < opt_.epsilon()){
            v = (opt_.alpha_l()*vn_)/(1 + std::fabs(opt_.y_ICR_l()*path_interpol.curvature(ind_)));
        }
    }

    return v;
}

RobotController::MoveCommandStatus RobotController_Kinematic_HBZ::computeMoveCommand(MoveCommand *cmd)
{
    // omni drive can rotate.
    *cmd = MoveCommand(true);

    if(path_interpol.n() < 2) {
        ROS_ERROR("[Line] path is too short (N = %d)", (int) path_interpol.n());

        stopMotion();
        return MoveCommandStatus::REACHED_GOAL;
    }

    /// get the pose as pose(0) = x, pose(1) = y, pose(2) = theta
    Eigen::Vector3d current_pose = pose_tracker_->getRobotPose();

    const geometry_msgs::Twist v_meas_twist = pose_tracker_->getVelocity();
    //get the robot's current angular velocity
    angular_vel_ = pose_tracker_->getVelocity().angular.z;

    double v_meas = getDirSign() * sqrt(v_meas_twist.linear.x * v_meas_twist.linear.x
            + v_meas_twist.linear.y * v_meas_twist.linear.y);

    x_meas_ = current_pose[0];
    y_meas_ = current_pose[1];
    theta_meas_ = current_pose[2];
    ///***///

    RobotController::findOrthogonalProjection();

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

    ///compute the control for the current point on the path

    //robot direction angle in path coordinates
    theta_e_ = MathHelper::AngleDelta(path_interpol.theta_p(ind_), theta_meas_);

    //robot position vector module
    double r = hypot(x_meas_ - path_interpol.p(ind_), y_meas_ - path_interpol.q(ind_));

    //robot position vector angle in world coordinates
    double theta_r = atan2(y_meas_ - path_interpol.q(ind_), x_meas_ - path_interpol.p(ind_));

    //robot position vector angle in path coordinates
    double delta_theta = MathHelper::AngleDelta(path_interpol.theta_p(ind_), theta_r);

    //current robot position in path coordinates
    xe_ = r * cos(delta_theta);
    ye_ = r * sin(delta_theta);

    ///***///

    ///Check the driving direction, and set the complementary angle in path coordinates, if driving backwards

    if (getDirSign() < 0.0) {
        theta_e_ = MathHelper::NormalizeAngle(M_PI + theta_e_);
        ROS_WARN_THROTTLE(1, "Driving backwards...");
    }

    ///***///

    ///Lyapunov-curvature speed control - skid steering

       double v = computeSpeed();

    ///***///

    ///compute the next point on the path

    double s_old = path_interpol.s_new();

    //calculate the speed of the "virtual vehicle"
    double s_prim_tmp = v * cos(theta_e_) + opt_.x_ICR()*angular_vel_*sin(theta_e_) + opt_.k1() * xe_;
    path_interpol.set_s_prim(s_prim_tmp > 0 ? s_prim_tmp : 0);

    //approximate the first derivative and calculate the next point
    double s_temp = Ts_*path_interpol.s_prim() + s_old;
    path_interpol.set_s_new(s_temp > 0 ? s_temp : 0);

    ///***///

    double exp_factor = RobotController::exponentialSpeedControl();
    v = v * exp_factor;

    //cmd_.speed = getDirSign()*std::max((double)PathFollowerParameters::getInstance()->min_velocity(), fabs(v));
    //allow the speed to be reduced to zero
    cmd_.speed = getDirSign() * v;

    ///***///

    ///Compute the delta_ and its derivative

    double delta_old = delta_;

    delta_ = MathHelper::AngleClamp(-opt_.theta_a()*tanh(ye_));

    double delta_prim = (delta_ - delta_old)/Ts_;
    ///***///

    ///Direction control

    cmd_.direction_angle = 0;

    double theta_diff = MathHelper::AngleDelta(delta_, theta_e_);

    double trig_ratio = (fabs(sin(theta_diff)))/(sin(theta_diff)*cos(theta_diff));
    double omega = delta_prim
            - opt_.lambda()*trig_ratio*ye_*v*sin(theta_e_)
            + opt_.lambda()*trig_ratio*ye_*opt_.x_ICR()*angular_vel_*cos(theta_e_)
            - opt_.k2()*trig_ratio*(theta_diff)*(theta_diff);

    omega += path_interpol.curvature(ind_)*path_interpol.s_prim();
    cmd_.rotation = boost::algorithm::clamp(omega, -opt_.max_angular_velocity(), opt_.max_angular_velocity());

    ///***///

    ///Plot the moving reference frame together with position vector and error components

    if (visualizer_->MarrayhasSubscriber()) {
        visualizer_->drawFrenetSerretFrame(getFixedFrame(), 0, current_pose, xe_, ye_, path_interpol.p(ind_),
                                           path_interpol.q(ind_), path_interpol.theta_p(ind_));
    }

    ///***///

    ///Compute the index of the new point

    double s_diff = std::numeric_limits<double>::max();
    uint old_ind = ind_;
    //if the robot is not moving, the Frenet-Serret frame should also not move
    if(fabs(v_meas) < 1e-3){
        s_diff = 0.0;
    }

    for (unsigned int i = old_ind; i < path_interpol.n(); i++){

        double s_diff_curr = std::abs(path_interpol.s_new() - path_interpol.s(i));

        if(s_diff_curr < s_diff){

            s_diff = s_diff_curr;
            ind_ = i;

        }

    }

    if(old_ind != ind_) {
        path_->fireNextWaypointCallback();
    }

    ///***///

    if (visualizer_->hasSubscriber()) {
        visualizer_->drawSteeringArrow(pose_tracker_->getFixedFrameId(), 1, pose_tracker_->getRobotPoseMsg(), cmd_.direction_angle, 0.2, 1.0, 0.2);
    }

    ///***///

    *cmd = cmd_;

    return MoveCommandStatus::OKAY;
}

void RobotController_Kinematic_HBZ::publishMoveCommand(const MoveCommand &cmd) const
{
    geometry_msgs::Twist msg;
    msg.linear.x  = cmd.getVelocity();
    msg.linear.y  = 0;
    msg.angular.z = cmd.getRotationalVelocity();

    cmd_pub_.publish(msg);
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

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

#include "tudat/astro/basic_astro/massRateModel.h"
#include "tudat/simulation/propagation_setup//propagationSettings.h"
#include "tudat/simulation/environment_setup/body.h"
#include "tudat/simulation/propagation_setup/dynamicsSimulator.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat::propagators;
using namespace tudat::simulation_setup;
using namespace tudat::numerical_integrators;

BOOST_AUTO_TEST_SUITE( test_body_mass_propagation )

double getDummyMassRate1(
        const SystemOfBodies bodies )
{
    return ( bodies.at( "Vehicle1" )->getBodyMass( ) + 2.0 * bodies.at( "Vehicle2" )->getBodyMass( ) ) / 1.0E4;
}

double getDummyMassRate2(
        const SystemOfBodies bodies )
{
    return ( 3.0 * bodies.at( "Vehicle1" )->getBodyMass( ) + 2.0 * bodies.at( "Vehicle2" )->getBodyMass( ) ) / 1.0E4;
}

// Test mass rate of single body, linearly decreasing with time
BOOST_AUTO_TEST_CASE( testBodyMassPropagation )
{
    // Crate bodies
    SystemOfBodies bodies;
    bodies.createEmptyBody( "Vehicle" );

    // Create mass rate model.
    std::map< std::string, std::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels;
    massRateModels[ "Vehicle" ] = std::make_shared< basic_astrodynamics::CustomMassRateModel >(
                [ ]( const double ){ return -0.01; } );

    // Create settings for propagation
    Eigen::VectorXd initialMass = Eigen::VectorXd( 1 );
    initialMass( 0 ) = 500.0;
    std::shared_ptr< PropagatorSettings< double > > propagatorSettings =
            std::make_shared< MassPropagatorSettings< double > >(
                std::vector< std::string >{ "Vehicle" }, massRateModels, initialMass,
                std::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) );

    // Define numerical integrator settings.
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1.0 );

    // Create dynamics simulation object.
    SingleArcDynamicsSimulator< double, double > dynamicsSimulator(
                bodies, integratorSettings, propagatorSettings, true, false, false );

    // Test propagated solution.
    std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( );
         stateIterator != integratedState.end( ); stateIterator++ )
    {
        BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ), 500.0 - 0.01 * stateIterator->first, 1.0E-13 );
    }
}

// Test coupled mass rate of two bodies. Model ius unphysical, but has an analytical solution, and allows the internal
// workings of the mass propagation to be more rigorously tested.
BOOST_AUTO_TEST_CASE( testTwoBodyMassPropagation )
{
    // Crate bodies
    SystemOfBodies bodies;
    bodies.createEmptyBody( "Earth" );
    bodies.createEmptyBody( "Vehicle1" );
    bodies.createEmptyBody( "Vehicle2" );

    // Create mass rate models.
    std::map< std::string, std::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels;
    massRateModels[ "Vehicle1" ] = std::make_shared< basic_astrodynamics::CustomMassRateModel >(
                std::bind( &getDummyMassRate1, bodies ) );
    massRateModels[ "Vehicle2" ] = std::make_shared< basic_astrodynamics::CustomMassRateModel >(
                std::bind( &getDummyMassRate2, bodies ) );
    bodies.at( "Earth" )->setEphemeris( std::make_shared< ephemerides::ConstantEphemeris >(
                                          [ ]( ){ return Eigen::Vector6d::Zero( ); } ) );
    bodies.at( "Vehicle1" )->setEphemeris( std::make_shared< ephemerides::ConstantEphemeris >(
                                          [ ]( ){ return Eigen::Vector6d::Zero( ); }, "Earth" ) );
    bodies.at( "Vehicle2" )->setEphemeris( std::make_shared< ephemerides::ConstantEphemeris >(
                                         [ ]( ){ return Eigen::Vector6d::Zero( ); }, "Earth" ) );

    // Create settings for propagation
    Eigen::VectorXd initialMass = Eigen::VectorXd( 2 );
    initialMass( 0 ) = 500.0;
    initialMass( 1 ) = 1000.0;
    std::shared_ptr< PropagatorSettings< double > > propagatorSettings =
            std::make_shared< MassPropagatorSettings< double > >(
                std::vector< std::string >{ "Vehicle1", "Vehicle2" }, massRateModels, initialMass,
                std::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) );

    // Define numerical integrator settings.
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1.0 );

    // Create dynamics simulation object.
    SingleArcDynamicsSimulator< double, double > dynamicsSimulator(
                bodies, integratorSettings, propagatorSettings, true, false, true );

    // Test propagated solution.
    std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( );
         stateIterator != integratedState.end( ); stateIterator++ )
    {
        // Test directly
        BOOST_CHECK_CLOSE_FRACTION(
                    stateIterator->second( 0 ),
                    100.0 * ( -std::exp( -stateIterator->first / 1E4 ) +
                              6.0 * std::exp( 4.0 * stateIterator->first / 1E4 ) ), 1.0E-13 );
        BOOST_CHECK_CLOSE_FRACTION(
                    stateIterator->second( 1 ),
                    100.0 * ( std::exp( -stateIterator->first / 1E4 ) +
                              9.0 * std::exp( 4.0 * stateIterator->first / 1E4 ) ), 1.0E-13 );

        // Test reset mass solution of vehicles.
        bodies.at( "Vehicle1" )->updateMass( stateIterator->first );
        bodies.at( "Vehicle2" )->updateMass( stateIterator->first );

        BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ), bodies.at( "Vehicle1" )->getBodyMass( ),
                std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 1 ), bodies.at( "Vehicle2" )->getBodyMass( ),
                std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  testSmartProjectionCameraFactor.cpp
 *  @brief Unit tests for SmartProjectionCameraFactor Class
 *  @author Chris Beall
 *  @author Luca Carlone
 *  @author Zsolt Kira
 *  @author Frank Dellaert
 *  @date   Sept 2013
 */

#include "smartFactorScenarios.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>

using namespace boost::assign;

static bool isDebugTest = false;

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;

static Key x1(1);
Symbol l1('l', 1), l2('l', 2), l3('l', 3);
Key c1 = 1, c2 = 2, c3 = 3;

static Point2 measurement1(323.0, 240.0);

static double rankTol = 1.0;

template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
    const PinholeCamera<CALIBRATION>& camera) {
  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
      Point3(0.5, 0.1, 0.3));
  Pose3 cameraPose = camera.pose();
  Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
  typename gtsam::traits<CALIBRATION>::TangentVector d;
  d.setRandom();
  d *= 0.1;
  CALIBRATION perturbedCalibration = camera.calibration().retract(d);
  return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
}

/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, perturbCameraPose) {
  using namespace vanilla;
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
      Point3(0.5, 0.1, 0.3));
  Pose3 perturbed_level_pose = level_pose.compose(noise_pose);
  Camera actualCamera(perturbed_level_pose, K2);

  Camera expectedCamera = perturbCameraPose(level_camera);
  CHECK(assert_equal(expectedCamera, actualCamera));
}

/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor) {
  using namespace vanilla;
  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
}

/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor2) {
  using namespace vanilla;
  params.setRankTolerance(rankTol);
  SmartFactor factor1(unit2, boost::none, params);
}

/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor3) {
  using namespace vanilla;
  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(measurement1, x1);
}

/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor4) {
  using namespace vanilla;
  params.setRankTolerance(rankTol);
  SmartFactor factor1(unit2, boost::none, params);
  factor1.add(measurement1, x1);
}

/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Equals ) {
  using namespace vanilla;
  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(measurement1, x1);

  SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
  factor2->add(measurement1, x1);
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiseless ) {
  using namespace vanilla;

  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);

  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);

  double expectedError = 0.0;
  DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
  CHECK(
      assert_equal(Z_4x1,
          factor1->reprojectionErrorAfterTriangulation(values), 1e-7));
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noisy ) {

  using namespace vanilla;

  // Project one landmark into two cameras and add noise on first
  Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2);
  Point2 level_uv_right = level_camera_right.project(landmark1);

  Values values;
  values.insert(c1, level_camera);
  Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
  values.insert(c2, perturbed_level_camera_right);

  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);

  // Point is now uninitialized before a triangulation event
  EXPECT(!factor1->point());

  double expectedError = 58640;
  double actualError1 = factor1->error(values);
  EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1);

  // Check triangulated point
  CHECK(factor1->point());
  EXPECT(
      assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4));

  // Check whitened errors
  Vector expected(4);
  expected << -7, 235, 58, -242;
  SmartFactor::Cameras cameras1 = factor1->cameras(values);
  Point3 point1 = *factor1->point();
  Vector actual = factor1->whitenedError(cameras1, point1);
  EXPECT(assert_equal(expected, actual, 1));

  SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
  vector<Point2> measurements;
  measurements.push_back(level_uv);
  measurements.push_back(level_uv_right);

  vector<Key> views;
  views.push_back(c1);
  views.push_back(c2);

  factor2->add(measurements, views);

  double actualError2 = factor2->error(values);
  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {

  using namespace vanilla;

  // Project three landmarks into three cameras
  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  // Create and fill smartfactors
  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
  vector<Key> views;
  views.push_back(c1);
  views.push_back(c2);
  views.push_back(c3);
  smartFactor1->add(measurements_cam1, views);
  smartFactor2->add(measurements_cam2, views);
  smartFactor3->add(measurements_cam3, views);

  // Create factor graph and add priors on two cameras
  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);

  // Create initial estimate
  Values initial;
  initial.insert(c1, cam1);
  initial.insert(c2, cam2);
  initial.insert(c3, perturbCameraPose(cam3));
  if (isDebugTest)
    initial.at<Camera>(c3).print("Smart: Pose3 before optimization: ");

  // Points are now uninitialized before a triangulation event
  EXPECT(!smartFactor1->point());
  EXPECT(!smartFactor2->point());
  EXPECT(!smartFactor3->point());

  EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1);
  EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1);
  EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1);

  // Error should trigger this and initialize the points, abort if not so
  CHECK(smartFactor1->point());
  CHECK(smartFactor2->point());
  CHECK(smartFactor3->point());

  EXPECT(
      assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(),
          1e-4));
  EXPECT(
      assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(),
          1e-4));
  EXPECT(
      assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(),
          1e-4));

  // Check whitened errors
  Vector expected(6);
  expected << 256, 29, -26, 29, -206, -202;
  Point3 point1 = *smartFactor1->point();
  SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
  Vector reprojectionError = cameras1.reprojectionError(point1,
      measurements_cam1);
  EXPECT(assert_equal(expected, reprojectionError, 1));
  Vector actual = smartFactor1->whitenedError(cameras1, point1);
  EXPECT(assert_equal(expected, actual, 1));

  // Optimize
  LevenbergMarquardtParams lmParams;
  if (isDebugTest) {
    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
    lmParams.verbosity = NonlinearOptimizerParams::ERROR;
  }
  LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams);
  Values result = optimizer.optimize();

  EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-5));
  EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-5));
  EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-5));

  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
  //  VectorValues delta = GFG->optimize();

  if (isDebugTest)
    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
  EXPECT(
      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
  if (isDebugTest)
    tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {

  using namespace vanilla;

  // Project three landmarks into three cameras
  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  vector<Key> views;
  views.push_back(c1);
  views.push_back(c2);
  views.push_back(c3);

  SfM_Track track1;
  for (size_t i = 0; i < 3; ++i) {
    track1.measurements.emplace_back(i + 1, measurements_cam1.at(i));
  }
  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
  smartFactor1->add(track1);

  SfM_Track track2;
  for (size_t i = 0; i < 3; ++i) {
    track2.measurements.emplace_back(i + 1, measurements_cam2.at(i));
  }
  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
  smartFactor2->add(track2);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);

  Values values;
  values.insert(c1, cam1);
  values.insert(c2, cam2);
  values.insert(c3, perturbCameraPose(cam3));
  if (isDebugTest)
    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams lmParams;
  if (isDebugTest)
    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if (isDebugTest)
    lmParams.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();

  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
  //  VectorValues delta = GFG->optimize();

  if (isDebugTest)
    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
  EXPECT(
      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
  if (isDebugTest)
    tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {

  using namespace vanilla;

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
      measurements_cam4, measurements_cam5;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);

  vector<Key> views;
  views.push_back(c1);
  views.push_back(c2);
  views.push_back(c3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
  smartFactor3->add(measurements_cam3, views);

  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
  smartFactor4->add(measurements_cam4, views);

  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
  smartFactor5->add(measurements_cam5, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(smartFactor4);
  graph.push_back(smartFactor5);
  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);

  Values values;
  values.insert(c1, cam1);
  values.insert(c2, cam2);
  values.insert(c3, perturbCameraPoseAndCalibration(cam3));
  if (isDebugTest)
    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams lmParams;
  lmParams.relativeErrorTol = 1e-8;
  lmParams.absoluteErrorTol = 0;
  lmParams.maxIterations = 20;
  if (isDebugTest)
    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if (isDebugTest)
    lmParams.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();

  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
  //  VectorValues delta = GFG->optimize();

  if (isDebugTest)
    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
  EXPECT(
      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 20));
  if (isDebugTest)
    tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, Cal3Bundler ) {

  using namespace bundler;

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
      measurements_cam4, measurements_cam5;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);

  vector<Key> views;
  views.push_back(c1);
  views.push_back(c2);
  views.push_back(c3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
  smartFactor3->add(measurements_cam3, views);

  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
  smartFactor4->add(measurements_cam4, views);

  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
  smartFactor5->add(measurements_cam5, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);

  Values values;
  values.insert(c1, cam1);
  values.insert(c2, cam2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(c3, perturbCameraPose(cam3));
  if (isDebugTest)
    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams lmParams;
  lmParams.relativeErrorTol = 1e-8;
  lmParams.absoluteErrorTol = 0;
  lmParams.maxIterations = 20;
  if (isDebugTest)
    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if (isDebugTest)
    lmParams.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();

  if (isDebugTest)
    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
  EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
  EXPECT(
      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 1));
  if (isDebugTest)
    tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {

  using namespace bundler;

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
      measurements_cam4, measurements_cam5;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);

  vector<Key> views;
  views.push_back(c1);
  views.push_back(c2);
  views.push_back(c3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
  smartFactor3->add(measurements_cam3, views);

  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
  smartFactor4->add(measurements_cam4, views);

  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
  smartFactor5->add(measurements_cam5, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);

  Values values;
  values.insert(c1, cam1);
  values.insert(c2, cam2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(c3, perturbCameraPoseAndCalibration(cam3));
  if (isDebugTest)
    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams lmParams;
  lmParams.relativeErrorTol = 1e-8;
  lmParams.absoluteErrorTol = 0;
  lmParams.maxIterations = 20;
  if (isDebugTest)
    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if (isDebugTest)
    lmParams.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();

  if (isDebugTest)
    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
  EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
  EXPECT(
      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
  if (isDebugTest)
    tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiselessBundler ) {

  using namespace bundler;
  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);

  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);

  double actualError = factor1->error(values);

  double expectedError = 0.0;
  DOUBLES_EQUAL(expectedError, actualError, 1e-3);

  Point3 oldPoint(0,0,0); // this takes the point stored in the factor (we are not interested in this)
  if (factor1->point())
    oldPoint = *(factor1->point());

  Point3 expectedPoint(0,0,0);
  if (factor1->point(values))
    expectedPoint = *(factor1->point(values));

  EXPECT(assert_equal(expectedPoint, landmark1, 1e-3));
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {

  using namespace bundler;
  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);

  NonlinearFactorGraph smartGraph;
  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);
  smartGraph.push_back(factor1);
  double expectedError = factor1->error(values);
  double expectedErrorGraph = smartGraph.error(values);
  Point3 expectedPoint(0,0,0);
  if (factor1->point())
    expectedPoint = *(factor1->point());

  // COMMENTS:
  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
  // value in the generalGrap
  NonlinearFactorGraph generalGraph;
  SFMFactor sfm1(level_uv, unit2, c1, l1);
  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
  generalGraph.push_back(sfm1);
  generalGraph.push_back(sfm2);
  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
  Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
  Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
  double actualError = 0.5 * (e1.squaredNorm() + e2.squaredNorm());
  double actualErrorGraph = generalGraph.error(values);

  DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);
  DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7);
  DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7);
  DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {

  using namespace bundler;
  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);

  NonlinearFactorGraph smartGraph;
  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);
  smartGraph.push_back(factor1);
  Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
  Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
  Point3 expectedPoint(0,0,0);
  if (factor1->point())
    expectedPoint = *(factor1->point());

  // COMMENTS:
  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
  // value in the generalGrap
  NonlinearFactorGraph generalGraph;
  SFMFactor sfm1(level_uv, unit2, c1, l1);
  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
  generalGraph.push_back(sfm1);
  generalGraph.push_back(sfm2);
  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
  Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
  Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second;
  Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18)
      - actualFullHessian.block(0, 18, 18, 3)
          * (actualFullHessian.block(18, 18, 3, 3)).inverse()
          * actualFullHessian.block(18, 0, 3, 18);
  Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1)
      - actualFullHessian.block(0, 18, 18, 3)
          * (actualFullHessian.block(18, 18, 3, 3)).inverse()
          * actualFullInfoVector.block(18, 0, 3, 1);

  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7));
  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7));
}

/* *************************************************************************/
// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){
//
//  Values values;
//  values.insert(c1, level_camera);
//  values.insert(c2, level_camera_right);
//
//  NonlinearFactorGraph smartGraph;
//  SmartFactor::shared_ptr factor1(new SmartFactor());
//  factor1->add(level_uv, c1, unit2);
//  factor1->add(level_uv_right, c2, unit2);
//  smartGraph.push_back(factor1);
//  GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values);
//
//  Point3 expectedPoint(0,0,0);
//  if(factor1->point())
//    expectedPoint = *(factor1->point());
//
//  // COMMENTS:
//  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
//  // value in the generalGrap
//  NonlinearFactorGraph generalGraph;
//  SFMFactor sfm1(level_uv, unit2, c1, l1);
//  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
//  generalGraph.push_back(sfm1);
//  generalGraph.push_back(sfm2);
//  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
//  GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values);
//
//  double alpha = 1.0;
//
//  VectorValues yExpected, yActual, ytmp;
//  VectorValues xtmp = map_list_of
//      (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0))
//      (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0))
//      (l1, (Vec(3) << 5.5, 0.5, 1.2));
//  gfg ->multiplyHessianAdd(alpha, xtmp, ytmp);
//
//  VectorValues x = map_list_of
//      (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9))
//      (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19))
//      (l1, (Vec(3) << 5.5, 0.5, 1.2));
//
//  gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual);
//  gfg ->multiplyHessianAdd(alpha, x, yExpected);
//
//  EXPECT(assert_equal(yActual,yExpected, 1e-7));
//}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {

  using namespace bundler;
  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);

  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);
  Matrix expectedE;
  Vector expectedb;

  CameraSet<Camera> cameras;
  cameras.push_back(level_camera);
  cameras.push_back(level_camera_right);

  factor1->error(values); // this is important to have a triangulation of the point
  Point3 point(0,0,0);
  if (factor1->point())
    point = *(factor1->point());
  vector<Matrix29> Fblocks;
  factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);

  NonlinearFactorGraph generalGraph;
  SFMFactor sfm1(level_uv, unit2, c1, l1);
  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
  generalGraph.push_back(sfm1);
  generalGraph.push_back(sfm2);
  values.insert(l1, point); // note: we get rid of possible errors in the triangulation
  Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
  Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse();

  Matrix3 expectedVinv = factor1->PointCov(expectedE);
  EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7));
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {

  using namespace bundler;

  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);
  double rankTol = 1;
  bool useEPI = false;

  // Hessian version
  SmartProjectionParams params;
  params.setRankTolerance(rankTol);
  params.setLinearizationMode(gtsam::HESSIAN);
  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
  params.setEnableEPI(useEPI);

  SmartFactor::shared_ptr explicitFactor(
      new SmartFactor(unit2, boost::none, params));
  explicitFactor->add(level_uv, c1);
  explicitFactor->add(level_uv_right, c2);

  GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
      values);
  HessianFactor& hessianFactor =
      dynamic_cast<HessianFactor&>(*gaussianHessianFactor);

  // Implicit Schur version
  params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
  SmartFactor::shared_ptr implicitFactor(
      new SmartFactor(unit2, boost::none, params));
  implicitFactor->add(level_uv, c1);
  implicitFactor->add(level_uv_right, c2);
  GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
      implicitFactor->linearize(values);
  CHECK(gaussianImplicitSchurFactor);
  typedef RegularImplicitSchurFactor<Camera> Implicit9;
  Implicit9& implicitSchurFactor =
      dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor);

  VectorValues x = map_list_of(c1,
      (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2,
      (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished());

  VectorValues yExpected, yActual;
  double alpha = 1.0;
  hessianFactor.multiplyHessianAdd(alpha, x, yActual);
  implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected);
  EXPECT(assert_equal(yActual, yExpected, 1e-7));
}

/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");

TEST( SmartProjectionCameraFactor, serialize) {
  using namespace vanilla;
  using namespace gtsam::serializationTestHelpers;
  SmartFactor factor(unit2);

  EXPECT(equalsObj(factor));
  EXPECT(equalsXML(factor));
  EXPECT(equalsBinary(factor));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#ifndef SSMPACK_MODEL_TRANSITION_MATRIX_HPP
#define SSMPACK_MODEL_TRANSITION_MATRIX_HPP

#include <armadillo>

namespace ssmkit {
namespace map {

struct TransitionMatrix {
  using TParameter = arma::vec;
  using TConditionVAR = int;

  TransitionMatrix(arma::mat t) : transfer{t} {}
  // should not be overloaded, should not be template
  TParameter operator()(const TConditionVAR &x) const {
    return transfer.col(x);
  }

  arma::mat transfer;
};

} // namespace map
} // namespace ssmkit

#endif // SSMPACK_MODEL_TRANSITION_MATRIX_HPP


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnTfParser/ITfParser.hpp"
#include "ParserPrototxtFixture.hpp"
// This is a special case for add, which supports broadcasting.
BOOST_AUTO_TEST_SUITE(TensorflowParser)

struct BroadcastForAddFixtureSlot1 : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    BroadcastForAddFixtureSlot1()
    {
        m_Prototext = R"(
        node {
          name: "graphInput"
          op: "Placeholder"
          attr {
            key: "dtype"
            value {
              type: DT_FLOAT
            }
          }
          attr {
            key: "shape"
            value {
               shape {
               }
             }
           }
        }
        node {
          name: "Const_1"
          op: "Const"
          attr {
            key: "dtype"
            value {
              type: DT_FLOAT
            }
          }
          attr {
            key: "value"
            value {
              tensor {
                dtype: DT_FLOAT
                tensor_shape {
                }
                float_val: 4.0
                float_val: 5.0
              }
            }
          }
        }
        node {
          name: "Add"
          op: "Add"
          input: "graphInput"
          input: "Const_1"
          attr {
            key: "T"
            value {
              type: DT_FLOAT
            }
          }
        }
        )";

        SetupSingleInputSingleOutput({ 1, 2, 2, 2 }, "graphInput", "Add");
    }
};

struct BroadcastForAddFixtureSlot0 : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    BroadcastForAddFixtureSlot0()
    {
        m_Prototext = R"(
    node {
      name: "graphInput"
      op: "Placeholder"
      attr {
        key: "dtype"
        value {
          type: DT_FLOAT
        }
      }
      attr {
        key: "shape"
        value {
           shape {
           }
         }
       }
    }
    node {
      name: "Const_1"
      op: "Const"
      attr {
        key: "dtype"
        value {
          type: DT_FLOAT
        }
      }
      attr {
        key: "value"
        value {
          tensor {
            dtype: DT_FLOAT
            tensor_shape {
            }
            float_val: 4.0
            float_val: 5.0
          }
        }
      }
    }
    node {
      name: "Add"
      op: "Add"
      input: "Const_1"
      input: "graphInput"
      attr {
        key: "T"
        value {
          type: DT_FLOAT
        }
      }
    }
    )";

        SetupSingleInputSingleOutput({ 1, 2, 2, 2 }, "graphInput", "Add");
    }
};

BOOST_FIXTURE_TEST_CASE(ParseBroadcastForAddition1, BroadcastForAddFixtureSlot1)
{
    RunTest<4>({ 1.0, 1.0, 2.0, 2.0, 3.0, 3.0, 4.0, 4.0 }, { 5.0, 6.0, 6.0, 7.0, 7.0, 8.0, 8.0, 9.0 });
}

BOOST_FIXTURE_TEST_CASE(ParseBroadcastForAddition0, BroadcastForAddFixtureSlot0)
{
    RunTest<4>({ 1.0, 1.0, 2.0, 2.0, 3.0, 3.0, 4.0, 4.0 }, { 5.0, 6.0, 6.0, 7.0, 7.0, 8.0, 8.0, 9.0 });
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE TimeTableTestModule
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <timetable/TimeTable.h>

BOOST_AUTO_TEST_SUITE(TimeTable)

BOOST_AUTO_TEST_CASE(SingleTimeLineTest)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<double> table(1.0, { { 10, 0.1 } });
#else
  ozp::TimeTable<double> table(1.0);
  table.insert(10, 0.1);
#endif
  BOOST_CHECK_CLOSE(table.end_time(), 2.0, 0.01);

  int c = 0;
  while (!table.finished()) {
    BOOST_CHECK_CLOSE(table.time(), 1.0 + c*0.1, 0.01);
    BOOST_CHECK_CLOSE(table.delta(), 0.1, 0.01);
    ++table;
    ++c;
  }

  --table;
  --c;

  BOOST_CHECK_EQUAL(c, 10);
  for (int i = 0; i < 10; ++i) {
    BOOST_CHECK_CLOSE(table.time(), 1.0 +  c*0.1, 0.01);
    BOOST_CHECK_CLOSE(table.delta(), 0.1, 0.01);
    --table;
    --c;
  }
}

BOOST_AUTO_TEST_CASE(MultiTimeLinesTest)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<double> table(1.0, { { 10, 0.1 }, { 4, 0.3 }, {5, 0.2 } });
#else
  ozp::TimeTable<double> table(1.0);
  table.insert(10, 0.1);
  table.insert(4, 0.3);
  table.insert(5, 0.2);
#endif

  BOOST_CHECK_CLOSE(table.end_time(), 4.2, 0.01);

  int c = 0;
  while (!table.finished()) {
    if (c <= 10) {
      BOOST_CHECK_CLOSE(table.time(), 1.0 + c*0.1, 0.01);
      BOOST_CHECK_CLOSE(table.delta(), 0.1, 0.01);
    }
    else if (c <= 14) {
      BOOST_CHECK_CLOSE(table.time(), 2.0 + (c-10)*0.3, 0.01);
      BOOST_CHECK_CLOSE(table.delta(), 0.3, 0.01);
    }
    else  {
      BOOST_CHECK_CLOSE(table.time(), 3.2 + (c - 14)*0.2, 0.01);
      BOOST_CHECK_CLOSE(table.delta(), 0.2, 0.01);
    }
    ++table;
    ++c;
  }

  --table;
  --c;

  BOOST_CHECK_EQUAL(c, 19);
  while (c <= 0) {
    if (c <= 10) {
      BOOST_CHECK_CLOSE(table.time(), 1.0 + c*0.1, 0.01);
      BOOST_CHECK_CLOSE(table.delta(), 0.1, 0.01);
    }
    else if (c <= 14) {
      BOOST_CHECK_CLOSE(table.time(), 2.0 + (c - 10)*0.3, 0.01);
      BOOST_CHECK_CLOSE(table.delta(), 0.3, 0.01);
    }
    else  {
      BOOST_CHECK_CLOSE(table.time(), 3.2 + (c - 14)*0.2, 0.01);
      BOOST_CHECK_CLOSE(table.delta(), 0.2, 0.01);
    }
    --table;
    --c;
  }
}

BOOST_AUTO_TEST_CASE(PostFixTest)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<double> table(1.0, { { 10, 0.1 } });
#else
  ozp::TimeTable<double> table(1.0);
  table.insert(10, 0.1);
#endif
  BOOST_CHECK_CLOSE((table++).time(), 1.0, 0.01);
  BOOST_CHECK_CLOSE(table.time(), 1.1, 0.01);
  BOOST_CHECK_CLOSE((table--).time(), 1.1, 0.01);
  BOOST_CHECK_CLOSE(table.time(), 1.0, 0.01);
}

BOOST_AUTO_TEST_CASE(StartTimeChange)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<double> table(1.0, { { 10, 0.1 } });
#else
  ozp::TimeTable<double> table(1.0);
  table.insert(10, 0.1);
#endif
  BOOST_CHECK_CLOSE(table.time(), 1.0, 0.01);
  BOOST_CHECK_EQUAL(table.at_start(), true);
  table.set_start_time(2.5);
  BOOST_CHECK_CLOSE(table.time(), 2.5, 0.01);
  BOOST_CHECK_EQUAL(table.at_start(), true);

  auto c = 0;
  while (!table.finished()) {
    BOOST_CHECK_CLOSE(table.time(), 2.5 + c*0.1, 0.01);
    BOOST_CHECK_CLOSE(table.delta(), 0.1, 0.01);
    ++table;
    ++c;
  }
  BOOST_CHECK_EQUAL(table.started(), true);
  BOOST_CHECK_EQUAL(table.finished(), true);

  table.reset();
  BOOST_CHECK_CLOSE(table.time(), 2.5, 0.01);
  BOOST_CHECK_EQUAL(table.at_start(), true);
  BOOST_CHECK_EQUAL(table.started(), false);
  BOOST_CHECK_EQUAL(table.finished(), false);

}

BOOST_AUTO_TEST_CASE(Clear)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<double> table(1.0, { { 10, 0.1 } });
#else
  ozp::TimeTable<double> table(1.0);
  table.insert(10, 0.1);
#endif;
  table.clear();
  BOOST_CHECK_CLOSE(table.time(), 0, 0.01);
  table.set_start_time(3.0);
#if (_MSC_VER >= 1800)
  table.insert({ {10, 0.1} });
#else
  table.insert(10, 0.1);
#endif;

  int c = 0;
  while (!table.finished()) {
    BOOST_CHECK_CLOSE(table.time(), 3.0 + c*0.1, 0.01);
    BOOST_CHECK_CLOSE(table.delta(), 0.1, 0.01);
    ++table;
    ++c;
  }
}

BOOST_AUTO_TEST_CASE(InBetweenDouble)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<double> table(1.0, { { 10, 0.1 } });
#else
  ozp::TimeTable<double> table(1.0);
  table.insert(10, 0.1);
#endif
  BOOST_CHECK_EQUAL(table.between_start_end(0.99999), false);
  BOOST_CHECK_EQUAL(table.between_start_end(1.0), true);
  BOOST_CHECK_EQUAL(table.between_start_end(2.0), true);
  BOOST_CHECK_EQUAL(table.between_start_end(2.000001), false);

  ++table; ++table;
  BOOST_CHECK_EQUAL(table.between_start_now(1.1999999), true);
  BOOST_CHECK_EQUAL(table.between_start_now(1.2), true);
  BOOST_CHECK_EQUAL(table.between_start_now(1.200001), false);
  BOOST_CHECK_EQUAL(table.between_start_now(2.0), false);

  BOOST_CHECK_EQUAL(table.between_now_end(1.1999999), false);
  BOOST_CHECK_EQUAL(table.between_now_end(1.1999999), false);
  BOOST_CHECK_EQUAL(table.between_now_end(1.2), true);
  BOOST_CHECK_EQUAL(table.between_now_end(2.0), true);
  BOOST_CHECK_EQUAL(table.between_now_end(2.0000001), false);
}

BOOST_AUTO_TEST_CASE(InBetweenFloat)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<float> table(1.0f, { { 10, 0.1f } });
#else
  ozp::TimeTable<float> table(1.0f);
  table.insert(10, 0.1f);
#endif
  BOOST_CHECK_EQUAL(table.between_start_end(0.9999f), false);
  BOOST_CHECK_EQUAL(table.between_start_end(1.0f), true);
  BOOST_CHECK_EQUAL(table.between_start_end(2.0f), true);
  BOOST_CHECK_EQUAL(table.between_start_end(2.0001f), false);

  ++table; ++table;
  BOOST_CHECK_EQUAL(table.between_start_now(1.1999f), true);
  BOOST_CHECK_EQUAL(table.between_start_now(1.2f), true);
  BOOST_CHECK_EQUAL(table.between_start_now(1.2001f), false);
  BOOST_CHECK_EQUAL(table.between_start_now(2.0f), false);

  BOOST_CHECK_EQUAL(table.between_now_end(1.1999f), false);
  BOOST_CHECK_EQUAL(table.between_now_end(1.1999f), false);
  BOOST_CHECK_EQUAL(table.between_now_end(1.2f), true);
  BOOST_CHECK_EQUAL(table.between_now_end(2.0f), true);
  BOOST_CHECK_EQUAL(table.between_now_end(2.0001f), false);
}
BOOST_AUTO_TEST_CASE(InBetweenInt)
{
#if (_MSC_VER >= 1800)
  ozp::TimeTable<int> table(1, { { 10, 1 } });
#else
  ozp::TimeTable<int> table(1);
  table.insert(10, 1);
#endif
  BOOST_CHECK_EQUAL(table.between_start_end(0), false);
  BOOST_CHECK_EQUAL(table.between_start_end(1), true);
  BOOST_CHECK_EQUAL(table.between_start_end(11), true);
  BOOST_CHECK_EQUAL(table.between_start_end(12), false);

  ++table; ++table;
  BOOST_CHECK_EQUAL(table.between_start_now(2), true);
  BOOST_CHECK_EQUAL(table.between_start_now(3), true);
  BOOST_CHECK_EQUAL(table.between_start_now(4), false);
  BOOST_CHECK_EQUAL(table.between_start_now(12), false);

  BOOST_CHECK_EQUAL(table.between_now_end(2), false);
  BOOST_CHECK_EQUAL(table.between_now_end(3), true);
  BOOST_CHECK_EQUAL(table.between_now_end(11), true);
  BOOST_CHECK_EQUAL(table.between_now_end(12), false);
}

BOOST_AUTO_TEST_SUITE_END()


//#define BOOST_SPIRIT_X3_DEBUG
//#define DEBUG_SYMBOLS
#include <cstdlib>
#include <iostream>
#include <functional>
#include <iomanip>
#include <list>
#include <boost/fusion/adapted/struct.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/spirit/home/x3.hpp>
#include <boost/spirit/home/x3/support/ast/variant.hpp>
#include <boost/foreach.hpp>

namespace x3 = boost::spirit::x3;

namespace client {
namespace ast {
    struct nil {};
    struct unary_op;
    struct binary_op;
    struct conditional_op;
    struct expression;

    using UnFunc = std::function<double(double)>;
    using BinFunc = std::function<double(double, double)>;

    struct operand : x3::variant<
                         nil,
                         double,
                         std::string,
                         x3::forward_ast<unary_op>,
                         x3::forward_ast<binary_op>,
                         x3::forward_ast<conditional_op>,
                         x3::forward_ast<expression>> {
        using base_type::base_type;
        using base_type::operator=;
    };

    struct unary_op {
        UnFunc op;
        operand rhs;
    };

    struct binary_op {
        BinFunc op;
        operand lhs;
        operand rhs;
    };

    struct conditional_op {
        operand lhs;
        operand rhs_true;
        operand rhs_false;
    };

    struct operation {
        BinFunc op;
        operand rhs;
    };

    struct expression {
        operand lhs;
        std::list<operation> rhs;
    };

} // namespace ast
} // namespace client

BOOST_FUSION_ADAPT_STRUCT(client::ast::expression, lhs, rhs)
BOOST_FUSION_ADAPT_STRUCT(client::ast::operation, op, rhs)
BOOST_FUSION_ADAPT_STRUCT(client::ast::conditional_op, lhs, rhs_true, rhs_false)
BOOST_FUSION_ADAPT_STRUCT(client::ast::binary_op, op, lhs, rhs)
BOOST_FUSION_ADAPT_STRUCT(client::ast::unary_op, op, rhs)

namespace client {
namespace parser {
    struct error_handler {
        template <typename It, typename Ctx>
        x3::error_handler_result
        on_error(It f, It l, x3::expectation_failure<It> const& e, Ctx const&)
            const {
            std::cout << std::string(f, l) << "\n"
                      << std::setw(1 + std::distance(f, e.where())) << "^"
                      << "-- expected: " << e.which() << "\n";
            return x3::error_handler_result::fail;
        }
    };

    struct expression_class : error_handler {};
    struct logical_class : error_handler {};
    struct equality_class : error_handler {};
    struct relational_class : error_handler {};
    struct additive_class : error_handler {};
    struct multiplicative_class : error_handler {};
    struct factor_class : error_handler {};
    struct primary_class : error_handler {};
    struct unary_class : error_handler {};
    struct binary_class : error_handler {};
    struct conditional_class : error_handler {};
    struct variable_class : error_handler {};

    // Rule declarations
    auto const expression =
        x3::rule<expression_class, ast::operand>{"expression"};
    auto const conditional =
        x3::rule<conditional_class, ast::operand>{"conditional"};
    auto const primary = x3::rule<primary_class, ast::operand>{"primary"};
    auto const logical = x3::rule<logical_class, ast::expression>{"logical"};
    auto const equality = x3::rule<equality_class, ast::expression>{"equality"};
    auto const relational =
        x3::rule<relational_class, ast::expression>{"relational"};
    auto const additive = x3::rule<additive_class, ast::expression>{"additive"};
    auto const multiplicative =
        x3::rule<multiplicative_class, ast::expression>{"multiplicative"};
    auto const factor = x3::rule<factor_class, ast::expression>{"factor"};
    auto const unary = x3::rule<unary_class, ast::unary_op>{"unary"};
    auto const binary = x3::rule<binary_class, ast::binary_op>{"binary"};
    auto const variable = x3::rule<variable_class, std::string>{"variable"};

    struct constant_ : x3::symbols<double> {
        constant_() {
            this->add("e", boost::math::constants::e<double>())(
                "pi", boost::math::constants::pi<double>());
        }
    } constant;

    struct ufunc_ : x3::symbols<ast::UnFunc> {
        ufunc_() {
            this->add("abs", &std::abs<double>);
        }
    } ufunc;

    struct bfunc_ : x3::symbols<ast::BinFunc> {
        bfunc_() {
            this->add("max", [](double a, double b) {
                return std::fmax(a, b);
            })("min", [](double a, double b) {
                return std::fmin(a, b);
            })("pow", [](double a, double b) { return std::pow(a, b); });
        }
    } bfunc;

    struct unary_op_ : x3::symbols<ast::UnFunc> {
        unary_op_() {
            this->add("+", [](double v) { return +v; })(
                "-", std::negate<double>{})("!", [](double v) { return !v; });
        }
    } unary_op;

    struct additive_op_ : x3::symbols<ast::BinFunc> {
        additive_op_() {
            this->add("+", std::plus<double>{})("-", std::minus<double>{});
        }
    } additive_op;

    struct multiplicative_op_ : x3::symbols<ast::BinFunc> {
        multiplicative_op_() {
            this->add("*", std::multiplies<>{})("/", std::divides<>{})(
                "%", [](double a, double b) { return std::fmod(a, b); });
        }
    } multiplicative_op;

    struct logical_op_ : x3::symbols<ast::BinFunc> {
        logical_op_() {
            this->add("&&", std::logical_and<double>{})(
                "||", std::logical_or<double>{});
        }
    } logical_op;

    struct relational_op_ : x3::symbols<ast::BinFunc> {
        relational_op_() {
            this->add("<", std::less<double>{})(
                "<=", std::less_equal<double>{})(">", std::greater<double>{})(
                ">=", std::greater_equal<double>{});
        }
    } relational_op;

    struct equality_op_ : x3::symbols<ast::BinFunc> {
        equality_op_() {
            this->add("==", std::equal_to<double>{})(
                "!=", std::not_equal_to<double>{});
        }
    } equality_op;

    struct power_ : x3::symbols<ast::BinFunc> {
        power_() {
            this->add(
                "**", [](double v, double exp) { return std::pow(v, exp); });
        }
    } power;

    auto const variable_def = x3::lexeme[x3::alpha >> *x3::alnum];

    // Rule defintions
    auto const expression_def = conditional;

    auto make_conditional_op = [](auto& ctx) {
        using boost::fusion::at_c;
        x3::_val(ctx) = ast::conditional_op{
            x3::_val(ctx), at_c<0>(x3::_attr(ctx)), at_c<1>(x3::_attr(ctx))};
    };

    auto const conditional_def =
        logical[([](auto& ctx) { _val(ctx) = _attr(ctx); })] >>
        -('?' > expression > ':' > expression)[make_conditional_op];

    auto const logical_def = equality >> *(logical_op > equality);
    auto const equality_def = relational >> *(equality_op > relational);
    auto const relational_def = additive >> *(relational_op > additive);
    auto const additive_def = multiplicative >> *(additive_op > multiplicative);
    auto const multiplicative_def = factor >> *(multiplicative_op > factor);
    auto const factor_def = primary >> *(power > factor);
    auto const unary_def =
        (unary_op > primary) | (ufunc > '(' > expression > ')');
    auto const binary_def = bfunc > '(' > expression > ',' > expression > ')';
    auto const primary_def = x3::double_ | ('(' > expression > ')') | binary |
                             unary | constant | variable;

    BOOST_SPIRIT_DEFINE(
        expression,
        logical,
        equality,
        relational,
        additive,
        multiplicative,
        factor,
        primary,
        unary,
        binary,
        conditional,
        variable);
} // namespace parser
} // namespace client

namespace client {
namespace ast {
    struct evaluator {
        typedef double result_type;

        evaluator(const double variable) : variable(variable) {}
        double variable;

        double
        operator()(operand const& ast) const {
            return boost::apply_visitor(*this, ast.get());
        }

        double
        operator()(nil) const {
            BOOST_ASSERT(0);
            return 0;
        }

        double
        operator()(expression const& ast) const {
            double state = boost::apply_visitor(*this, ast.lhs);
            BOOST_FOREACH (operation const& op, ast.rhs) {
                state = (*this)(op, state);
            }
            return state;
        }

        double
        operator()(operation const& ast, double lhs) const {
            double rhs = boost::apply_visitor(*this, ast.rhs);
            return ast.op(lhs, rhs);
        }

        double
        operator()(unary_op const& ast) const {
            double rhs = boost::apply_visitor(*this, ast.rhs);
            return ast.op(rhs);
        }

        double
        operator()(binary_op const& ast) const {
            double rhs = boost::apply_visitor(*this, ast.rhs);
            double lhs = boost::apply_visitor(*this, ast.lhs);
            return ast.op(lhs, rhs);
        }

        double
        operator()(conditional_op const& ast) const {
            bool lhs = boost::apply_visitor(*this, ast.lhs);
            if (lhs) {
                return boost::apply_visitor(*this, ast.rhs_true);
            }
            return boost::apply_visitor(*this, ast.rhs_false);
        }

        double
        operator()(double const& ast) const {
            return ast;
        }

        double
        operator()(std::string const& ast) const {
            return variable;
        }
    };
} // namespace ast
} // namespace client

int
main() {
    std::list<std::string> test_expressions{
        "0",
        "(n == 0) ? 0 : ((n == 1) ? 1 : 2)",
        "(n == 0) ? 0 : ((n == 1) ? 1 : (((n % 100 == 2 || n % 100 == 22 || n "
        "% 100 == 42 || n % 100 == 62 || n % 100 == 82) || n % 1000 == 0 && (n "
        "% 100000 >= 1000 && n % 100000 <= 20000 || n % 100000 == 40000 || n % "
        "100000 == 60000 || n % 100000 == 80000) || n != 0 && n % 1000000 == "
        "100000) ? 2 : ((n % 100 == 3 || n % 100 == 23 || n % 100 == 43 || n % "
        "100 == 63 || n % 100 == 83) ? 3 : ((n != 1 && (n % 100 == 1 || n % "
        "100 == 21 || n % 100 == 41 || n % 100 == 61 || n % 100 == 81)) ? 4 : "
        "5))))",
        "(n == 0) ? 0 : ((n == 1) ? 1 : ((n == 2) ? 2 : ((n % 100 >= 3 && n % "
        "100 <= 10) ? 3 : ((n % 100 >= 11 && n % 100 <= 99) ? 4 : 5))))",
        "(n == 0) ? 0 : ((n == 1) ? 1 : ((n == 2) ? 2 : ((n == 3) ? 3 : ((n == "
        "6) ? 4 : 5))))",
        "(n == 0 || n == 1) ? 0 : ((n >= 2 && n <= 10) ? 1 : 2)",
        "n != 1",
        "n > 1",
        "(n % 100 == 1) ? 0 : ((n % 100 == 2) ? 1 : ((n % 100 == 3 || n % 100 "
        "== 4) ? 2 : 3))",
        "(n % 10 == 0 || n % 100 >= 11 && n % 100 <= 19) ? 0 : ((n % 10 == 1 "
        "&& n % 100 != 11) ? 1 : 2)",
        "(n % 10 == 1) ? 0 : ((n % 10 == 2) ? 1 : ((n % 100 == 0 || n % 100 == "
        "20 || n % 100 == 40 || n % 100 == 60 || n % 100 == 80) ? 2 : 3))",
        "n % 10 != 1 || n % 100 == 11",
        "(n % 10 == 1 && n % 100 != 11) ? 0 : ((n % 10 >= 2 && n % 10 <= 4 && "
        "(n % 100 < 12 || n % 100 > 14)) ? 1 : 2)",
        "(n % 10 == 1 && (n % 100 < 11 || n % 100 > 19)) ? 0 : ((n % 10 >= 2 "
        "&& n % 10 <= 9 && (n % 100 < 11 || n % 100 > 19)) ? 1 : 2)",
        "(n % 10 == 1 && n % 100 != 11 && n % 100 != 71 && n % 100 != 91) ? 0 "
        ": ((n % 10 == 2 && n % 100 != 12 && n % 100 != 72 && n % 100 != 92) ? "
        "1 : ((((n % 10 == 3 || n % 10 == 4) || n % 10 == 9) && (n % 100 < 10 "
        "|| n % 100 > 19) && (n % 100 < 70 || n % 100 > 79) && (n % 100 < 90 "
        "|| n % 100 > 99)) ? 2 : ((n != 0 && n % 1000000 == 0) ? 3 : 4)))",
        "(n == 1) ? 0 : ((n == 0 || n % 100 >= 2 && n % 100 <= 10) ? 1 : ((n % "
        "100 >= 11 && n % 100 <= 19) ? 2 : 3))",
        "(n == 1) ? 0 : ((n == 0 || n % 100 >= 2 && n % 100 <= 19) ? 1 : 2)",
        "(n == 1) ? 0 : ((n % 10 >= 2 && n % 10 <= 4 && (n % 100 < 12 || n % "
        "100 > 14)) ? 1 : 2)",
        "(n == 1) ? 0 : ((n == 2) ? 1 : 2)",
        "(n == 1) ? 0 : ((n == 2) ? 1 : ((n > 10 && n % 10 == 0) ? 2 : 3))",
        "(n == 1) ? 0 : ((n == 2) ? 1 : ((n >= 3 && n <= 6) ? 2 : ((n >= 7 && "
        "n <= 10) ? 3 : 4)))",
        "(n == 1) ? 0 : ((n >= 2 && n <= 4) ? 1 : 2)",
        "(n == 1 || n == 11) ? 0 : ((n == 2 || n == 12) ? 1 : ((n >= 3 && n <= "
        "10 || n >= 13 && n <= 19) ? 2 : 3))",
        "n != 1 && n != 2 && n != 3 && (n % 10 == 4 || n % 10 == 6 || n % 10 "
        "== 9)",
        "n >= 2 && (n < 11 || n > 99)",
    };

    for (const std::string& str : test_expressions) {
        for (double idx = 0; idx <= 1000; ++idx) {
            std::cout << "-------------------------" << std::endl;
            std::cout << "Expression: " << std::quoted(str) << std::endl;
            std::cout << "n: " << idx << std::endl;
            client::ast::operand program;
            client::ast::evaluator eval(idx);

            std::string::const_iterator iter = str.begin();
            std::string::const_iterator end = str.end();
            bool r = phrase_parse(
                iter, end, client::parser::expression, x3::space, program);

            if (r && iter == end) {
                std::cout << "Parsing succeeded" << std::endl;
                std::cout << "Result: " << eval(program) << std::endl;
                std::cout << "-------------------------" << std::endl;
            } else {
                std::string rest(iter, end);
                std::cout << "Parsing failed" << std::endl;
                std::cout << "stopped at: \" " << rest << "\"" << std::endl;
                std::cout << "-------------------------" << std::endl;
                return EXIT_FAILURE;
            }
        }
    }

    std::cout << "///////////////////////////////////////////////////\n\n";
    std::cout << "Expression parser...\n\n";
    std::cout << "///////////////////////////////////////////////////\n\n";

    std::string str;
    while (true) {
        std::cout << "Type an expression...or [q or Q] to quit\n\n";
        std::getline(std::cin, str);

        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        std::cout << "Enter variable value: ";
        std::string variable_str;
        std::getline(std::cin, variable_str);
        double variable = std::stod(variable_str);

        client::ast::operand program;
        client::ast::evaluator eval(variable);

        std::string::const_iterator iter = str.begin();
        std::string::const_iterator end = str.end();
        bool r = phrase_parse(
            iter, end, client::parser::expression, x3::space, program);

        if (r && iter == end) {
            std::cout << "-------------------------" << std::endl;
            std::cout << "Parsing succeeded" << std::endl;
            std::cout << "Result: " << eval(program) << std::endl;
            std::cout << "-------------------------" << std::endl;
        } else {
            std::string rest(iter, end);
            std::cout << "-------------------------" << std::endl;
            std::cout << "Parsing failed" << std::endl;
            std::cout << "stopped at: \" " << rest << "\"" << std::endl;
            std::cout << "-------------------------" << std::endl;
        }
    }

    std::cout << "Bye... :-) \n\n";
    return EXIT_SUCCESS;
}