/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_array.hpp>
#include <boost/type_traits/remove_bounds.hpp>

namespace sprig {
	//
	// array_length
	//
	template<typename T>
	struct array_length
		: public boost::mpl::size_t<
			sizeof(T) / sizeof(typename boost::remove_bounds<T>::type)
		>
	{
		BOOST_MPL_ASSERT((boost::is_array<T>));
	};
}	// namespace sprig

#endif	// #ifndef SPRIG_TYPE_TRAITS_ARRAY_LENGTH_HPP


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

#include "QuantizeHelper.hpp"

#include <armnnUtils/Permute.hpp>

#include <QuantizeHelper.hpp>
#include <ResolveType.hpp>

#include <backendsCommon/test/CommonTestUtils.hpp>

#include <boost/test/unit_test.hpp>

#include <map>
#include <vector>

namespace
{

armnn::INetworkPtr CreateResizeNetwork(const armnn::ResizeDescriptor& descriptor,
                                       const armnn::TensorInfo& inputInfo,
                                       const armnn::TensorInfo& outputInfo)
{
    using namespace armnn;

    INetworkPtr network(INetwork::Create());
    IConnectableLayer* input  = network->AddInputLayer(0, "input");
    IConnectableLayer* resize = network->AddResizeLayer(descriptor, "resize");
    IConnectableLayer* output = network->AddOutputLayer(0, "output");

    Connect(input, resize, inputInfo, 0, 0);
    Connect(resize, output, outputInfo, 0, 0);

    return network;
}

template<armnn::DataType ArmnnType>
void ResizeEndToEnd(const std::vector<armnn::BackendId>& backends,
                    armnn::DataLayout dataLayout,
                    armnn::ResizeMethod resizeMethod)
{
    using namespace armnn;
    using T = ResolveType<ArmnnType>;

    constexpr unsigned int inputWidth  = 3u;
    constexpr unsigned int inputHeight = inputWidth;

    constexpr unsigned int outputWidth  = 5u;
    constexpr unsigned int outputHeight = outputWidth;

    TensorShape inputShape  = MakeTensorShape(1, 1, inputHeight, inputWidth, dataLayout);
    TensorShape outputShape = MakeTensorShape(1, 1, outputHeight, outputWidth, dataLayout);

    const float   qScale  = IsQuantizedType<T>() ? 0.25f : 1.0f;
    const int32_t qOffset = IsQuantizedType<T>() ? 50    : 0;

    TensorInfo inputInfo(inputShape, ArmnnType, qScale, qOffset);
    TensorInfo outputInfo(outputShape, ArmnnType, qScale, qOffset);

    std::vector<float> inputData =
    {
       1.f, 2.f, 3.f,
       4.f, 5.f, 6.f,
       7.f, 8.f, 9.f
    };

    std::vector<float> expectedOutputData;
    switch(resizeMethod)
    {
        case ResizeMethod::Bilinear:
        {
            expectedOutputData =
            {
                1.0f, 1.6f, 2.2f, 2.8f, 3.0f,
                2.8f, 3.4f, 4.0f, 4.6f, 4.8f,
                4.6f, 5.2f, 5.8f, 6.4f, 6.6f,
                6.4f, 7.0f, 7.6f, 8.2f, 8.4f,
                7.0f, 7.6f, 8.2f, 8.8f, 9.0f
            };
            break;
        }
        case ResizeMethod::NearestNeighbor:
        {
            expectedOutputData =
            {
                1.f, 1.f, 2.f, 2.f, 3.f,
                1.f, 1.f, 2.f, 2.f, 3.f,
                4.f, 4.f, 5.f, 5.f, 6.f,
                4.f, 4.f, 5.f, 5.f, 6.f,
                7.f, 7.f, 8.f, 8.f, 9.f
            };
            break;
        }
        default:
        {
            throw InvalidArgumentException("Unrecognized resize method");
        }
    }

    ResizeDescriptor descriptor;
    descriptor.m_TargetWidth  = outputWidth;
    descriptor.m_TargetHeight = outputHeight;
    descriptor.m_Method       = resizeMethod;
    descriptor.m_DataLayout   = dataLayout;

    // swizzle data if needed
    if (dataLayout == armnn::DataLayout::NHWC)
    {
        constexpr size_t dataTypeSize = sizeof(float);
        const armnn::PermutationVector nchwToNhwc = { 0, 3, 1, 2 };

        std::vector<float> tmp(inputData.size());
        armnnUtils::Permute(inputInfo.GetShape(), nchwToNhwc, inputData.data(), tmp.data(), dataTypeSize);
        inputData = tmp;
    }

    // quantize data
    std::vector<T> qInputData          = armnnUtils::QuantizedVector<T>(inputData, qScale, qOffset);
    std::vector<T> qExpectedOutputData = armnnUtils::QuantizedVector<T>(expectedOutputData, qScale, qOffset);

    INetworkPtr network = CreateResizeNetwork(descriptor, inputInfo, outputInfo);

    EndToEndLayerTestImpl<ArmnnType, ArmnnType>(std::move(network),
                                                { { 0, qInputData } },
                                                { { 0, qExpectedOutputData } },
                                                backends);
}

} // anonymous namespace

template<armnn::DataType ArmnnType>
void ResizeBilinearEndToEnd(const std::vector<armnn::BackendId>& backends,
                            armnn::DataLayout dataLayout)
{
    ResizeEndToEnd<ArmnnType>(backends, dataLayout, armnn::ResizeMethod::Bilinear);
}

template<armnn::DataType ArmnnType>
void ResizeNearestNeighborEndToEnd(const std::vector<armnn::BackendId>& backends,
                                   armnn::DataLayout dataLayout)
{
    ResizeEndToEnd<ArmnnType>(backends, dataLayout, armnn::ResizeMethod::NearestNeighbor);
}


#ifndef __PRINTS_HPP__
#define __PRINTS_HPP__

#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <iostream>

template <typename T, int m, int n>
void printMat(const Eigen::Matrix<T, m, n>& mat){
    for (int i = 0; i < m; i++){
        for (int j = 0; j < n; j++){
            std::cout << mat(i, j) << ", ";
        }
        std::cout << std::endl;
    }
}

void printMat(const Eigen::MatrixXd& mat){
    for (int i = 0; i < mat.rows(); i++){
        for (int j = 0; j < mat.cols(); j++){
            std::cout << mat(i, j) << ", ";
        }
        std::cout << std::endl;
    }
}

void printMat(const Eigen::RowVectorXd& mat){
    for (int j = 0; j < mat.cols(); j++){
        std::cout << mat(j) << ", ";
    }
    std::cout << std::endl;
}

template <typename Ty = int>
void printVector(const std::vector<Ty>& vec){
    for (size_t i = 0; i < vec.size(); i++){
        std::cout << vec[i] << ", ";
    }
    std::cout << std::endl;
}

#endif  //__PRINTS_HPP__


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file simulate-online-new.cpp
    \brief This file runs a simulation of the calibration problem in iterative
           mode.
  */

#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/three_point_methods.hpp>

#include <sm/BoostPropertyTree.hpp>

#include <aslam/backend/CompressedColumnMatrix.hpp>

#include <aslam/calibration/statistics/UniformDistribution.h>
#include <aslam/calibration/statistics/NormalDistribution.h>
#include <aslam/calibration/data-structures/VectorDesignVariable.h>
#include <aslam/calibration/geometry/Transformation.h>
#include <aslam/calibration/core/IncrementalEstimator.h>
#include <aslam/calibration/core/IncrementalOptimizationProblem.h>
#include <aslam/calibration/core/OptimizationProblem.h>
#include <aslam/calibration/base/Timestamp.h>

#include "aslam/calibration/2dlrf/utils.h"
#include "aslam/calibration/2dlrf/ErrorTermMotion.h"
#include "aslam/calibration/2dlrf/ErrorTermObservation.h"

using namespace aslam::calibration;
using namespace sm::kinematics;
using namespace sm;

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <conf_file>" << std::endl;
    return -1;
  }

  // load configuration file
  BoostPropertyTree propertyTree;
  propertyTree.loadXml(argv[1]);

  // steps to simulate
  const size_t steps = propertyTree.getInt("lrf/problem/steps");

  // timestep size
  const double T = propertyTree.getDouble("lrf/problem/timestep");

  // true state
  std::vector<Eigen::Vector3d> x_true;
  x_true.reserve(steps);

  // integrated odometry
  std::vector<Eigen::Vector3d> x_odom;
  x_odom.reserve(steps);

  // true control input
  std::vector<Eigen::Vector3d> u_true;
  const double sineWaveAmplitude = propertyTree.getDouble(
    "lrf/problem/sineWaveAmplitude");
  const double sineWaveFrequency = propertyTree.getDouble(
    "lrf/problem/sineWaveFrequency");
  genSineWavePath(u_true, steps, sineWaveAmplitude, sineWaveFrequency, T);

  // measured control input
  std::vector<Eigen::Vector3d> u_noise;
  u_noise.reserve(steps);

  // number of landmarks
  const size_t nl = propertyTree.getInt("lrf/problem/numLandmarks");

  // playground size
  const Eigen::Vector2d min(propertyTree.getDouble("lrf/problem/groundMinX"),
    propertyTree.getDouble("lrf/problem/groundMinY"));
  const Eigen::Vector2d max(propertyTree.getDouble("lrf/problem/groundMaxX"),
    propertyTree.getDouble("lrf/problem/groundMaxY"));

  // bearing measurements
  std::vector<std::vector<double> > b;
  b.reserve(steps);
  b.push_back(std::vector<double>(nl, 0));

  // range measurements
  std::vector<std::vector<double> > r;
  r.reserve(steps);
  r.push_back(std::vector<double>(nl, 0));

  // covariance matrix for motion model
  Eigen::Matrix3d Q = Eigen::Matrix3d::Zero();
  Q(0, 0) = propertyTree.getDouble("lrf/problem/motion/sigma2_x");
  Q(1, 1) = propertyTree.getDouble("lrf/problem/motion/sigma2_y");
  Q(2, 2) = propertyTree.getDouble("lrf/problem/motion/sigma2_t");

  // covariance matrix for observation model
  Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
  R(0, 0) = propertyTree.getDouble("lrf/problem/observation/sigma2_r");
  R(1, 1) = propertyTree.getDouble("lrf/problem/observation/sigma2_b");

  // landmark positions
  std::vector<Eigen::Vector2d> x_l;
  UniformDistribution<double, 2>(min, max).getSamples(x_l, nl);

  // true calibration parameters
  const Eigen::Vector3d Theta(propertyTree.getDouble("lrf/problem/thetaTrue/x"),
    propertyTree.getDouble("lrf/problem/thetaTrue/y"),
    propertyTree.getDouble("lrf/problem/thetaTrue/t"));

  // guessed calibration parameters
  const Eigen::Vector3d Theta_hat(
    propertyTree.getDouble("lrf/problem/thetaHat/x"),
    propertyTree.getDouble("lrf/problem/thetaHat/y"),
    propertyTree.getDouble("lrf/problem/thetaHat/t"));

  // initial state
  const Eigen::Vector3d x_0(propertyTree.getDouble("lrf/problem/x0/x"),
    propertyTree.getDouble("lrf/problem/x0/y"),
    propertyTree.getDouble("lrf/problem/x0/t"));
  x_true.push_back(x_0);
  x_odom.push_back(x_0);
  u_noise.push_back(Eigen::Vector3d::Zero());

  // simulate
  for (size_t i = 1; i < steps; ++i) {
    Eigen::Matrix3d B = Eigen::Matrix3d::Identity();
    B(0, 0) = cos(x_true[i - 1](2));
    B(0, 1) = -sin(x_true[i - 1](2));
    B(1, 0) = sin(x_true[i - 1](2));
    B(1, 1) = cos(x_true[i - 1](2));
    Eigen::Vector3d xk = x_true[i - 1] + T * B * u_true[i];
    xk(2) = angleMod(xk(2));
    x_true.push_back(xk);
    u_noise.push_back(u_true[i] +
      NormalDistribution<3>(Eigen::Vector3d::Zero(), Q).getSample());
    B(0, 0) = cos(x_odom[i - 1](2));
    B(0, 1) = -sin(x_odom[i - 1](2));
    B(1, 0) = sin(x_odom[i - 1](2));
    B(1, 1) = cos(x_odom[i - 1](2));
    xk = x_odom[i - 1] + T * B * u_noise[i];
    xk(2) = angleMod(xk(2));
    x_odom.push_back(xk);
    const double ct = cos(x_true[i](2));
    const double st = sin(x_true[i](2));
    std::vector<double> rk(nl, 0);
    std::vector<double> bk(nl, 0);
    for (size_t j = 0; j < nl; ++j) {
      const double aa = x_l[j](0) - x_true[i](0) - Theta(0) * ct +
        Theta(1) * st;
      const double bb = x_l[j](1) - x_true[i](1) - Theta(0) * st -
        Theta(1) * ct;
      const double range = sqrt(aa * aa + bb * bb) +
        NormalDistribution<1>(0, R(0, 0)).getSample();
      rk[j] = range;
      bk[j] = angleMod(atan2(bb, aa) - x_true[i](2) - Theta(2) +
        NormalDistribution<1>(0, R(1, 1)).getSample());
    }
    r.push_back(rk);
    b.push_back(bk);
  }

  // landmark guess
  std::vector<Eigen::Vector2d> x_l_hat;
  initLandmarks(x_l_hat, x_odom, Theta_hat, r, b);

  // create landmarks design variables
  std::vector<boost::shared_ptr<VectorDesignVariable<2> > > dv_x_l;
  dv_x_l.reserve(nl);
  for (size_t i = 0; i < nl; ++i) {
    dv_x_l.push_back(boost::make_shared<VectorDesignVariable<2> >(x_l_hat[i]));
    dv_x_l[i]->setActive(true);
  }

  // create calibration parameters design variable
  auto dv_Theta = boost::make_shared<VectorDesignVariable<3> >(Theta_hat);
  dv_Theta->setActive(true);

  std::cout << "calibration before: " << *dv_Theta << std::endl;

  // create incremental estimator
  IncrementalEstimator incrementalEstimator(PropertyTree(propertyTree,
    "lrf/estimator"));

  // batch size
  const size_t batchSize = propertyTree.getInt("lrf/estimator/batchSize");

  // run over the dataset
  for (size_t i = 0; i < steps; i += batchSize) {

    // create batch
    auto batch = boost::make_shared<IncrementalEstimator::Batch>();

    // create starting state variable at k-1 and activate it
    auto dv_xkm1 = boost::make_shared<VectorDesignVariable<3> >(x_odom[i]);
    dv_xkm1->setActive(true);
    batch->addDesignVariable(dv_xkm1, 0);

    // add the calibration variable to the batch
    batch->addDesignVariable(dv_Theta, 2);

    // add the landmarks in the story
    for (size_t k = 0; k < nl; ++k)
      batch->addDesignVariable(dv_x_l[k], 1);

    // create other state variables and error terms
    for (size_t j = i + 1; j < i + batchSize && j < steps; ++j) {
      // create state variable at k and activate it
      auto dv_xk = boost::make_shared<VectorDesignVariable<3> >(x_odom[j]);
      dv_xk->setActive(true);

      // add the variables to the batch
      batch->addDesignVariable(dv_xk, 0);

      // motion error term
      auto e_mot = boost::make_shared<ErrorTermMotion>(dv_xkm1.get(),
        dv_xk.get(), T, u_noise[j], Q);
      batch->addErrorTerm(e_mot);

      // observation error terms
      for (size_t k = 0; k < nl; ++k) {
        auto e_obs = boost::make_shared<ErrorTermObservation>(dv_xk.get(),
          dv_x_l[k].get(), dv_Theta.get(), r[j][k], b[j][k], R);
        batch->addErrorTerm(e_obs);
      }
      // switch state variable
      dv_xkm1 = dv_xk;
    }

    // add the measurement batch to the estimator
    std::cout << "calibration before batch: " << *dv_Theta << std::endl;
    auto ret = incrementalEstimator.addBatch(batch);
    std::cout << "calibration after batch: " << *dv_Theta << std::endl;
    auto problem = incrementalEstimator.getProblem();
    std::cout << "ordering: ";
    for (auto it = problem->getGroupsOrdering().cbegin(); it !=
        problem->getGroupsOrdering().cend(); ++it)
      std::cout << *it << " ";
    std::cout << std::endl;
    ret.batchAccepted ? std::cout << "ACCEPTED" : std::cout << "REJECTED";
    std::cout << std::endl;
    std::cout << "information gain: " << ret.informationGain << std::endl;
    std::cout << "rank of Psi: " << ret.rankPsi << std::endl;
    std::cout << "rank deficiency of Psi: " << ret.rankPsiDeficiency
      << std::endl;
    std::cout << "rank of Theta: " << ret.rankTheta << std::endl;
    std::cout << "rank deficiency of Theta: " << ret.rankThetaDeficiency
      << std::endl;
    std::cout << "unobservable basis: " << std::endl << ret.nobsBasis
      << std::endl;
    std::cout << "unobservable basis (scaled): " << std::endl
      << ret.nobsBasisScaled << std::endl;
    std::cout << "QR tolerance: " << ret.qrTolerance << std::endl;
    std::cout << "SVD tolerance: " << ret.svdTolerance << std::endl;
    std::cout << "time [s]: " << ret.elapsedTime << std::endl;
    std::cout << std::endl;
  }

  std::cout << "final calibration: " << *dv_Theta << std::endl;
  std::cout << "covariance: " << std::endl <<
    incrementalEstimator.getSigma2Theta() << std::endl;

  // for debugging purpose, write the Jacobian to file
  std::ofstream jacobianFile("J.txt");
  incrementalEstimator.getJacobianTranspose().writeMATLAB(jacobianFile);

  // for debugging purpose, output some infos
  std::cout << "information gain: " << incrementalEstimator.getInformationGain()
    << std::endl;
  std::cout << "rank of Psi: " << incrementalEstimator.getRankPsi()
    << std::endl;
  std::cout << "rank deficiency of Psi: "
    << incrementalEstimator.getRankPsiDeficiency() << std::endl;
  std::cout << "rank of Theta: " << incrementalEstimator.getRankTheta()
    << std::endl;
  std::cout << "rank deficiency of Theta: "
    << incrementalEstimator.getRankThetaDeficiency() << std::endl;
  std::cout << "QR tolerance: " << incrementalEstimator.getQRTolerance()
    << std::endl;
  std::cout << "SVD tolerance: " << incrementalEstimator.getSVDTolerance()
    << std::endl;
  std::cout << "unobservable basis: " << std::endl
    << incrementalEstimator.getNobsBasis() << std::endl;
    std::cout << "unobservable basis (scaled): " << std::endl
    << incrementalEstimator.getNobsBasis(true) << std::endl;
  std::cout << "observable basis: " << std::endl
    << incrementalEstimator.getObsBasis() << std::endl;
  std::cout << "observable covariance: " << std::endl
    << incrementalEstimator.getSigma2ThetaObs() << std::endl;
  std::cout << "memory usage [MB]: " << incrementalEstimator.getMemoryUsage() /
    1024.0 / 1024.0 << std::endl;
  std::cout << "peak memory usage [MB]: "
    << incrementalEstimator.getPeakMemoryUsage() / 1024.0 / 1024.0 << std::endl;
  std::cout << "marginalized group: " << incrementalEstimator.getMargGroupId()
    << std::endl;
  std::cout << "number of batches: " << incrementalEstimator.getNumBatches()
    << " out of " << round((double)steps / batchSize) << std::endl;

  // fetch the problem
  auto problem = incrementalEstimator.getProblem();

  // some other output
  std::cout << "number of design variables: " << problem->numDesignVariables()
    << std::endl;
  std::cout << "number of error terms: " << problem->numErrorTerms()
    << std::endl;
  std::cout << "Jacobian matrix is: "
    << incrementalEstimator.getJacobianTranspose().cols() << "x"
    << incrementalEstimator.getJacobianTranspose().rows() << std::endl;

  // output results to file
  std::ofstream x_true_log("x_true.txt");
  for (size_t i = 0; i < steps; ++i)
    x_true_log << x_true[i].transpose() << std::endl;
  std::ofstream x_odom_log("x_odom.txt");
  for (size_t i = 0; i < steps; ++i)
    x_odom_log << x_odom[i].transpose() << std::endl;
  std::ofstream l_log("l.txt");
  for (size_t i = 0; i < nl; ++i)
    l_log << x_l[i].transpose() << std::endl;

  // align landmarks
  Eigen::MatrixXd l = Eigen::MatrixXd::Zero(3, nl);
  Eigen::MatrixXd l_est = Eigen::MatrixXd::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i) {
    l(0, i) = x_l[i](0);
    l(1, i) = x_l[i](1);
    l_est(0, i) = dv_x_l[i]->getValue()(0);
    l_est(1, i) = dv_x_l[i]->getValue()(1);
  }
  Transformation<double, 3> trans(threePointSvd(l, l_est));
  Eigen::MatrixXd l_est_trans = Eigen::MatrixXd::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i)
    l_est_trans.col(i) = trans(l_est.col(i));
  std::ofstream l_est_trans_log("l_est_trans.txt");
  for (size_t i = 0; i < nl; ++i)
    l_est_trans_log << l_est_trans.col(i).head<2>().transpose() << std::endl;

  // align poses
  std::vector<Eigen::Vector3d> x_est_trans;
  x_est_trans.reserve(steps);
  std::ofstream x_est_trans_log("x_est_trans.txt");
  const IncrementalOptimizationProblem::DesignVariablesP&
    dvsx = problem->getDesignVariablesGroup(0);
  for (auto it = dvsx.cbegin(); it != dvsx.cend(); ++ it) {
    const VectorDesignVariable<3>* dv_xk =
      dynamic_cast<const VectorDesignVariable<3>*>(*it);
    Eigen::Vector3d pose((Eigen::Vector3d()
      << dv_xk->getValue().head<2>(), 0).finished());
    trans.transform(pose, pose);
    pose(2) = dv_xk->getValue()(2);
    x_est_trans.push_back(pose);
    x_est_trans_log << pose.transpose() << std::endl;
  }

  return 0;
}


// Copyright (c) 2014-2020 The DigiByte Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <net.h>
#include <signet.h>
#include <uint256.h>
#include <validation.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(validation_tests, TestingSetup)

#define END_OF_SUPPLY_CURVE 110579600

// Set OUTPUT_SUPPLY_SAMPLES_ENABLED to 1 to output
// sampled Supply-curve data points (SAMPLE_INTERVAL)
#ifndef OUTPUT_SUPPLY_SAMPLES_ENABLED
    #define OUTPUT_SUPPLY_SAMPLES_ENABLED 0
#endif

#ifndef SAMPLE_INTERVAL
    #define SAMPLE_INTERVAL 100
#endif

#ifndef WHOLE_COIN
    #define WHOLE_COIN false
#endif

#define CALC_COIN(amount) (WHOLE_COIN ? (amount / COIN) : (amount))

#if OUTPUT_SUPPLY_SAMPLES_ENABLED
    #define DEBUG(x,y) if (x % SAMPLE_INTERVAL == 0) { \
        std::cerr << x << ';' << CALC_COIN(y) << std::endl; \
    }
#else
    #define DEBUG(x,y) (void) (x)
#endif

#ifndef BLOCK_TIME_SECONDS
    #define BLOCK_TIME_SECONDS 15
#endif

#ifndef SECONDS_PER_MONTH
    #define SECONDS_PER_MONTH (60 * 60 * 24 * 365 / 12)
#endif

#ifndef ENABLE_TESTNET_SUBSIDY_TESTS
    #define ENABLE_TESTNET_SUBSIDY_TESTS 0
#endif

static void TestBlockSubsidy(const Consensus::Params& consensusParams, int nMaxBlocks, CAmount* nSumOut)
{
    CAmount nSum = 0;
    CAmount nInitialSubsidy = 72000 * COIN;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);

    /* Before first hard fork */

    // 72000 reward for the first 1440 blocks
    for (int nBlocks = 0; nBlocks < 1440 && nBlocks < consensusParams.nDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK_EQUAL(nSubsidy, nInitialSubsidy);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // 16000 rewards until block height 5760
    for (int nBlocks = 1440; nBlocks < 5760 && nBlocks < consensusParams.nDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK_EQUAL(nSubsidy, 16000 * COIN);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // 8000 mining rewards until block height 67,200
    for (int nBlocks = 5760; nBlocks < consensusParams.nDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK_EQUAL(nSubsidy, 8000 * COIN);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // Dynamic mining rewards from block height 67,200 to block height 400,000
    for (int nBlocks = consensusParams.nDiffChangeTarget; nBlocks < consensusParams.alwaysUpdateDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);

        CAmount nExpectedSubsidy = 8000 * COIN;
        int nHeightWithinFork = (nHeight - consensusParams.nDiffChangeTarget);

        for (int i = 0; i < (nHeightWithinFork / consensusParams.patchBlockRewardDuration) + 1; ++i) {
            nExpectedSubsidy -= nExpectedSubsidy / 200; // dec by 0.5%
        }

        BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // Updated dynamic mining rewards from block height 400,000 to block height 1,430,000
    for (int nBlocks = consensusParams.alwaysUpdateDiffChangeTarget; nBlocks < consensusParams.workComputationChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);

        CAmount nExpectedSubsidy = 2459 * COIN;
        int nHeightWithinFork = (nHeight - consensusParams.alwaysUpdateDiffChangeTarget);

        for (int i = 0; i < (nHeightWithinFork / consensusParams.patchBlockRewardDuration2) + 1; ++i) {
            nExpectedSubsidy -= nExpectedSubsidy / 100; // dec by 1% per month
        }

        BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    {
        // Updated dynamic mining rewards from block height 1,430,000 to max block height.
        // Intended blockheight: 41.6 million
        // Actual blockheight: 110.5 million
        CAmount nExpectedSubsidyStart = 2157 * COIN / 2;
        CAmount nExpectedSubsidy = nExpectedSubsidyStart;
        int nMonthsConsidered = 0;

        for (int nBlocks = consensusParams.workComputationChangeTarget; nBlocks < nMaxBlocks; ++nBlocks) {
            int nHeight = nBlocks;
            CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);

            int nHeightWithinFork = (nHeight - consensusParams.workComputationChangeTarget);
            int nMonths = nHeightWithinFork * BLOCK_TIME_SECONDS / SECONDS_PER_MONTH;

            if (nMonthsConsidered < nMonths) {
                // Calculate new subsidy for number of months `nMonths`.
                // This is a major optimization in order to reduce the
                // number of inner loops

                // Recalculate subsidy
                for (int i = nMonthsConsidered; i < nMonths; ++i) {
                    // Decay factor: 98884/100000
                    nExpectedSubsidy *= 98884;
                    nExpectedSubsidy /= 100000;
                    ++nMonthsConsidered;
                }
            }

            if (nExpectedSubsidy < COIN) { // ToDo: Alter consensus
                nExpectedSubsidy = COIN;
            }

            BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

            nSum += nSubsidy;
            DEBUG(nBlocks, nSubsidy);
        }
    }

    CAmount nSubsidy = GetBlockSubsidy(nMaxBlocks, consensusParams);
    CAmount nExpectedSubsidy = 1 * COIN;

    BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

    if (nSumOut != NULL) {
        *nSumOut = nSum;
    }
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    CAmount sum;
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    const auto testChainParams = CreateChainParams(*m_node.args, CBaseChainParams::TESTNET);
    TestBlockSubsidy(chainParams->GetConsensus(), END_OF_SUPPLY_CURVE, &sum); // Mainnet

    CAmount nExpectedTotalSupply = 2239167398214795680ULL;
    BOOST_CHECK_EQUAL(sum, nExpectedTotalSupply);

#if OUTPUT_SUPPLY_SAMPLES_ENABLED
    // Output the accumulated supply until END_OF_SUPPLY_CURVE
    std::cout << "(mainnet): MAXIMUM SUPPLY: " << sum << " dgbSATS (" << (sum / COIN) << " DGB)";
#elif ENABLE_TESTNET_SUBSIDY_TESTS != 0
    // Only perform test on TESTNET too if requested so.
    TestBlockSubsidy(testChainParams->GetConsensus(), END_OF_SUPPLY_CURVE, NULL); // Testnet
#endif
}

BOOST_AUTO_TEST_CASE(signet_parse_tests)
{
    ArgsManager signet_argsman;
    signet_argsman.ForceSetArg("-signetchallenge", "51"); // set challenge to OP_TRUE
    const auto signet_params = CreateChainParams(signet_argsman, CBaseChainParams::SIGNET);
    CBlock block;
    BOOST_CHECK(signet_params->GetConsensus().signet_challenge == std::vector<uint8_t>{OP_TRUE});
    CScript challenge{OP_TRUE};

    // empty block is invalid
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // no witness commitment
    CMutableTransaction cb;
    cb.vout.emplace_back(0, CScript{});
    block.vtx.push_back(MakeTransactionRef(cb));
    block.vtx.push_back(MakeTransactionRef(cb)); // Add dummy tx to exercise merkle root code
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // no header is treated valid
    std::vector<uint8_t> witness_commitment_section_141{0xaa, 0x21, 0xa9, 0xed};
    for (int i = 0; i < 32; ++i) {
        witness_commitment_section_141.push_back(0xff);
    }
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(SignetTxs::Create(block, challenge));
    BOOST_CHECK(CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // no data after header, valid
    std::vector<uint8_t> witness_commitment_section_325{0xec, 0xc7, 0xda, 0xa2};
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(SignetTxs::Create(block, challenge));
    BOOST_CHECK(CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // Premature end of data, invalid
    witness_commitment_section_325.push_back(0x01);
    witness_commitment_section_325.push_back(0x51);
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // has data, valid
    witness_commitment_section_325.push_back(0x00);
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(SignetTxs::Create(block, challenge));
    BOOST_CHECK(CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // Extraneous data, invalid
    witness_commitment_section_325.push_back(0x00);
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));
}

//! Test retrieval of valid assumeutxo values.
BOOST_AUTO_TEST_CASE(test_assumeutxo)
{
    const auto params = CreateChainParams(*m_node.args, CBaseChainParams::REGTEST);

    // These heights don't have assumeutxo configurations associated, per the contents
    // of chainparams.cpp.
    std::vector<int> bad_heights{0, 100, 111, 115, 209, 211};

    for (auto empty : bad_heights) {
        const auto out = ExpectedAssumeutxo(empty, *params);
        BOOST_CHECK(!out);
    }

    const auto out110 = *ExpectedAssumeutxo(110, *params);
    BOOST_CHECK_EQUAL(out110.hash_serialized.ToString(), "574fd055b5dc5a59e9bc78fb43ada77bc7c7b173fd554f6f0704060506f9ac59");
    BOOST_CHECK_EQUAL(out110.nChainTx, 110U);

    const auto out210 = *ExpectedAssumeutxo(200, *params);
    BOOST_CHECK_EQUAL(out210.hash_serialized.ToString(), "51c8d11d8b5c1de51543c579736e786aa2736206d1e11e627568029ce092cf62");
    BOOST_CHECK_EQUAL(out210.nChainTx, 200U);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <typeinfo>

#include <boost/simd/algorithm.hpp>
#include <boost/simd/pack.hpp>

struct adder
{
  template <typename T>
  T operator()(T const& a, T const& b)
  {
    return a + b;
  }
};

//! [runtime-dec]
void compute(float* a, float* b, float* res, int size, BOOST_SIMD_DEFAULT_SITE const& arch)
{
  //! [runtime-dec]
  std::cout << " Architecture: " << typeid(arch).name() << std::endl;
  namespace bs = boost::simd;
  bs::transform(a, a + size, b, res, adder{});
}


// Copyright 2019-2021 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef NDT__NDT_VOXEL_HPP_
#define NDT__NDT_VOXEL_HPP_

#include <common/types.hpp>
#include <experimental/optional>
#include <ndt/ndt_common.hpp>
#include <voxel_grid/voxels.hpp>

#include <Eigen/Core>

using autoware::common::types::bool8_t;

namespace autoware
{
namespace localization
{
namespace ndt
{

enum class Invertibility
{
  INVERTIBLE,
  NOT_INVERTIBLE,
  UNKNOWN
};

/// Dynamic Voxel implementation for the NDT map. A dynamic voxel updates its state with each added
/// observation and hence it is to be only used when a raw point cloud is being
/// transformed into the ndt map representation.
class NDT_PUBLIC DynamicNDTVoxel
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Point = Eigen::Vector3d;
  using Cov = Eigen::Matrix3d;
  DynamicNDTVoxel();

  // TODO(yunus.caliskan): make this configurable.
  // Number of points a voxel should have to count as occupied. Set to the dimension of a 3D point.
  static constexpr uint32_t NUM_POINT_THRESHOLD = 3U;

  /// Add a point to the cell, update the centroid and covariance. This function
  /// Uses Welford's online algorithm:
  /// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_online_algorithm
  /// \param pt Point to add to the voxel.
  void add_observation(const Point & pt);

  /// Try to stabilize the covariance
  /// \return True if stabilization succeeds and covariance is invertible
  bool8_t try_stabilize();

  /// Check if the cell contains enough points to be used in ndt matching
  /// \return True if cell has more points than NUM_POINT_THRESHOLD
  bool8_t usable() const noexcept;

  /// Returns the covariance of the points in the voxel. If not all points are used to update the
  /// covariance or the cell does not have enough points for covariance calculation,
  /// throws an error.
  /// \return covariance of the cell
  const Cov & covariance() const;

  /// Returns the inverse covariance calculated from the covariance. If the covariance is not
  /// invertible, throws an error
  /// \return inverse covariance of the cell
  std::experimental::optional<Cov> inverse_covariance() const;

  /// Returns the mean of the points in the cell. Throw if the cell does not have enough points.
  /// \return centroid of the cell
  const Point & centroid() const;

  /// Get number of points residing in the voxel.
  /// \return Number of points.
  uint64_t count() const noexcept;

private:
  Point m_centroid;
  Cov m_M2;  // Used in covariance computation.
  Cov m_covariance;
  Invertibility m_invertible{Invertibility::UNKNOWN};
  uint64_t m_num_points{0U};
};

/// Static Voxel implementation for the NDT map. A static voxel is used to represent a pre-computed
/// ndt cell, hence it doesn't contain any logic for updating its states.
class NDT_PUBLIC StaticNDTVoxel
{
public:
  using Point = Eigen::Vector3d;
  using Cov = Eigen::Matrix3d;
  /// Initialize an empty voxel
  StaticNDTVoxel();

  /// Initialize a voxel given the centroid and the covariance.
  /// \param centroid Centroid of the voxel.
  /// \param inv_covariance Covariance of the voxel.
  StaticNDTVoxel(const Point & centroid, const Cov & inv_covariance);

  /// Calculates and returns the covariance of the points in the voxel. Throw if voxel is empty.
  /// \return covariance of the cell
  Cov covariance() const;
  /// Returns the mean of the points in the cell. Throw if voxel is empty.
  /// \return centroid of the cell
  const Point & centroid() const;

  /// Returns the inverse covariance of the points in the voxel. Throw if voxel is empty.
  /// \return inverse covariance of the cell
  const Cov & inverse_covariance() const;

  /// Check if the cell is occupied and can be used in ndt matching
  /// \return True if cell is occupied
  bool8_t usable() const noexcept;

private:
  Point m_centroid;
  Cov m_inv_covariance;
  bool8_t m_occupied{false};
};
}  // namespace ndt
}  // namespace localization
}  // namespace autoware

#endif  // NDT__NDT_VOXEL_HPP_


// pseudo random junk generator, with a pre-diefined seed.
// it is not that fast, but offers repeatable, low-quality psudo-randomness.
#include <random>
#include <iostream>
#include <boost/lexical_cast.hpp>

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

int main(int argc, char** argv)
{
  if(argc != 1+2)
  {
    cerr << argv[0] << " <seed> <size_in_bytes>" << endl;
    return 2;
  }
  const auto seed = boost::lexical_cast<uint32_t>(argv[1]);
  const auto requestedBytes = boost::lexical_cast<uint64_t>(argv[2]);

  std::mt19937_64 gen{seed};
  std::uniform_int_distribution<uint8_t> dist(0, 255);

  constexpr auto blockSize = 1024u*1024u;
  char buf[blockSize];
  uint64_t written = 0;

  while(written < requestedBytes)
  {
    for(auto& b: buf)
      b = dist(gen);
    const auto bytesLeft = requestedBytes - written;
    const auto writeSize = std::min<uint64_t>(blockSize, bytesLeft);
    cout.write(buf, writeSize);
    cout.flush();
    written += writeSize;
  }
}


//
// Created by Beck on 7/3/18.
// Mahony filter for attitude estimation
// http://www.olliw.eu/2013/imu-data-fusing/#mahonycode
//

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

ros::Publisher pose_pub;
string imu_topic, publisher_topic;

Quaterniond q = Quaterniond::Identity();  // state
double sampleFreq = 400.0;  // sample frequency in Hz
double Kp = 0.5;   // proportional gain
double Ki = 0.1;   // integral gain
Vector3d integral = {0.0, 0.0, 0.0};

void pub_fused_pose(const sensor_msgs::Imu &msg)
{
    sensor_msgs::Imu imu_with_pose;
    imu_with_pose.header = msg.header;
    imu_with_pose.orientation.w = q.w();
    imu_with_pose.orientation.x = q.x();
    imu_with_pose.orientation.y = q.y();
    imu_with_pose.orientation.z = q.z();
    imu_with_pose.angular_velocity = msg.angular_velocity;
    imu_with_pose.angular_velocity_covariance = msg.angular_velocity_covariance;
    imu_with_pose.linear_acceleration = msg.linear_acceleration;
    imu_with_pose.linear_acceleration_covariance = msg.linear_acceleration_covariance;

    pose_pub.publish(imu_with_pose);
}

void imu_callback(const sensor_msgs::Imu::ConstPtr &imu)
{
    Vector3d halfd, halfe;

    Vector3d acc(imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z);
    acc.normalize();
    Vector3d omg(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z);
    // omg.normalize();
    double dt = 1.0 / sampleFreq;

    if (!((acc[0] == 0.0) && (acc[1] == 0.0) && (acc[2] == 0.0))) {
        // Estimate the gravity vector d from quaternion q
        // d = Im(q^-1 e_z q)
        halfd[0] = q.x() * q.z() - q.w() * q.y();
        halfd[1] = q.w() * q.x() + q.y() * q.z();
        halfd[2] = q.w() * q.w() - 0.5f + q.z() * q.z();

        // calculate error vector e = a x d
        halfe = acc.cross(halfd);

        if (Ki > 0.0) {
            // In = In-1 + Ki * dt * e
            integral += 2.0 * Ki * dt * halfe;
            omg += integral;
        }
        else {
            integral.setZero();
        }

        // Apply proportional feedback w' = w + Kp * e + In
        omg += 2.0 * Kp * halfe;
    }

    // Integrate rate of change using dq = 0.5 * q x w'
    omg *= 0.5 * dt;

    q.w() += (-q.x() * omg[0] - q.y() * omg[1] - q.z() * omg[2]);
    q.x() += (q.w() * omg[0] + q.y() * omg[2] - q.z() * omg[1]);
    q.y() += (q.w() * omg[1] - q.x() * omg[2] + q.z() * omg[0]);
    q.z() += (q.w() * omg[2] + q.x() * omg[1] - q.y() * omg[0]);

    q.normalize();
    pub_fused_pose(*imu);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "attitude_estimator_imu");
    ros::NodeHandle n("~");

    n.param("imu_raw", imu_topic, string("/dji_sdk/imu")); // 400Hz
    n.param("publisher_topic", publisher_topic, string("/attitude_estimator/imu"));

    ros::Subscriber s3 = n.subscribe(imu_topic, 400, imu_callback);
    pose_pub = n.advertise<sensor_msgs::Imu>(publisher_topic, 100);

    ros::Rate r(400);
    ros::spin();
}


#include <cmath>
#include <array>
#include <vector>
#include <fstream>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <memory>
#include <boost/program_options.hpp>

#include "TFile.h"
#include "TH2F.h"

namespace cluster_shape_filter {

  static constexpr size_t n_dim = 2;

  using Position = std::array<double, n_dim>;

  struct Limits {
    double down, up;
    Limits() : down(-std::numeric_limits<double>::infinity()), up(std::numeric_limits<double>::infinity()) {}
    Limits(double _down, double _up) : down(_down), up(_up) {}
    double size() const { return std::abs(down - up); }
  };

  using BoxLimits = std::array<Limits, n_dim>;

  struct Point {
    Position x;
    int weight;
    int cluster_index;
  };

  struct Center {
    Position x;
    int n_clusters;
    BoxLimits limits;

    double distance2(const Point& p) const {
      double d = 0;
      for (size_t n = 0; n < x.size(); ++n)
        d += std::pow(x[n] - p.x[n], 2);
      return d;
    }

    double area() const {
      double s = 1;
      for (size_t n = 0; n < limits.size(); ++n)
        s *= limits[n].size();
      return s;
    }
  };

  using AsymmetricCut = std::array<Center, 2>;

  std::ostream& operator<<(std::ostream& os, const AsymmetricCut& c) {
    for (size_t b = 0; b < c.size(); ++b) {
      for (size_t d = 0; d < c[b].limits.size(); ++d) {
        os << " " << c[b].limits[d].down << " " << c[b].limits[d].up;
      }
    }

    for (size_t b = 0; b < c.size(); ++b) {
      const double v = c[b].area() > 0 ? c[b].n_clusters / c[b].area() : 0.;
      os << " " << v << " " << c[b].n_clusters;
    }
    return os;
  }

  template <typename Object>
  Object* ReadObject(TFile& file, const std::string& name) {
    auto object = dynamic_cast<Object*>(file.Get(name.c_str()));
    if (!object) {
      std::ostringstream ss;
      ss << "Object '" << name << "' with type '" << typeid(Object).name() << "' not found in '" << file.GetName()
         << "'.";
      throw std::runtime_error(ss.str());
    }
    return object;
  }

  struct Arguments {
    std::string input, output, map_output;
    double loss{0.005};
    size_t minEntries{100};
    int exMax{10}, eyMax{15};
    bool verbose{false};
  };

  class ClusterAnalyzer {
  public:
    ClusterAnalyzer(const Arguments& _args) : args(_args) {}

    void analyze() {
      static const std::vector<std::string> subName = {"barrel", "endcap"};

      TFile resFile(args.input.c_str(), "READ");
      if (resFile.IsZombie())
        throw std::runtime_error("Input file not opened.");

      std::ofstream outFile;
      outFile.exceptions(std::ofstream::failbit | std::ofstream::badbit);
      outFile.open(args.output);
      std::shared_ptr<std::ofstream> mapFile;
      if (!args.map_output.empty()) {
        mapFile = std::make_shared<std::ofstream>();
        mapFile->exceptions(std::ofstream::failbit | std::ofstream::badbit);
        mapFile->open(args.map_output);
      }

      for (size_t is = 0; is < subName.size(); ++is) {
        for (int ix = 0; ix <= args.exMax; ++ix) {
          std::cout << " " << subName.at(is) << " dx= " << ix << " ";
          for (int iy = 0; iy <= args.eyMax; ++iy) {
            std::ostringstream histName;
            //                    histName << "hrpc_" << is << "_" << ix << "_" << iy;
            histName << "hspc_" << is << "_" << ix << "_" << iy;
            auto histo = ReadObject<TH2F>(resFile, histName.str());

            std::cout << ".";

            // Initial guess
            AsymmetricCut c;
            c[0].n_clusters = 0;
            c[0].x[0] = ix + 1;
            c[0].x[1] = iy + 1;

            c[1].n_clusters = 0;
            c[1].x[0] = -ix - 1;
            c[1].x[1] = -iy - 1;

            std::ostringstream flag;
            flag << subName.at(is) << "_" << ix << "_" << iy;
            process(*histo, c, flag.str());

            if (histo->Integral() >= args.minEntries) {
              // Fix barrel_0_0
              if (is == 0 && ix == 0 && iy == 0) {
                c[0].limits[1].down = 0.;
                c[1].limits[1].up = 0.;
              }

              outFile << is << " " << ix << " " << iy << c << std::endl;
            }

            if (mapFile)
              *mapFile << ix << " " << iy << " " << histo->Integral() << std::endl;
          }
          outFile << std::endl;
          std::cout << std::endl;
          if (mapFile)
            *mapFile << ix << " " << args.eyMax + 1 << " " << 0 << "\n" << std::endl;
        }

        if (mapFile) {
          for (int iy = 0; iy <= args.eyMax + 1; ++iy)
            *mapFile << args.exMax + 1 << " " << iy << " " << 0 << std::endl;
          *mapFile << std::endl << std::endl;
        }

        outFile << std::endl;
      }
    }

  private:
    using HistArray = std::array<std::array<TH1D*, 2>, 2>;

    HistArray CreateHistArray(const std::vector<Point>& points, const TH1D& line) const {
      HistArray x;
      for (size_t n = 0; n < x.size(); ++n) {
        for (size_t d = 0; d < x[n].size(); ++d) {
          std::ostringstream name;
          name << n << "_" << d;
          x[n][d] = dynamic_cast<TH1D*>(line.Clone(name.str().c_str()));
        }
      }

      for (const auto& point : points) {
        for (size_t d = 0; d < x[point.cluster_index].size(); ++d) {
          x[point.cluster_index][d]->Fill(point.x[d], point.weight);
        }
      }
      return x;
    }

    static double getFraction(const TH1D& line, double fraction) {
      double n = line.Integral();
      double s = 0;

      for (int i = 0; i <= line.GetNbinsX() + 1; ++i) {
        s += line.GetBinContent(i);
        if (s > n * fraction) {
          return line.GetXaxis()->GetBinUpEdge(i) -
                 (s - n * fraction) / line.GetBinContent(i) * line.GetXaxis()->GetBinWidth(i);
        }
      }

      throw std::runtime_error("Unable to determine a point for the given fraction.");
    }

    void kMeans(std::vector<Point>& points, AsymmetricCut& c, const TH1D& line) const {
      int changes;
      do {
        changes = 0;

        // Sort point into clusters
        for (auto& point : points) {
          const int n = c[0].distance2(point) < c[1].distance2(point) ? 0 : 1;
          if (n != point.cluster_index) {
            point.cluster_index = n;
            ++changes;
          }
        }

        if (changes != 0) {
          // Re-compute centers
          auto x = CreateHistArray(points, line);

          for (size_t n = 0; n < c.size(); ++n) {
            for (size_t d = 0; d < c[n].x.size(); ++d) {
              c[n].n_clusters = std::ceil(x[n][d]->Integral());
              if (x[n][d]->Integral() > 0)
                c[n].x[d] = getFraction(*x[n][d], 0.5);
            }
          }
        }
      } while (changes != 0);
    }

    void findLimits(const std::vector<Point>& points, AsymmetricCut& c, const TH1D& line) const {
      auto x = CreateHistArray(points, line);

      for (size_t b = 0; b < x.size(); ++b) {
        for (size_t d = 0; d < x[b].size(); ++d) {
          if (x[b][d]->Integral() <= 0)
            continue;
          Limits best_limits;
          for (double f = (args.loss / 2) / 100; f < args.loss / 2 - (args.loss / 2) / 200;
               f += (args.loss / 2) / 100) {
            const Limits limits(getFraction(*x[b][d], f), getFraction(*x[b][d], 1 - (args.loss / 2 - f)));
            if (limits.size() < best_limits.size())
              best_limits = limits;
          }
          c[b].limits[d] = best_limits;
        }
      }
    }

    static void printOut(const TH2F& histo, const AsymmetricCut& c, const std::string& flag) {
      printToFile(histo, "points_" + flag + ".dat");

      std::ofstream limitsFile("limits_" + flag + ".par");
      for (size_t b = 0; b < c.size(); ++b) {
        for (size_t d = 0; d < c[b].limits.size(); ++d) {
          limitsFile << " l" << b << d << "=" << c[b].limits[d].down << std::endl;
          limitsFile << " h" << b << d << "=" << c[b].limits[d].up << std::endl;
        }
      }

      std::ofstream centersFile("centers_" + flag + ".dat");
      for (size_t b = 0; b < c.size(); ++b)
        centersFile << " " << c[b].x[0] << " " << c[b].x[1] << std::endl;
    }

    void process(const TH2F& histo, AsymmetricCut& c, const std::string& flag) {
      std::vector<Point> points;
      points.reserve(histo.GetNbinsX() * histo.GetNbinsY());

      for (int i = 1; i <= histo.GetNbinsX(); ++i) {
        for (int j = 1; j <= histo.GetNbinsY(); ++j) {
          if (histo.GetBinContent(i, j) <= 0)
            continue;
          Point p;
          p.x[0] = histo.GetXaxis()->GetBinCenter(i);
          p.x[1] = histo.GetYaxis()->GetBinCenter(j);
          p.weight = std::ceil(histo.GetBinContent(i, j));
          points.push_back(p);
        }
      }

      auto line = histo.ProjectionY("_py", 0, 0);
      line->Reset();

      kMeans(points, c, *line);
      findLimits(points, c, *line);
      if (args.verbose)
        printOut(histo, c, flag);
    }

    static void printToFile(const TH2F& h2, const std::string& fileName) {
      std::ofstream file;
      file.exceptions(std::ofstream::failbit | std::ofstream::badbit);
      file.open(fileName);

      for (int i = 1; i <= h2.GetNbinsX(); i++) {
        for (int j = 1; j <= h2.GetNbinsY(); j++)
          file << " " << h2.GetXaxis()->GetBinLowEdge(i) << " " << h2.GetYaxis()->GetBinLowEdge(j) << " "
               << h2.GetBinContent(i, j) << "\n";

        file << " " << h2.GetXaxis()->GetBinLowEdge(i) << " " << h2.GetYaxis()->GetXmax() << " 0\n\n";
      }

      for (int j = 1; j <= h2.GetNbinsY(); j++) {
        file << " " << h2.GetXaxis()->GetXmax() << " " << h2.GetYaxis()->GetBinLowEdge(j) << " 0\n";
      }

      file << " " << h2.GetXaxis()->GetXmax() << " " << h2.GetYaxis()->GetXmax() << " 0" << std::endl;
    }

  private:
    const Arguments args;
  };

}  // namespace cluster_shape_filter

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

  Arguments args;
  po::options_description desc("Cluster shape filter analyzer");
  std::ostringstream ss_loss;
  ss_loss << std::setprecision(4) << args.loss;
  // clang-format off
  desc.add_options()
      ("help", "print help message")
      ("input", po::value<std::string>(&args.input)->required(), "input file with extracted cluster shapes")
      ("output", po::value<std::string>(&args.output)->required(), "output calibration file")
      ("map-output", po::value<std::string>(&args.map_output)->default_value(""), "output calibration file")
      ("loss",
       po::value<double>(&args.loss)->default_value(args.loss, ss_loss.str()),
       "(1 - efficiency) threshold for the WP definition")
      ("min-entries", po::value<size_t>(&args.minEntries)->default_value(args.minEntries), "")
      ("ex-max", po::value<int>(&args.exMax)->default_value(args.exMax), "")
      ("ey-max", po::value<int>(&args.eyMax)->default_value(args.eyMax), "")
      ("verbose", po::bool_switch(&args.verbose));
  // clang-format on

  try {
    po::variables_map variables;
    po::store(po::command_line_parser(argc, argv).options(desc).run(), variables);
    if (variables.count("help")) {
      std::cout << desc << std::endl;
      return 0;
    }
    po::notify(variables);

    cluster_shape_filter::ClusterAnalyzer analyzer(args);
    analyzer.analyze();

  } catch (po::error& e) {
    std::cerr << "ERROR: " << e.what() << ".\n\n" << desc << std::endl;
  } catch (std::exception& e) {
    std::cerr << "\nERROR: " << e.what() << std::endl;
  }

  return 0;
}


#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <quadrotor_msgs/TrajectoryData.h>
#include <math.h>
#include "goto_solver.h"

#define num_seg 3
TrajectoryInfo trajInfo[num_seg];
TrajectoryInfo trajInfo2[num_seg];
TrajectoryInfo trajInfo3[num_seg];
TrajectoryInfo trajInfo4[num_seg];
int count;
double tscale = 1;

double cur_t, t_end = 0;

double stops[4][3] = { {0.0, 0.0, 0.0}, {0.0, 0.6, 0.0}, {0.2, 0.6, 0}, {0.2, 0.2, 0}};

void init_traj(){
	for(int i=0; i<num_seg; i++) {
		trajInfo[i] = cal_goto(stops[i][0], stops[i][1], stops[i][2], stops[i+1][0], stops[i+1][1], stops[i+1][2]);
	}
    count = 0;
}

void initialize_var()
{
    set_t_goto(6);
}

int main(int argc, char **argv)
{
    ros::init(argc,argv,"gen_trajectory");
    initialize_var();
    init_traj();
    ros::NodeHandle nh("~");
    ros::Publisher pub = nh.advertise<quadrotor_msgs::TrajectoryData>("trajectoryinfo", 10, true);
    ros::Publisher pub2 = nh.advertise<quadrotor_msgs::TrajectoryData>("trajectoryinfo2", 10, true);
    ros::Publisher pub3 = nh.advertise<quadrotor_msgs::TrajectoryData>("trajectoryinfo3", 10, true);
    ros::Publisher pub4 = nh.advertise<quadrotor_msgs::TrajectoryData>("trajectoryinfo4", 10, true);
    ros::Rate loop_rate(200);
    quadrotor_msgs::TrajectoryData seg;
    quadrotor_msgs::TrajectoryData seg2;
    quadrotor_msgs::TrajectoryData seg3;
    quadrotor_msgs::TrajectoryData seg4;
    double t_start = 0;
    double t_end = 0;
    TrajectoryInfo cur_traj,cur_traj2,cur_traj3,cur_traj4;
    ROS_INFO("Before getchar");
    char c = getchar();
    ROS_INFO("Start sending trajectory");
    ros::spinOnce();
    cur_traj = trajInfo[count];
    cur_traj2 = trajInfo2[count];
    cur_traj3 = trajInfo3[count];
    cur_traj4 = trajInfo4[count];
    cur_t = ros::Time::now().toSec();
    t_end = cur_t + cur_traj.duration*tscale;
    for(int i = 0; i<8 ; i++){
	seg.xcoef[i] = cur_traj.xcoef[i];
	seg.ycoef[i] = cur_traj.ycoef[i];
	seg.zcoef[i] = cur_traj.zcoef[i];
	seg2.xcoef[i] = cur_traj2.xcoef[i];
	seg2.ycoef[i] = cur_traj2.ycoef[i];
	seg2.zcoef[i] = cur_traj2.zcoef[i];
	seg3.xcoef[i] = cur_traj3.xcoef[i];
	seg3.ycoef[i] = cur_traj3.ycoef[i];
	seg3.zcoef[i] = cur_traj3.zcoef[i];
	seg4.xcoef[i] = cur_traj4.xcoef[i];
	seg4.ycoef[i] = cur_traj4.ycoef[i];
	seg4.zcoef[i] = cur_traj4.zcoef[i];
    }
    seg.duration = cur_traj.duration;
    seg2.duration = cur_traj2.duration;
    seg3.duration = cur_traj3.duration;
    seg4.duration = cur_traj4.duration;
    pub.publish(seg);
    pub2.publish(seg2);
    pub3.publish(seg3);
    pub4.publish(seg4);
    ROS_INFO("seg publish: dur %f", seg.duration);
    while(count < num_seg){
        cur_t = ros::Time::now().toSec();
        ros::spinOnce();
        loop_rate.sleep();
    	if(cur_t >= t_end){
    	    count = count+1;
    	    if(count < num_seg){
            	cur_traj = trajInfo[count];
            	cur_traj2 = trajInfo2[count];
            	cur_traj3 = trajInfo3[count];
            	cur_traj4 = trajInfo4[count];
     		t_end = cur_t + cur_traj.duration*tscale;
    		for(int i = 0; i<8 ; i++){
    		    seg.xcoef[i] = cur_traj.xcoef[i];
    		    seg.ycoef[i] = cur_traj.ycoef[i];
    		    seg.zcoef[i] = cur_traj.zcoef[i];
    		    seg2.xcoef[i] = cur_traj2.xcoef[i];
    		    seg2.ycoef[i] = cur_traj2.ycoef[i];
    		    seg2.zcoef[i] = cur_traj2.zcoef[i];
    		    seg3.xcoef[i] = cur_traj3.xcoef[i];
    		    seg3.ycoef[i] = cur_traj3.ycoef[i];
    		    seg3.zcoef[i] = cur_traj3.zcoef[i];
    		    seg4.xcoef[i] = cur_traj4.xcoef[i];
    		    seg4.ycoef[i] = cur_traj4.ycoef[i];
    		    seg4.zcoef[i] = cur_traj4.zcoef[i];
    		}
    		seg.duration = cur_traj.duration;
    		seg2.duration = cur_traj2.duration;
    		seg3.duration = cur_traj3.duration;
    		seg4.duration = cur_traj4.duration;
 	        pub.publish(seg);
 	        pub2.publish(seg2);
 	        pub3.publish(seg3);
 	        pub4.publish(seg4);
        	ROS_INFO("seg publish: dur %f", seg.duration);
      	    }
	   }
    }
    sleep(50);
    return 0;
}


#include "biggles/kalman_filter.hpp"
#include "biggles/observation.hpp"
#include "biggles/partition.hpp"
#include "biggles/samplers.hpp"
#include "biggles/simulate.hpp"
#include "biggles/track.hpp"
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <cmath>
#include <ctime>
#include <inttypes.h>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <utility>
#include <vector>

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

static boost::mt19937 random_generator(std::time(0));

using namespace biggles;

void simulate_one_track(const model::parameters& params)
{
    boost::shared_ptr<track> t_ptr(simulate::generate_track(params, point_2d(10,10), point_2d(), 30));
    boost::shared_ptr<track> t2_ptr(simulate::generate_track(params, point_2d(50,10), point_2d(), t_ptr->last_time_stamp() + 10));

    BOOST_FOREACH(const observation& o, t2_ptr->observations())
    {
        t_ptr->insert(o);
    }

    ok1(t_ptr->size() > 0);

    // save track out to file. This file can be loaded by graph from GNU plotutils
    std::ofstream tx("biggles_simulate_track_tx.txt");
    std::ofstream ty("biggles_simulate_track_ty.txt");

    tx << "#m=-1,S=4\n";
    ty << "#m=-1,S=4\n";

    BOOST_FOREACH(const observation& o, *t_ptr)
    {
        tx << std::setw(16) << t(o) << ' '
           << std::setw(16) << x(o) << " 0" << '\n';
        ty << std::setw(16) << t(o) << ' '
           << std::setw(16) << y(o) << " 0" << '\n';
    }

    // mark end of this dataset
    tx << '\n';
    ty << '\n';

    tx << "#m=2,S=2\n";
    ty << "#m=2,S=2\n";

    Eigen::Matrix2f R(Eigen::Matrix2f::Identity() * 0.1f * 0.1f);
    matrix4f Q(detail::initQ());

    // interpolate missing observations
    kalman_filter kf(*t_ptr, R, Q);
    kalman_filter::states_and_cov_deque smoothed_states_and_covariances;

    // use a _front_ inserter since we generate results in reverse order
    rts_smooth(kf, std::front_inserter(smoothed_states_and_covariances));

    smoothed_states_and_covariances = kf.predictions();

    // the observation matrix
    Eigen::Matrix<float, 2, 4> B;
    B << 1, 0, 0, 0,
         0, 0, 1, 0;

    // save interpolated track out to file
    time_stamp time_stamp(t_ptr->first_time_stamp());
    track::const_iterator track_obs_it(t_ptr->begin());
    const float normalisation(-logf(2.f * M_PI));
    BOOST_FOREACH(const kalman_filter::state_covariance_pair& state_and_covariance,
                  smoothed_states_and_covariances)
    {
        // estimate covariance of pdf
        Eigen::Matrix2f covariance = B * state_and_covariance.second * B.transpose() + R;

        // calculate predicted observation
        Eigen::Vector2f predicted_obs = B * state_and_covariance.first;

        tx << std::setw(16) << time_stamp << ' '
           << std::setw(16) << predicted_obs[0] << ' '
           << std::setw(16) << sqrt(covariance(0,0)) << '\n';

        ty << std::setw(16) << time_stamp << ' '
           << std::setw(16) << predicted_obs[1] << ' '
           << std::setw(16) << sqrt(covariance(1,1)) << '\n';

        // do we have an observation at this time stamp?
        if((track_obs_it != t_ptr->end()) && (t(*track_obs_it) == time_stamp))
        {
            // ... yes

            // find associated observation
            const observation& o(*track_obs_it);
            ++track_obs_it;

            // estimate covariance of pdf
            Eigen::Matrix2f covariance = B * state_and_covariance.second * B.transpose() + R;

            // calculate predicted observation
            Eigen::Vector2f predicted_obs = B * state_and_covariance.first;

            // actual observation
            Eigen::Vector2f obs;
            obs[0] = x(o);
            obs[1] = y(o);

            // this is just a multivariate Gaussian log pdf
            Eigen::Vector2f delta = obs - predicted_obs;
            float a = delta.transpose() * covariance.inverse() * delta;
            float log_pdf = 0.f;
            log_pdf += -0.5f * a;
            log_pdf += -0.5f * logf(covariance.determinant());
            log_pdf += normalisation;

            std::cout << "# " << time_stamp << ' ' << log_pdf << '\n';
        }

        ++time_stamp;
    }
}

void simulate_partition(const model::parameters& params)
{
    partition_ptr_t p;

    // simulate full data
    simulate::generate_partition(p, params, 0, 128 + 128, 128.f, 128.f);

    diag("Generated %zu new tracks.", p->tracks().size());
    diag("Generated %zu clutter observations.", p->clutter().size());

    // crop partition to a specific region
    simulate::crop_partition(p, p, 0.f, 128.f, 0.f, 128.f, 64, 64 + 128);

    const track_collection& simulated_tracks(p->tracks());
    const clutter_t& simulated_clutter(p->clutter());

    diag("Cropped to %zu tracks.", simulated_tracks.size());
    diag("Cropped to %zu clutter observations.", simulated_clutter.size());

    // create a synthetic dataset from the partition
    partition_ptr_t partition_ptr;
    simulate::demote_all_tracks_from_partition(partition_ptr, p);

    // check observations count
    size_t expected_count = p->clutter().size();
    BOOST_FOREACH(const boost::shared_ptr<const track>& p_t, p->tracks())
    {
        expected_count += p_t->size();
    }

    ok1(partition_ptr->clutter().size() == expected_count);
    ok1(partition_ptr->tracks().size() == 0);

}

int main(int argc, char** argv)
{
    plan_tests(1 + 2);

    float sigma_R = 0.1f;

    // initialise model parameters
    model::parameters params;
    model::mean_new_tracks_per_frame(params)           = 1.5f;
    model::mean_false_observations_per_frame(params)   = 2.f;
    model::frame_to_frame_survival_probability(params) = 0.95f;
    model::generate_observation_probability(params)    = 0.8f;
    model::observation_error_covariance(params)       << sigma_R * sigma_R, 0, 0, sigma_R * sigma_R;

    simulate_one_track(params);
    simulate_partition(params);

    return exit_status();
}