/*
 * (C) Crown copyright 2021 Met Office
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include <iostream>
#include <unordered_set>

#include <boost/make_unique.hpp>

#include "atlas/mesh.h"
#include "atlas/meshgenerator.h"
#include "atlas/util/PolygonLocator.h"
#include "atlas/util/PolygonXY.h"

#include "eckit/mpi/Comm.h"
#include "ioda/distribution/AtlasDistribution.h"
#include "ioda/distribution/DistributionFactory.h"
#include "oops/util/Logger.h"

namespace ioda {

namespace  {
const char DIST_NAME[] = "Atlas";
}  // namespace

// -----------------------------------------------------------------------------

/// \brief Assigns records to MPI ranks for the AtlasDistribution.
class AtlasDistribution::RecordAssigner {
 public:
  /// Constructs an Atlas grid and mesh using settings loaded from the `grid` section
  /// of `config`; then partitions the mesh across processes making up the `atlas::mpi::Comm()`
  /// communicator.
  explicit RecordAssigner(const eckit::Configuration & config);

  /// If this record hasn't been assigned to any process yet, assigns it to the process
  /// owning the partition containing `point`.
  ///
  /// It is assumed that records will be assigned in consecutive order.
  void assignRecord(std::size_t recNum, const eckit::geometry::Point2 & point);

  /// Returns true if record `recNum` has been assigned to the calling process, false otherwise.
  bool isMyRecord(std::size_t recNum) const;

 private:
  bool isInMyDomain(const eckit::geometry::Point2 & point) const;

 private:
  atlas::Mesh mesh_;
  std::unique_ptr<atlas::util::PolygonLocator> locator_;

  std::unordered_set<std::size_t> myRecords_;
  std::size_t nextRecordToAssign_ = 0;
};

AtlasDistribution::RecordAssigner::RecordAssigner(const eckit::Configuration & config)
{
  eckit::LocalConfiguration gridConfig(config, "grid");
  atlas::util::Config atlasConfig(gridConfig);

  atlas::Grid grid(atlasConfig);

  atlasConfig.set("type", grid.meshgenerator().getString("type"));
  atlas::MeshGenerator generator(atlasConfig);

  mesh_ = generator.generate(grid);
  if (mesh_->nb_partitions() != atlas::mpi::comm().size()) {
    std::stringstream msg;
    msg << "The number of mesh partitions, " << mesh_->nb_partitions()
        << ", is different from the number of MPI processes, " << atlas::mpi::comm().size();
    throw eckit::Exception(msg.str(), Here());
  }

  locator_ = boost::make_unique<atlas::util::PolygonLocator>(
        atlas::util::ListPolygonXY(mesh_.polygons()), mesh_.projection());
}

void AtlasDistribution::RecordAssigner::assignRecord(std::size_t recNum,
                                                     const eckit::geometry::Point2 & point) {
  if (recNum == nextRecordToAssign_) {
    const bool myRecord = isInMyDomain(point);
    oops::Log::debug() << "RecordAssigner::assignRecord(): is " << recNum << " my record? "
                       << myRecord << std::endl;
    if (myRecord)
      myRecords_.insert(recNum);
    ++nextRecordToAssign_;
  } else {
    // We assume records will be assigned in consecutive order
    ASSERT(recNum < nextRecordToAssign_);
  }
}

bool AtlasDistribution::RecordAssigner::isMyRecord(std::size_t recNum) const {
  return myRecords_.find(recNum) != myRecords_.end();
}

bool AtlasDistribution::RecordAssigner::isInMyDomain(const eckit::geometry::Point2 & point) const {
  const atlas::idx_t partition = (*locator_)(point);
  oops::Log::debug() << "RecordAssigner::isInMyDomain(): Polygon locator says "
                     << point << " is in domain " << partition << std::endl;
  return partition == atlas::mpi::comm().rank();
}

// -----------------------------------------------------------------------------

static DistributionMaker<AtlasDistribution> maker(DIST_NAME);

AtlasDistribution::AtlasDistribution(const eckit::mpi::Comm & comm,
                                   const eckit::Configuration & config)
  : NonoverlappingDistribution(comm),
    recordAssigner_(boost::make_unique<RecordAssigner>(config))
{
  oops::Log::trace() << "AtlasDistribution constructed" << std::endl;
}

AtlasDistribution::~AtlasDistribution() {
  oops::Log::trace() << "AtlasDistribution destructed" << std::endl;
}

void AtlasDistribution::assignRecord(const std::size_t recNum,
                                    const std::size_t locNum,
                                    const eckit::geometry::Point2 & point) {
  recordAssigner_->assignRecord(recNum, point);
  NonoverlappingDistribution::assignRecord(recNum, locNum, point);
}

bool AtlasDistribution::isMyRecord(std::size_t RecNum) const {
  return recordAssigner_->isMyRecord(RecNum);
}

std::string AtlasDistribution::name() const {
  return DIST_NAME;
}
// -----------------------------------------------------------------------------

}  // namespace ioda


#define BOOST_TEST_MODULE "Test LocalLengthGroupSpace class"

#include <boost/test/unit_test.hpp>
#include "LocalLengthGroupSpace.hpp"
#include "TimeSeriesSet.hpp"
#include "distance/Distance.hpp"
#include "Exception.hpp"
#include "Group.hpp"

#define TOLERANCE 1e-9

using namespace konex;

struct MockData
{
  std::string test_group_5_10_space = "datasets/test/test_group_5_10_space.txt";
  std::string test_group_5_10_different_space = "datasets/test/test_group_5_10_different_space.txt";
};

BOOST_AUTO_TEST_CASE( local_length_group_space, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  dist_t distance = pairwiseDistance;
  TimeSeriesSet tsSet;
  tsSet.loadData(data.test_group_5_10_space, 5, 0, " ");

  LocalLengthGroupSpace groups(tsSet, 10);

  BOOST_CHECK_EQUAL( groups.getNumberOfGroups(), 0 );

  groups.generateGroups( distance, 0.5 );
  BOOST_CHECK_EQUAL( groups.getNumberOfGroups(), 1 );
}

BOOST_AUTO_TEST_CASE( local_length_group_space_different, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  dist_t distance = pairwiseDistance;
  TimeSeriesSet tsSet;
  tsSet.loadData(data.test_group_5_10_different_space, 5, 0, " ");

  LocalLengthGroupSpace groups(tsSet, 10);

  BOOST_CHECK_EQUAL( groups.getNumberOfGroups(), 0 );

  groups.generateGroups( distance, 0.5 );
  BOOST_CHECK_EQUAL( groups.getNumberOfGroups(), 2);
}

BOOST_AUTO_TEST_CASE( groups_best_group, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  dist_t distance = warpedDistance;
  TimeSeriesSet tsSet;
  tsSet.loadData(data.test_group_5_10_different_space, 5, 0, " ");

  LocalLengthGroupSpace groups(tsSet, 10);
  groups.generateGroups( distance, 0.5 );

  setWarpingBandRatio(1.0);

  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,0,10), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,0,9), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,0,8), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,0,6), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,0,5), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,0,4), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,4,10), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(1,5,10), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(0), groups.getBestGroup(tsSet.getTimeSeries(0,3,7), distance, INF).first);

  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,0,10), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,0,9), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,0,8), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,0,6), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,0,5), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,0,4), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,4,10), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,5,10), distance, INF).first);
  BOOST_CHECK_EQUAL( groups.getGroup(1), groups.getBestGroup(tsSet.getTimeSeries(4,6,10), distance, INF).first);
}


#include "worldGenerator.hpp"

#include <stdexcept>
#include <algorithm>
#include <set>

#include <boost/make_shared.hpp>

#include "world.hpp"
#include "point.hpp"

#include "anthill.hpp"
#include "creature.hpp"
#include "ant.hpp"
#include "obstacle.hpp"
#include "food.hpp"

// initialize static members
#define INIT_STATIC(cl, st, val) st cl::val = st();
INIT_STATIC(WorldGenerator, AnthillParams, default_anthill_params);
INIT_STATIC(WorldGenerator, AntsParams, default_ants_params);
INIT_STATIC(WorldGenerator, ObstaclesParams, default_obstacles_params);
INIT_STATIC(WorldGenerator, FoodsParams, default_food_params);
INIT_STATIC(WorldGenerator, PheromoneParams, default_pheromone_params);

void WorldGenerator::placeAnthill(World* world, AnthillParams& params)
{
    for(unsigned i = 0; i < params.quantity; i++)
    {
        Point pos(rand() % world->width_, rand() % (world->height_));

        world->addSimulationObject<Anthill>(
            boost::make_shared<Anthill>(world, pos));
    }
}

void WorldGenerator::placeAnts(World* world, AntsParams& params)
{
    if(params.min_dist_from_anthill > params.max_dist_from_anthill)
        throw std::runtime_error("Min dist can't be higher than max dist");
    if(world->getSimulationObjects<Anthill>().size() == 0)
        throw std::runtime_error("No anthill!");

    auto free_spaces = getFreeSpaces(world);
    unsigned num_spawned = 0;

    while(num_spawned < params.quantity)
    {
        if(free_spaces.empty())
            throw std::runtime_error(
                "WorldGenerator::placeAnts no available empty space");

        Point pos = fitNewPos(world, params, free_spaces);

        world->addSimulationObject<Creature>(
            boost::make_shared<Ant>(world, pos,
                decideAntType(num_spawned, params)));
        num_spawned++;
    }
}

void WorldGenerator::placeObstacles(World* world, ObstaclesParams& params)
{
    if(params.quantity_per_100_by_100 > 10000)
        throw std::runtime_error("Invalid quantity per 100 by 100 of obstacle");

    const int num_to_spawn = params.quantity_per_100_by_100 *
        world->width_ * world->height_ / 10000;

    int num_spawned = 0;
    while(num_spawned < num_to_spawn)
    {
        for(const auto& point : params.blob.generate(
                world->width_, world->height_))
        {
            if(num_spawned > num_to_spawn) break;

            if(!point.isInBounds(world->width_, world->height_))
                continue;

            world->addSimulationObject<Obstacle>(
                boost::make_shared<Obstacle>(world, point));

            num_spawned++;
        }
    }
}

void WorldGenerator::placeFoods(World* world, FoodsParams& params)
{
    if(params.quantity_per_100_by_100 > 10000)
        throw std::runtime_error("Invalid quantity per 100 by 100 of foods");

    const unsigned num_to_spawn = params.quantity_per_100_by_100 *
        world->width_ * world->height_ / 10000;

    unsigned num_spawned = 0;
    while(num_spawned < num_to_spawn)
    {
        for(const auto& point : params.blob.generate(
                world->width_, world->height_))
        {
            if(num_spawned > num_to_spawn) break;

            if(!point.isInBounds(world->width_, world->height_))
                continue;

            world->addSimulationObject<Food>(
                boost::make_shared<Food>(world, point));

            num_spawned++;
        }
    }
}

void WorldGenerator::initPheromoneMaps(World* world, PheromoneParams& params)
{
    world->pheromone_maps_.emplace_back(
        boost::make_shared<PheromoneMap>(world, PheromoneMap::Type::ToFood,
            world->width_, world->height_, params.to_food_decay_rate));
    world->pheromone_maps_.emplace_back(
        boost::make_shared<PheromoneMap>(world, PheromoneMap::Type::FromFood,
            world->width_, world->height_, params.from_food_decay_rate));
    world->pheromone_maps_.emplace_back(
        boost::make_shared<PheromoneMap>(world, PheromoneMap::Type::Anthill,
            world->width_, world->height_, params.anthill_decay_rate));
}

std::set<Point> WorldGenerator::getTakenSpaces(World* world)
{
    // using set because entities may overlap but we want unique spaces
    auto obstacles = world->getSimulationObjects<Obstacle>();
    std::set<Point> s;
    for(const auto& o : obstacles) s.insert(o->getPos());
    return s;
}

std::vector<Point> WorldGenerator::getFreeSpaces(World* world)
{
    std::vector<Point> v = getAllPoints(world);
    for(const auto& t : getTakenSpaces(world))
        v.erase(std::remove(v.begin(), v.end(), t), v.end());
    return v;
}

std::vector<Point> WorldGenerator::getAllPoints(World* world)
{
    std::vector<Point> v;
    auto dim = world->getDimensions();
    for(unsigned x = 0; x < (unsigned)dim.posX(); x++)
        for(unsigned y = 0; y < (unsigned)dim.posY(); y++)
            v.emplace_back(Point(x, y));
    return v;
}

Point WorldGenerator::fitNewPos(World* world, AntsParams& params,
    std::vector<Point>& free_spaces)
{
    // todo: handle more anthills
    const auto anthill_pos =
        world->getSimulationObjects<Anthill>()[0]->getPos();

    Point pos;
    unsigned failed_counter = 0;
    const unsigned failed_threshold = 100;
    do
    {
        pos = free_spaces[rand() % free_spaces.size()];
        failed_counter++;

        if(failed_counter > failed_threshold)
        {
            failed_counter = 0;
            params.max_dist_from_anthill *= 2;
        }
    } while(
        pos.getDistance(anthill_pos) < params.min_dist_from_anthill ||
        pos.getDistance(anthill_pos) > params.max_dist_from_anthill);

    free_spaces.erase(
            std::remove(free_spaces.begin(), free_spaces.end(), pos),
        free_spaces.end());

    return pos;
}

Ant::Type WorldGenerator::decideAntType(const int num_spawned, AntsParams& params)
{
    if(num_spawned == 0)
        // spawn one queen
        return Ant::Type::Queen;
    else if(num_spawned ==1){
        // at least one Scout
        return Ant::Type::Scout;
    }else if(num_spawned <= params.ratio_scouts * params.quantity)
        return Ant::Type::Scout;
    else
        return Ant::Type::Worker;
}


#include <boost/geometry/multi/core/topological_dimension.hpp>


#include <arrow/table.h>
#include <arrow/array.h>
#include <arrow/builder.h>
#include <arrow/compute/api_vector.h>
#include <arrow/pretty_print.h>

#include <boost/assert.hpp>

#include <iostream>
#include <memory>
#include <limits>
#include <iterator>
#include <algorithm>

#include "attribute_mapper.h"
#include "arrow_utils.h"
#include "arrow_convenience.h"
#include "arrowcube.h"

#include "htm.h"
#include <iomanip>

/******************************************************************************/

using namespace std;
using namespace arrow;
using namespace htm;

template <typename BuilderT>
std::shared_ptr<ChunkedArray> from_builder(BuilderT &b)
{
  return shared_ptr<ChunkedArray>(
      new ChunkedArray( { b.Finish().ValueOrDie() } ));
}

int main(int argc, char **argv)
{
  shared_ptr<Table> t = read_feather_table(
      "/Users/cscheid/data/ztf/feather-summaries/20200918_summary.feather_u");
  describe_table(t);

  {
    RowIterator rows({
        t->GetColumnByName("ra"),
        t->GetColumnByName("dec")
      });

    NumericBuilder<DoubleType> x, y, z;
    shared_ptr<ChunkedArray> htm_ids =
        map_rows<UInt64Type>(rows, [&rows, &x, &y, &z]() {
          double ra = rows.cols_[0].value<DoubleType>(),
              dec = rows.cols_[1].value<DoubleType>();
          Vec2 v(ra, dec);
          Vec3 c = to_cartesian(v);
          OK_OR_DIE(x.Append(c.x));
          OK_OR_DIE(y.Append(c.y));
          OK_OR_DIE(z.Append(c.z));
          uint64_t result = htm_id(c, 5);
          return result;
        });

    shared_ptr<Table> xyz = make_table({
        { "x", from_builder(x) },
        { "y", from_builder(y) },
        { "z", from_builder(z) }
      });

    shared_ptr<Table> g = make_gaussian_stats_table(xyz);

    t = arrow_cbind({t, g, make_table({{"htm_id", htm_ids}})});
  }

  compute::SortOptions options({
      compute::SortKey("htm_id", compute::SortOrder::Ascending)
    });
  t = sort_table(t, &options);
  t = CompressAggregation<UInt64Type, DoubleType>::call(
      t, { "htm_id" },
      { "magpsf",
        "s0",
        "s1",
        "s2",
        "s3",
        "s4",
        "s5",
        "s6",
        "s7",
        "s8",
        "s9" });
  cerr << "Aggregated" << endl;

  write_arrow(t, "agg.arrow");

  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_ACCESS_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_ACCESS_HPP

#include <boost/geometry/core/access.hpp>

#include <boost/geometry/extensions/algebra/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch
{

template <typename Vector, typename CoordinateType, std::size_t Dimension>
struct access<vector_tag, Vector, CoordinateType, Dimension, boost::false_type>
{
    static inline CoordinateType get(Vector const& v)
    {
        return traits::access<Vector, Dimension>::get(v);
    }
    static inline void set(Vector& v, CoordinateType const& value)
    {
        traits::access<Vector, Dimension>::set(v, value);
    }
};

template <typename Vector, typename CoordinateType, std::size_t Dimension>
struct access<vector_tag, Vector, CoordinateType, Dimension, boost::true_type>
{
    static inline CoordinateType get(Vector const* v)
    {
        return traits::access<typename boost::remove_pointer<Vector>::type, Dimension>::get(*v);
    }
    static inline void set(Vector* v, CoordinateType const& value)
    {
        traits::access<typename boost::remove_pointer<Vector>::type, Dimension>::set(*v, value);
    }
};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<quaternion_tag, Q, CoordinateType, Dimension, boost::false_type>
{
    static inline CoordinateType get(Q const& v)
    {
        return traits::access<Q, Dimension>::get(v);
    }
    static inline void set(Q& v, CoordinateType const& value)
    {
        traits::access<Q, Dimension>::set(v, value);
    }
};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<quaternion_tag, Q, CoordinateType, Dimension, boost::true_type>
{
    static inline CoordinateType get(Q const* v)
    {
        return traits::access<typename boost::remove_pointer<Q>::type, Dimension>::get(*v);
    }
    static inline void set(Q* v, CoordinateType const& value)
    {
        traits::access<typename boost::remove_pointer<Q>::type, Dimension>::set(*v, value);
    }
};

template<typename M, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<matrix_tag, M, CoordinateType, I, J, boost::false_type>
    : detail::indexed_access_non_pointer<M, CoordinateType, I, J>
{};

template<typename M, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<matrix_tag, M, CoordinateType, I, J, boost::true_type>
    : detail::indexed_access_pointer<M, CoordinateType, I, J>
{};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<rotation_quaternion_tag, Q, CoordinateType, Dimension, boost::false_type>
{
    static inline CoordinateType get(Q const& v)
    {
        return traits::access<Q, Dimension>::get(v);
    }
    static inline void set(Q& v, CoordinateType const& value)
    {
        traits::access<Q, Dimension>::set(v, value);
    }
};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<rotation_quaternion_tag, Q, CoordinateType, Dimension, boost::true_type>
{
    static inline CoordinateType get(Q const* v)
    {
        return traits::access<typename boost::remove_pointer<Q>::type, Dimension>::get(*v);
    }
    static inline void set(Q* v, CoordinateType const& value)
    {
        traits::access<typename boost::remove_pointer<Q>::type, Dimension>::set(*v, value);
    }
};

template<typename RM, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<rotation_matrix_tag, RM, CoordinateType, I, J, boost::false_type>
    : detail::indexed_access_non_pointer<RM, CoordinateType, I, J>
{};

template<typename RM, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<rotation_matrix_tag, RM, CoordinateType, I, J, boost::true_type>
    : detail::indexed_access_pointer<RM, CoordinateType, I, J>
{};

} // namespace core_dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_ACCESS_HPP


#include "BopPsiReceiver.h"
#include <future>
#include "Crypto/PRNG.h"
#include "Crypto/Commit.h"
#include "PSI/SimpleHasher.h"
#include "Common/Log.h"
#include "OT/Base/naor-pinkas.h"
#include <unordered_map>
#include <type_traits>
#include <typeinfo>
#include <boost/mpl/if.hpp>
namespace mpl = boost::mpl;

namespace bOPRF
{

	std::string hexString(u8* data, u64 length)
	{
		std::stringstream ss;

		for (u64 i = 0; i < length; ++i)
		{

			ss << std::hex << std::setw(2) << std::setfill('0') << (u16)data[i];
		}

		return ss.str();
	}

	BopPsiReceiver::BopPsiReceiver()
	{
	}

	BopPsiReceiver::~BopPsiReceiver()
	{
	}

	void BopPsiReceiver::init(u64 senderSize, u64 recverSize, u64 statSecParam, Channel & chl0, SSOtExtReceiver& ots, block seed)
	{
		init(senderSize, recverSize, statSecParam, { &chl0 }, ots, seed);
	}

	void BopPsiReceiver::init(u64 senderSize, u64 recverSize, u64 statSecParam, const std::vector<Channel*>& chls, SSOtExtReceiver& otRecv, block seed)
	{

		mStatSecParam = statSecParam;
		mSenderSize = senderSize;
		mRecverSize = recverSize;

		mNumStash = get_stash_size(recverSize);

		gTimer.setTimePoint("Init.start");
		PRNG prngHashing(seed);
		block myHashSeeds;
		myHashSeeds = prngHashing.get_block();
		auto& chl0 = *chls[0];

		// we need a random hash function, so both commit to a seed and then decommit later
		chl0.asyncSend(&myHashSeeds, sizeof(block));
		block theirHashingSeeds;
		chl0.asyncRecv(&theirHashingSeeds, sizeof(block));

		//gTimer.setTimePoint("Init.hashSeed");

		// init Cuckoo hash
		mBins.init(mRecverSize, mSenderSize);

		// makes codeword for each bins
		mSSOtMessages.resize(mBins.mBinCount + mNumStash);

		//do base OT
		if (otRecv.hasBaseSSOts() == false)
		{
			//Timer timer;
			gTimer.setTimePoint("Init: BaseSSOT start");
			Log::setThreadName("receiver");
			BaseSSOT baseSSOTs(chl0, OTRole::Sender);
			baseSSOTs.exec_base(prngHashing);
			baseSSOTs.check();
			otRecv.setBaseSSOts(baseSSOTs.sender_inputs);
			gTimer.setTimePoint("Init: BaseSSOT done");
			//	Log::out << gTimer;
		}

		mHashingSeed = myHashSeeds ^ theirHashingSeeds;

		//gTimer.setTimePoint("Init.ExtStart");
		//extend OT
		otRecv.Extend(mBins.mBinCount + mNumStash, mSSOtMessages, chl0);

		//gTimer.setTimePoint("r Init.Done");
		//	Log::out << gTimer;
	}

	void BopPsiReceiver::sendInput(std::vector<block>& inputs, Channel & chl)
	{
		sendInput(inputs, { &chl });
	}

	struct has_const_member
	{
		const bool x;

		has_const_member(bool x_)
			: x(x_)
		{ }

	};

	void BopPsiReceiver::sendInput(std::vector<block>& inputs, const std::vector<Channel*>& chls)
	{

		//const bool leq1 = true;
		//define keysearch of mask based on mask length
//		typedef std::conditional<leq1, u32, u64>::type uMask;

		// check that the number of inputs is as expected.
		if (inputs.size() != mRecverSize)
			throw std::runtime_error("inputs.size() != mN");
		gTimer.setTimePoint("R Online.Start");

		//asign channel
		auto& chl = *chls[0];

		SHA1 sha1;
		u8 hashBuff[SHA1::HashSize];

		//random seed
		PRNG prng(_mm_set_epi32(42534612345, 34557734565, 211234435, 23987045));

		u64 codeWordSize = get_codeword_size(std::max<u64>(mSenderSize, mRecverSize)); //by byte
		u64 maskSize = get_mask_size(mSenderSize, mRecverSize); //by byte
		blockBop codeWord;

		//hash all items, use for: 1) arrage each item to bin using Cuckoo;
		//                         2) use for psedo-codeword.
		std::array<AES, 4> AESHASH;
		TODO("make real keys seeds");
		for (u64 i = 0; i < AESHASH.size(); ++i)
			AESHASH[i].setKey(_mm_set1_epi64x(i));

		std::array<std::vector<block>, 4> aesHashBuffs;
		aesHashBuffs[0].resize(inputs.size());
		aesHashBuffs[1].resize(inputs.size());
		aesHashBuffs[2].resize(inputs.size());
		aesHashBuffs[3].resize(inputs.size());

		for (u64 i = 0; i < inputs.size(); i += stepSize)
		{
			auto currentStepSize = std::min(stepSize, inputs.size() - i);

			AESHASH[0].ecbEncBlocks(inputs.data() + i, currentStepSize, aesHashBuffs[0].data() + i);
			AESHASH[1].ecbEncBlocks(inputs.data() + i, currentStepSize, aesHashBuffs[1].data() + i);
			AESHASH[2].ecbEncBlocks(inputs.data() + i, currentStepSize, aesHashBuffs[2].data() + i);
			AESHASH[3].ecbEncBlocks(inputs.data() + i, currentStepSize, aesHashBuffs[3].data() + i);
		};

		//insert item to corresponding bin
		mBins.insertItems(aesHashBuffs);
		//mBins.print();

		//we use 4 unordered_maps, we put the mask to the corresponding unordered_map
		//that indicates of the hash function index 0,1,2. and the last unordered_maps is used for stash bin
		std::array<std::unordered_map<u64, std::pair<block, u64>>, 3> localMasks;
		//store the masks of elements that map to bin by h0
		localMasks[0].reserve(mBins.mBinCount); //upper bound of # mask
		//store the masks of elements that map to bin by h1
		localMasks[1].reserve(mBins.mBinCount);
		//store the masks of elements that map to bin by h2
		localMasks[2].reserve(mBins.mBinCount);

		std::unique_ptr<ByteStream> locaStashlMasks(new ByteStream());
		locaStashlMasks->resize(mNumStash* maskSize);

		//======================Bucket BINs (not stash)==========================

		//pipelining the execution of the online phase (i.e., OT correction step) into multiple batches
		TODO("run in parallel");
		auto binStart = 0;
        auto binEnd = mBins.mBinCount / mBins.binSize;
        auto tableSize = mBins.mBinCount / mBins.binSize;

        gTimer.setTimePoint("R Online.computeBucketMask start");
        //for each batch
        for (u8 binIdx = 0; binIdx < mBins.binSize; binIdx++)
        {
            for (u64 stepIdx = binStart; stepIdx < binEnd; stepIdx += stepSize)
            {
                // compute the size of current step & end index.
                auto currentStepSize = std::min(stepSize, binEnd - stepIdx);
                auto stepEnd = stepIdx + currentStepSize;

                // make a buffer for the pseudo-code we need to send
                std::unique_ptr<ByteStream> buff(new ByteStream());
                buff->resize((sizeof(blockBop) * currentStepSize));
                auto myOt = buff->getArrayView<blockBop>();

                // for each bin, do encoding
                for (u64 bIdx = stepIdx, i = 0; bIdx < stepEnd; bIdx++, ++i)
                {
                    auto &item = mBins.mBins[binIdx][bIdx];
                    block mask(ZeroBlock);

                    if (item.isEmpty() == false)
                    {
                        codeWord.elem[0] = aesHashBuffs[0][item.mIdx];
                        codeWord.elem[1] = aesHashBuffs[1][item.mIdx];
                        codeWord.elem[2] = aesHashBuffs[2][item.mIdx];
                        codeWord.elem[3] = aesHashBuffs[3][item.mIdx];

                        // encoding will send to the sender.
                        myOt[i] =
                            codeWord ^ mSSOtMessages[bIdx + binIdx * tableSize][0] ^ mSSOtMessages[bIdx][1];

                        //compute my mask
                        sha1.Reset();
                        sha1.Update((u8 *)&item.mHashIdx, sizeof(u64)); //
                        sha1.Update((u8 *)&mSSOtMessages[bIdx + binIdx * tableSize][0], codeWordSize);
                        sha1.Final(hashBuff);

                        // store the my mask value here
                        memcpy(&mask, hashBuff, maskSize);

                        //store my mask into corresponding buff at the permuted position
                        // localMasks[item.mHashIdx].emplace(*(u64 *)&mask, std::pair<block, u64>(mask, item.mIdx));
                        localMasks[binIdx].emplace(*(u64 *)&mask, std::pair<block, u64>(mask, binIdx));
                    }
                    else
                    {
                        // no item for this bin, just use a dummy.
                        myOt[i] = prng.get_block512(codeWordSize);
                    }
                }
                // send the OT correction masks for the current step
                chl.asyncSend(std::move(buff));
            } // Done with compute the masks for the main set of bins.
        }
		gTimer.setTimePoint("R Online.sendBucketMask done");

		//receive the sender's marks, we have 3 buffs that corresponding to the mask of elements used hash index 0,1,2
		for (u64 buffIdx = 0; buffIdx < 3; buffIdx++)
		{
			ByteStream recvBuff;
			chl.recv(recvBuff);

			// double check the size.
			if (recvBuff.size() != mSenderSize* maskSize)
			{
				Log::out << "recvBuff.size() != expectedSize" << Log::endl;
				throw std::runtime_error("rt error at " LOCATION);
			}

			auto theirMasks = recvBuff.data();

			//loop each mask
			if (maskSize >= 8)
			{
				//if masksize>=8, we can check 64 bits of key from the map first
				for (u64 i = 0; i < mSenderSize; ++i)
				{
					auto& msk = *(u64*)(theirMasks);

					// check 64 first bits
					auto match = localMasks[buffIdx].find(msk);

					//if match, check for whole bits
					if (match != localMasks[buffIdx].end())
					{
						if (memcmp(theirMasks, &match->second.first, maskSize) == 0) // check full mask
						{
							mIntersection.push_back(match->second.second);
							//Log::out << "#id: " << match->second.second << Log::endl;
						}
					}
					theirMasks += maskSize;
				}
			}
			else
			{
				for (u64 i = 0; i < mSenderSize; ++i)
				{
					for (auto match = localMasks[buffIdx].begin(); match != localMasks[buffIdx].end(); ++match)
					{
						if (memcmp(theirMasks, &match->second.first, maskSize) == 0) // check full mask
						{
							mIntersection.push_back(match->second.second);
							//Log::out << "#id: " << match->second.second << Log::endl;
						}
					}
					theirMasks += maskSize;
				}
			}
		}
		gTimer.setTimePoint("R Online.Bucket done");

		//======================STASH BIN==========================
		std::unique_ptr<ByteStream> stashBuff(new ByteStream());
		stashBuff->resize((sizeof(blockBop)*mBins.mStash.size()));
		auto myOt = stashBuff->getArrayView<blockBop>();

		gTimer.setTimePoint("R Online.Stash start");
		// compute the encoding for each item in the stash.
		for (u64 i = 0, otIdx = mBins.mBinCount; i < mBins.mStash.size(); ++i, ++otIdx)
		{
			auto& item = mBins.mStash[i];
			block mask(ZeroBlock);

			if (item.isEmpty() == false)
			{
				codeWord.elem[0] = aesHashBuffs[0][item.mIdx];
				codeWord.elem[1] = aesHashBuffs[1][item.mIdx];
				codeWord.elem[2] = aesHashBuffs[2][item.mIdx];
				codeWord.elem[3] = aesHashBuffs[3][item.mIdx];

				myOt[i] =
					codeWord
					^ mSSOtMessages[otIdx][0]
					^ mSSOtMessages[otIdx][1];

				sha1.Reset();
				sha1.Update((u8*)&mSSOtMessages[otIdx][0], codeWordSize);
				sha1.Final(hashBuff);

				memcpy(locaStashlMasks->data() + i * maskSize, hashBuff, maskSize);
			}
			else
			{
				myOt[i] = prng.get_block512(codeWordSize);
			}
		}

		chl.asyncSend(std::move(stashBuff));
		gTimer.setTimePoint("R Online.sendStashMask done");

		//receive masks from the stash
		for (u64 sBuffIdx = 0; sBuffIdx < mNumStash; sBuffIdx++)
		{
			ByteStream recvBuff;
			chl.recv(recvBuff);
			if (mBins.mStash[sBuffIdx].isEmpty() == false)
			{
				// double check the size.
				auto cntMask = mSenderSize;
				gTimer.setTimePoint("Online.MaskReceived from STASH");
				if (recvBuff.size() != cntMask* maskSize)
				{
					Log::out << "recvBuff.size() != expectedSize" << Log::endl;
					throw std::runtime_error("rt error at " LOCATION);
				}

				auto theirMasks = recvBuff.data();
					for (u64 i = 0; i < cntMask; ++i)
					{
						//check stash
							if (memcmp(theirMasks, locaStashlMasks->data()+ sBuffIdx*maskSize, maskSize) == 0)
							{
								mIntersection.push_back(mBins.mStash[sBuffIdx].mIdx);
								//Log::out << "#id: " << match->second.second << Log::endl;
							}

						theirMasks += maskSize;
					}
			}
		}

	gTimer.setTimePoint("Online.Done");
	//	Log::out << gTimer << Log::endl;
}
}


//
// $Id: IsotopeCalculator.cpp 2051 2010-06-15 18:39:13Z chambm $
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2006 Louis Warschaw Prostate Cancer Center
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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.
//

#define PWIZ_SOURCE

#include "IsotopeCalculator.hpp"
#include "IsotopeTable.hpp"
#include "Ion.hpp"
#include "pwiz/utility/misc/Std.hpp"
#include <boost/ptr_container/ptr_vector.hpp>

namespace pwiz {
namespace chemistry {

using boost::ptr_vector;

class IsotopeCalculator::Impl
{
    public:

    Impl(double abundanceCutoff, double massPrecision);

    MassDistribution distribution(const Formula& formula,
                                  int chargeState,
                                  int normalization) const;
    private:

    double abundanceCutoff_;
    double massPrecision_;

    ptr_vector<IsotopeTable> tableStorage_;
    typedef map<Element::Type, const IsotopeTable*> TableMap;
    TableMap tableMap_;

    void initializeIsotopeTables();
    MassDistribution distributionManually(Element::Type e, int atomCount) const;
};

IsotopeCalculator::Impl::Impl(double abundanceCutoff, double massPrecision)
:   abundanceCutoff_(abundanceCutoff),
    massPrecision_(massPrecision)
{
    initializeIsotopeTables();
}

namespace {

bool hasLessMass(const MassAbundance& a, const MassAbundance& b)
{
    return a.mass < b.mass;
}

MassAbundance coalesce(const MassAbundance& a, const MassAbundance& b)
{
    double abundance = a.abundance + b.abundance;
    double mass = (a.abundance*a.mass + b.abundance*b.mass) / abundance;
    return MassAbundance(mass, abundance);
}

class Distinct
{
    public:

    Distinct(double mass, double precision)
    :   mass_(mass), precision_(precision)
    {}

    bool operator()(const MassAbundance& ma)
    {
        return fabs(ma.mass - mass_) > precision_;
    }

    private:
    double mass_;
    double precision_;
};

MassDistribution coalesceDistribution(const MassDistribution& md, double precision)
{
    // assumes MassDistribution md is sorted by mass!

    MassDistribution result;

    for (MassDistribution::const_iterator it=md.begin(); it!=md.end();)
    {
        Distinct distinct(it->mass, precision);
        MassDistribution::const_iterator next = find_if(it, md.end(), distinct);
        result.push_back(accumulate(it, next, MassAbundance(), coalesce));
        it = next;
    }

    return result;
}

class Convolve
{
    public:

    Convolve(double cutoff = 0)
    :   cutoff_(cutoff)
    {}

    MassDistribution operator()(const MassDistribution& m, const MassDistribution& n)
    {
        if (m.empty()) return n;
        if (n.empty()) return m;

        MassDistribution result;

        // we could break out of these loops early if the mass distributions
        // are sorted by abundance

        for (MassDistribution::const_iterator i=m.begin(); i!=m.end(); ++i)
        for (MassDistribution::const_iterator j=n.begin(); j!=n.end(); ++j)
        {
            double mass = i->mass + j->mass;
            double abundance = i->abundance * j->abundance;
            if (abundance > cutoff_)
                result.push_back(MassAbundance(mass, abundance));
        }

        return result;
    }

    private:
    double cutoff_;
};

class Ionize
{
    public:

    Ionize(int chargeState)
    :   chargeState_(chargeState)
    {}

    void operator()(MassAbundance& ma) const
    {
        ma.mass = Ion::mz(ma.mass, chargeState_);
    }

    private:
    int chargeState_;
};

void normalize(MassDistribution& md, int normalization)
{
    if (md.empty())
        return;

    double abundanceScale = 1;

    if (normalization & IsotopeCalculator::NormalizeAbundance)
    {
        double sumSquaredAbundances = 0;
        for (MassDistribution::iterator it=md.begin(); it!=md.end(); ++it)
        {
            double a = it->abundance;
            sumSquaredAbundances += a*a;
        }

        abundanceScale = sqrt(sumSquaredAbundances);
    }

    double massShift = (normalization & IsotopeCalculator::NormalizeMass) ? md[0].mass : 0;

    for (MassDistribution::iterator it=md.begin(); it!=md.end(); ++it)
    {
        it->mass -= massShift;
        it->abundance /= abundanceScale;
    }
}

} // namespace

MassDistribution IsotopeCalculator::Impl::distribution(const Formula& formula,
                                                       int chargeState,
                                                       int normalization) const
{
    // collect the distributions for each element in the formula

    vector<MassDistribution> distributions;

    Formula::Map formulaData = formula.data();
    for (Formula::Map::const_iterator it=formulaData.begin(); it!=formulaData.end(); ++it)
    {
        Element::Type e = it->first;
        int atomCount = it->second;

        TableMap::const_iterator table = tableMap_.find(e);
        if (table != tableMap_.end())
            distributions.push_back(table->second->distribution(atomCount));
        else
            distributions.push_back(distributionManually(e, atomCount));
    }

    // coalesce each elemental distribution

    vector<MassDistribution> coalescedDistributions;
    for (vector<MassDistribution>::iterator it=distributions.begin(); it!=distributions.end(); ++it)
        coalescedDistributions.push_back(coalesceDistribution(*it, massPrecision_));

    // combine the distributions and sort by mass

    MassDistribution combined = accumulate(coalescedDistributions.begin(), coalescedDistributions.end(),
                                           MassDistribution(), Convolve(abundanceCutoff_));

    sort(combined.begin(), combined.end(), hasLessMass);

    MassDistribution result = coalesceDistribution(combined, massPrecision_);

    // adjust for charge state

    if (chargeState)
        for_each(result.begin(), result.end(), Ionize(chargeState));

    // normalize if requested

    if (normalization)
        normalize(result, normalization);

    return result;
}

namespace {

struct TableInfo
{
    Element::Type element;
    int maxAtomCount;
};

TableInfo tableInfo_[] =
{
    {Element::C, 5000},
    {Element::H, 8000},
    {Element::N, 1500},
    {Element::O, 1500},
    {Element::S, 50},
};

const int tableInfoSize_ = sizeof(tableInfo_)/sizeof(TableInfo);

} // namespace

void IsotopeCalculator::Impl::initializeIsotopeTables()
{
    for (TableInfo* it=tableInfo_; it!=tableInfo_+tableInfoSize_; ++it)
    {
        IsotopeTable* temp(new IsotopeTable(Element::Info::record(it->element).isotopes,
                                            it->maxAtomCount,
                                            abundanceCutoff_));
        tableMap_[it->element] = temp; // store pointer in the map
        tableStorage_.push_back(temp); // maintain ownership in the ptr_vector
    }
}

MassDistribution IsotopeCalculator::Impl::distributionManually(Element::Type e,
                                                                        int atomCount) const
{
    throw runtime_error("[IsotopeCalculator::distribution()] No table for element "
        + Element::Info::record(e).symbol);
}

PWIZ_API_DECL IsotopeCalculator::IsotopeCalculator(double abundanceCutoff, double massPrecision)
:   impl_(new Impl(abundanceCutoff, massPrecision))
{}

PWIZ_API_DECL IsotopeCalculator::~IsotopeCalculator(){} // auto destruction of impl_

PWIZ_API_DECL
MassDistribution IsotopeCalculator::distribution(const Formula& formula,
                                                 int chargeState,
                                                 int normalization) const
{
    return impl_->distribution(formula, chargeState, normalization);
}

} // namespace chemistry
} // namespace pwiz


#pragma once

#include <Eigen/Dense>
#include <PnC/Filters/Basic/filters.hpp>
#include <PnC/DracoPnC/DracoDefinition.hpp>
#include <PnC/DracoPnC/StateEstimator/BodyEstimator.hpp>
#include <Utils/IO/Pthread.hpp>
#include <vector>

class DracoStateProvider;
class RobotSystem;

////////////////////////////////////////////
// Must to be synchronized with MoCap system
/////////////////////////////////////////////
#define MOCAP_DATA_PORT 51128
#define IP_ADDRESS "192.168.0.144"
#define NUM_MARKERS 13

typedef struct {
    int visible[NUM_MARKERS];
    double data[NUM_MARKERS * 3];
} draco_message;

enum MocapLed { cTorsoLed = 0, lTorsoLed = 1, rTorsoLed = 2 };

class DracoMoCapManager : public Pthread {
   public:
    friend class BodyEstimator;

    DracoMoCapManager(RobotSystem *);
    virtual ~DracoMoCapManager();

    virtual void run(void);

    Eigen::Quaternion<double> body_quat_;  // R_global(mocap)_torso
    void CoordinateUpdateCall() { b_update_call_ = true; }

   protected:
    std::vector<Eigen::Vector3d> healthy_led_list_;

    std::vector<filter *> body_led0_filter_;
    std::vector<filter *> body_led1_filter_;
    std::vector<filter *> body_led2_filter_;

    double initialization_duration_;
    Eigen::Vector3d offset_;
    Eigen::Quaternion<double> imu_body_ori_;

    DracoStateProvider *sp_;
    Eigen::MatrixXd R_coord_;
    bool b_update_call_;

    void _Print_message(const draco_message &msg);
    void _UpdateLEDPosData(const draco_message &msg);
    void _CoordinateUpdate(draco_message &msg);
    void _CoordinateChange(draco_message &msg);
    // R_torso_global(mocap)
    Eigen::MatrixXd _GetOrientation(const Eigen::Vector3d &,
                                    const Eigen::Vector3d &,
                                    const Eigen::Vector3d &);

    int socket_;
    std::vector<int> marker_cond_;

    Eigen::VectorXd led_pos_data_;
    Eigen::VectorXd led_kin_data_;
    Eigen::VectorXd led_pos_raw_data_;

    int lfoot_idx;
    int rfoot_idx;

    RobotSystem *robot_;
};


#include <iostream>
//#include <iomanip>
//#include <fstream>

//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/vector.hpp>
//#include <boost/numeric/ublas/io.hpp>

#include <vector>
#include <cmath>
#include <cstring>

//#include <unistd.h>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
//#include "NRG_main.hpp"
//#include "TwoChQS.hpp"

#ifndef pi
#define pi 3.141592653589793238462643383279502884197169
#endif

int main (int argc, char* argv[]){

  CNRGCodeHandler ThisCode;

  CNRGbasisarray Acut;
  CNRGbasisarray Abasis;

  CNRGmatrix* MatArray;

  ThisCode.NumNRGmats=0;
  ThisCode.pAcut=&Acut;
  ThisCode.pAbasis=&Abasis;
  //ThisCode.MatArray=MatArray;

  ThisCode.ReadGenPars("");

  cout << "Lambda = " << ThisCode.Lambda << endl;
  cout << "Nsitesmax = " << ThisCode.Nsitesmax << endl;

  Acut.Nshell=0;
  Abasis.Nshell=0;
  ThisCode.ReadArrays("test");

  Acut.PrintAll();

}
// end MAIN


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuIdentityOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_operators.h"
#include "hoCuNDArray_blas.h"
#include "hoCuCgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoCuPartialDerivativeOperator.h"
#include "GPUTimer.h"

#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

int
main(int argc, char** argv)
{
  string acquisition_filename;
  string outputFile;
  uintd3 imageSize;
  floatd3 voxelSize;
  float reg_weight;
  int device;
  unsigned int dump;
  unsigned int downsamples;
  unsigned int iterations;

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    ("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    ("output,f", po::value<string>(&outputFile)->default_value("reconstruction.real"), "Output filename")
    ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    ("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    ("SAG","Use exact SAG correction if present")
    ("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    ("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    ("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    ("weight,w",po::value<float>(&reg_weight)->default_value(float(0.1f)),"Regularization weight")
    ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    ("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    ;

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

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }
  std::cout << "Command line options:" << std::endl;
  for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
    boost::any a = it->second.value();
    std::cout << it->first << ": ";
    if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
    else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
    else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
    else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
    else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
    else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
    else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
    else std::cout << "Unknown type" << std::endl;
    std::cout << std::endl;
  }
  cudaSetDevice(device);
  cudaDeviceReset();

  //Really weird stuff. Needed to initialize the device?? Should find real bug.
  cudaDeviceManager::Instance()->lockHandle();
  cudaDeviceManager::Instance()->unlockHandle();

  boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
  ps->load(acquisition_filename);
  ps->get_geometry()->print(std::cout);
  ps->downsample(downsamples);

  float SDD = ps->get_geometry()->get_SDD();
  float SAD = ps->get_geometry()->get_SAD();

  boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
  if (vm.count("binning")){
    std::cout << "Loading binning data" << std::endl;
    binning->load(vm["binning"].as<string>());
  } else
    binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));

  binning->print(std::cout);

  floatd3 imageDimensions;
  if (vm.count("dimensions")){
    imageDimensions = vm["dimensions"].as<floatd3>();
    voxelSize = imageDimensions/imageSize;
  }
  else imageDimensions = voxelSize*imageSize;

  float lengthOfRay_in_mm = norm(imageDimensions);
  unsigned int numSamplesPerPixel = 3;
  float minSpacing = min(voxelSize)/numSamplesPerPixel;

  unsigned int numSamplesPerRay;
  if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
  else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

  float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
  size_t numProjs = ps->get_projections()->get_size(2);
  size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);

  std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);
  std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
  std::cout << "Image size " << imageDimensions << std::endl;

  is_dims.push_back(binning->get_number_of_bins());

  hoCuNDArray<float> projections(*ps->get_projections());

  // Define encoding operator
  boost::shared_ptr< hoCuConebeamProjectionOperator >
    E( new hoCuConebeamProjectionOperator() );

  E->setup(ps,binning,imageDimensions);
  E->set_domain_dimensions(&is_dims);
  E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

  // Define regularization operator
  boost::shared_ptr< hoCuIdentityOperator<float> >
    I( new hoCuIdentityOperator<float>() );

  I->set_weight(reg_weight);

  hoCuCgSolver<float> solver;

  solver.set_encoding_operator(E);

  if( reg_weight>0.0f ) {
    std::cout << "Adding identity operator with weight " << reg_weight << std::endl;
    solver.add_regularization_operator(I);
  }

  solver.set_max_iterations(iterations);
  solver.set_tc_tolerance(1e-8);
  solver.set_output_mode(hoCuCgSolver<float>::OUTPUT_VERBOSE);

  /*  if (vm.count("TV")){
    boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dx (new hoCuPartialDerivativeOperator<float,4>(0) );
    boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dy (new hoCuPartialDerivativeOperator<float,4>(1) );
    boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dz (new hoCuPartialDerivativeOperator<float,4>(2) );
    boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dt (new hoCuPartialDerivativeOperator<float,4>(3) );

    dx->set_codomain_dimensions(&is_dims);
    dy->set_codomain_dimensions(&is_dims);
    dz->set_codomain_dimensions(&is_dims);
    dt->set_codomain_dimensions(&is_dims);

    dx->set_domain_dimensions(&is_dims);
    dy->set_domain_dimensions(&is_dims);
    dz->set_domain_dimensions(&is_dims);
    dt->set_domain_dimensions(&is_dims);

    dx->set_weight(vm["TV"].as<float>());
    dy->set_weight(vm["TV"].as<float>());
    dz->set_weight(vm["TV"].as<float>());
    dt->set_weight(vm["TV"].as<float>());

    solver.add_regularization_group_operator(dx);
    solver.add_regularization_group_operator(dy);
    solver.add_regularization_group_operator(dz);
    solver.add_regularization_group_operator(dt);
    solver.add_group(1);
    }*/

  // Run solver
  //

  boost::shared_ptr< hoCuNDArray<float> > result;

  {
    GPUTimer timer("\nRunning conjugate gradient solver");
    result = solver.solve(&projections);
  }

  write_nd_array<float>( result.get(), outputFile.c_str());
}