#pragma once
//standard include
#include <math.h>
#include <iostream>

//opencv include
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include <boost/circular_buffer.hpp>

#include "field_obj_tracker/track3d.hpp"

struct DepthInfo
{
	float depth;
	bool  error;
};

//this contains all the info we need to decide between goals once we are certain if it is a goal
struct GoalInfo
{
	float confidence;
	float distance;
	cv::Rect rect;
	size_t contour_index;
	bool depth_error;
	cv::Point com;
	cv::Rect br;
	cv::RotatedRect rtRect;
};

//This contains all the necessary info for a goal
struct GoalFound
{
	float distance;
	float confidence;
	size_t contour_index;
	cv::Rect rect;
	cv::RotatedRect rotated_rect;
	std::string id;
};

class GoalDetector
{
	public:
		GoalDetector(void);

		std::vector< GoalFound > return_found(void) const;

		void drawOnFrame(cv::Mat &image,const std::vector< std::vector< cv::Point>> &contours, const std::vector< GoalFound > &goals) const;

		//These are the three functions to call to run GoalDetector
		//they fill in _contours, _infos, _depth_mins, etc
		void clear(void);

		//If your objectypes have the same width it's safe to run
		//getContours and computeConfidences with different types
		void findTargets(const cv::Mat& image, const cv::Mat& depth, const ObjectNum objtype, const image_geometry::PinholeCameraModel &model);
		const std::vector< std::vector< cv::Point > > getContours(const cv::Mat& image);

		bool Valid(void) const;
		void setBlueScale(double blue_scale);
		void setRedScale(double red_scale);
		void setOtsuThreshold(int otsu_threshold);
		void setMinConfidence(double min_valid_confidence);

	private:
		// Save detection info
		bool        _isValid;
		std::vector< GoalFound > _return_found;
		float       _min_valid_confidence;

		int         _otsu_threshold;
		int         _blue_scale;
		int         _red_scale;

		float createConfidence(float expectedVal, float expectedStddev, float actualVal);
		//float distanceUsingFixedHeight(const cv::Rect &rect,const cv::Point &center, float expected_delta_height) const;
		bool generateThresholdAddSubtract(const cv::Mat& imageIn, cv::Mat& imageOut);
		void isValid();
		const std::string getObjectId(ObjectNum type) const;
		const std::vector<DepthInfo> getDepths(const cv::Mat &depth, const std::vector< std::vector< cv::Point > > &contours, const ObjectNum objtype, const image_geometry::PinholeCameraModel &model);
		const std::vector< GoalInfo > getInfo(const cv::Size &frame_size, const std::vector< std::vector< cv::Point > > &contours, const std::vector<DepthInfo> &depth_maxs, ObjectNum objtype, const image_geometry::PinholeCameraModel &model);
};


//#include <Eigen/Core>
#include <Eigen/LU>
#include <stdint.h>
#include "tilebase.h"
#include "nd.h"
#include "xform.h"

#define LOG(...)          fprintf(stderr,__VA_ARGS__)
#define REPORT(msg1,msg2) LOG("%s(%d) - %s()\n\t%s\n\t%s\n",__FILE__,__LINE__,__FUNCTION__,msg1,msg2)
#define TRY(e)            do{ if(!(e)) {REPORT(#e,"Expression evaluated as false"); goto Error;}}while(0)

using namespace Eigen;

typedef  Map<Matrix<float,Dynamic,Dynamic,RowMajor> > xform_t;
typedef  Matrix<int64_t,Dynamic,1>     vi64_t;
typedef  vi64_t                        vori_t;
typedef  vi64_t                        vshape_t;

/** returned value is reference to static variable.
    Past references may change value when this function is called again!
 */
static const vori_t vori(aabb_t box_nm)
{ int64_t *v=0;
  size_t ndim=0;
  AABBGet(box_nm,&ndim,&v,0);
  vi64_t r(ndim+2,1); // bbox omits color and affine-lifting dimensions
  r.tail<2>()<<0,1;
  r.segment(0,ndim)=Map<vi64_t>(v,ndim);
  return r;
}

/** returned value is reference to static variable.
    Past references may change value when this function is called again!
 */
static const vshape_t vshape(aabb_t box_nm)
{ int64_t *v=0;
  size_t ndim=0;
  AABBGet(box_nm,&ndim,0,&v);
  vi64_t r(ndim+2,1); // bbox omits color and affine-lifting dimensions
  r.tail<2>()<<0,1;
  r.segment(0,ndim)=Map<vi64_t>(v,ndim);
  return r;
}

static const xform_t xform(tile_t self)
{ xform_t xform(TileTransform(self),5,5);
  return xform;
}

/** makes a bounding box from two opposed corners */
static aabb_t fromCorners(const MatrixXf& r1, const MatrixXf& r2)
{ vi64_t tl,br,s;
  tl=r1.cwiseMin(r2).cast<int64_t>();
  br=r1.cwiseMax(r2).cast<int64_t>();
  s=br-tl;
  aabb_t out=AABBMake(tl.rows()-2); // remove affine and color dims
  AABBSet(out,tl.rows()-2,tl.data(),s.data());
  return out;
}

/** makes a bounding box from an array's shape. */
static aabb_t fromShape(nd_t shape)
{ aabb_t out=AABBMake(3); // hardcoded number of dimensions
  size_t *s=ndshape(shape);
  int64_t o[3]={0,0,0},
         is[3]={s[0],s[1],s[2]};
  return AABBSet(out,3,o,is);
}

/** caller must free returned result with AABBFree */
aabb_t AABBToPx(tile_t self, aabb_t box_nm)
{ MatrixXf T,r1,r2;
  r1=vori(box_nm).cast<float>();
  T=xform(self).inverse();
  r2=(vori(box_nm)+vshape(box_nm)).cast<float>();
  r2.bottomRightCorner<1,1>()<<1;
  aabb_t pbox=fromShape(TileShape(self)),
          out=AABBIntersectIP(fromCorners(T*r1,T*r2),pbox);
  AABBFree(pbox);
  return out;
Error:
  return 0;
}

static size_t* int64_to_static_sizet(int64_t* v,size_t n)
{ size_t i;
  static size_t *out=0,sz=0;
  if(n>sz)
  { sz=1.2*n+50;
    TRY(out=(size_t*)malloc(sizeof(size_t)*sz));
  }
  for(i=0;i<n;++i) out[i]=(size_t)v[i];
Error:
  return out;
}

static size_t* int64_to_new_sizet(int64_t* v,size_t n)
{ size_t i,*out=0;
  TRY(out=(size_t*)malloc(sizeof(size_t)*n));
  for(i=0;i<n;++i) out[i]=(size_t)v[i];
Error:
  return out;
}

//caller must use ndfree to release shape field in returned struct.
struct nd_subarray_t AABBToSubarray(tile_t self, aabb_t box_nm)
{ aabb_t box_px=0;
  struct nd_subarray_t out={0};
  TRY(box_px=AABBToPx(self,box_nm)); // should already be cropped to tile
  { size_t ndim,i;
    int64_t *ori,*shape;
    nd_t s=TileShape(self);
    AABBGet(box_px,&ndim,&ori,&shape);
    ndPushShape(s);
    for(i=0;i<ndim;++i)
      ndShapeSet(s,(unsigned)i,(size_t)shape[i]); // leave the non spatial dimensions the same.  This will get all color channels, for example.
    out.shape=ndheap(s);
    ndPopShape(s);

    TRY(out.ori=(size_t*)malloc(sizeof(size_t)*ndndim(s)));
    for(i=0;i<ndim;++i)
      out.ori[i]=(size_t)ori[i];
    for(;i<ndndim(s);++i)
      out.ori[i]=0;
  }
Finalize:
  AABBFree(box_px);
  return out;
Error:
  out.ori=0;
  ndfree(out.shape);
  out.shape=0;
  goto Finalize;
}


#include "TimeSeriesSet.hpp"

#include <algorithm>
#include <string>
#include <fstream>
#include <iostream>
#include <cstring>
#include <boost/tokenizer.hpp>

#include "distance/Distance.hpp"
#include "Exception.hpp"

using std::string;
using std::cout;
using std::endl;

namespace konex {

TimeSeriesSet::~TimeSeriesSet()
{
  this->clearData();
}

int countNumberOfLines(std::ifstream& f)
{
  int lineCount = 0;
  string line;
  for (lineCount = 0; std::getline(f, line); ++lineCount);
  f.clear();
  f.seekg(0);
  return lineCount;
}

inline int calcPAALength(int srcLength, int n)
{
  return (srcLength - 1) / n + 1;
}

void doPAA(const data_t* source, data_t* dest, int srcLength, int n)
{
  data_t sum = 0;
  int count = 0;
  int destLength = calcPAALength(srcLength, n);
  for (int i = 0; i < srcLength; i++)
  {
    count++;
    sum += source[i];
    if (count == n || i == srcLength - 1) {
      dest[i / n] = sum / count;
      sum = 0;
      count = 0;
    }
  }
}

TimeSeries tsPAA(const TimeSeries& source, int n)
{
  data_t sum = 0;
  int count = 0;
  int srcLength = source.getLength();
  int destLength = calcPAALength(srcLength, n);
  TimeSeries dest(destLength);
  for (int i = 0; i < srcLength; i++)
  {
    count++;
    sum += source[i];
    if (count == n || i == srcLength - 1) {
      dest[i / n] = sum / count;
      sum = 0;
      count = 0;
    }
  }
  return dest;
}

void TimeSeriesSet::loadData(const string& filePath, int maxNumRow,
                             int startCol, const string& separators)
{
  this->clearData();

  std::ifstream f(filePath);
  if (!f.is_open())
  {
    f.close();
    throw KOnexException(string("Cannot open ") + filePath);
  }

  if (maxNumRow <= 0) {
    maxNumRow = countNumberOfLines(f);
  }
  else {
    maxNumRow = std::min(maxNumRow, countNumberOfLines(f));
  }

  int length = -1;
  string line;
  boost::char_separator<char> sep(separators.c_str());
  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;

  this->itemCount = 0;
  for (int row = 0; row < maxNumRow; row++, this->itemCount++)
  {
    // Read and process line by line
    if (getline(f, line))
    {
      tokenizer tokens(line, sep);

      // Number of columns in the first line is assumed to be number of columns of
      // the whole dataset
      if (row == 0)
      {
        // If this is the first row, set length of each row to length of this row
        length = std::distance(tokens.begin(), tokens.end());
        this->data = new data_t[maxNumRow * length];
        memset(this->data, 0, maxNumRow * length * sizeof(data_t*));

      }
      else if (length != std::distance(tokens.begin(), tokens.end()))
      {
        f.close();
        this->clearData();
        throw KOnexException("File contains time series with inconsistent lengths");
      }

      int col = 0;
      for (tokenizer::iterator tok_iter = tokens.begin();
           tok_iter != tokens.end(); tok_iter++, col++)
      {
        // Only read columns from startCol and after
        if (col >= startCol)
        {
          try
          {
            data[row * (length - startCol) + (col - startCol)] = (data_t)std::stod(*tok_iter);
          }
          catch (const std::invalid_argument& e)
          {
            f.close();
            this->clearData();
            throw KOnexException("Dataset file contains unparsable text");
          }
          catch (const std::out_of_range& e)
          {
            f.close();
            this->clearData();
            throw KOnexException("Values are out of range");
          }
        }
      }
    }
    else {
      break;
    }
  }

  if (f.bad())
  {
    f.close();
    this->clearData();
    throw KOnexException("Error while reading file");
  }

  this->itemLength = length - startCol;
  this->filePath = filePath;

  f.close();
}

void TimeSeriesSet::saveData(const string& filePath, char separator) const
{
  std::ofstream f(filePath);
  if (!f.is_open())
  {
    f.close();
    throw KOnexException(string("Cannot open ") + filePath);
  }
  for (int i = 0; i < itemCount; i++) {
    for (int j = 0; j < itemLength; j++) {
      f << data[i * itemLength + j] << separator;
    }
    f << endl;
  }
  f.close();
}

void TimeSeriesSet::clearData()
{
  delete[] this->data;
  this->data = nullptr;
  this->itemCount = 0;
  this->itemLength = 0;
}

TimeSeries TimeSeriesSet::getTimeSeries(int index, int start, int end) const
{
  if (index < 0 || index >= this->itemCount)
  {
    throw KOnexException("Invalid time series index");
  }
  if (start < 0 && end < 0)
  {
    return TimeSeries(this->data + index * this->itemLength, index, 0, this->itemLength);
  }
  if (start < 0 || start >= end || end > this->itemLength)
  {
    throw KOnexException("Invalid starting or ending position of a time series");
  }
  return TimeSeries(this->data + index * this->itemLength, index, start, end);
}

std::pair<data_t, data_t> TimeSeriesSet::normalize(void)
{
  int length = this->getItemLength() * this->getItemCount();

  if (!length)
  {
    throw KOnexException("No data to normalize");
  }

  // auto minAndMax = std::minmax(this->data);
  // data_t diff = *minAndMax.first - *minAndMax.second;
  data_t MIN = INF;
  data_t MAX = -INF;
  unsigned int i;

  // find min and max in 1.5 comparisons per element
  for (int ts = 0; ts < this->itemCount; ts++)
  {
    data_t x, y, z;

    // start at 0 if even, 1 if odd
    if ((i = (length % 2 != 0)))
    {
      if (data[ts * this->itemLength] < MIN)
      {
        MIN = data[ts * this->itemLength];
      }
      if (data[ts * this->itemLength] > MAX)
      {
        MAX = data[ts * this->itemLength];
      }
    }

    for (; i < this->itemLength - 1; i += 2)
    {
      x = data[ts * this->itemLength + i];
      y = data[ts * this->itemLength + (i + 1)];
      if ( x > y )
      {
        z = y;
        y = x;
        x = z;
      }
      if ( y > MAX )
      {
        MAX = y;
      }
      if ( x < MIN )
      {
        MIN = x;
      }
    }
  }

  data_t diff = MAX - MIN;

  // do not divide by zero
  if (diff == 0.0)
  {
    if (MAX != 0)
    {
      // zero out data (use memset instead...)
      for (int ts = 0; ts < this->itemCount; ts++)
      {
        for (i = 0; i < this->itemLength; i++)
        {
          data[ts * this->itemLength + i] = 0;
        }
      }
    }
  }
  else
  {
    // normalize
    for (int ts = 0; ts < this->itemCount; ts++)
    {
      for (i = 0; i < this->itemLength; i++)
      {
        data[ts * this->itemLength + i] = (data[ts * this->itemLength + i] - MIN)/ diff;
      }
    }
  }
  normalized = true;
  return std::make_pair(MIN, MAX);
}

void TimeSeriesSet::PAA(int n)
{
  if (n <= 0) {
    throw KOnexException("Block size must be positive");
  }
  int newItemLength = calcPAALength(this->itemLength, n);
  auto new_data = new data_t[this->itemCount * newItemLength];
  for (int ts = 0; ts < this->itemCount; ts++)
  {
    doPAA(this->data + ts * this->itemLength, new_data + ts * newItemLength,
      this->itemLength, n);
  }
  delete this->data;
  this->data = new_data;
  this->itemLength = newItemLength;
}

bool TimeSeriesSet::isLoaded()
{
  return this->data != nullptr;
}

std::vector<candidate_time_series_t> TimeSeriesSet::kSimRaw(
  const TimeSeries& query, int k, int PAABlock)
{
  if (k <= 0) {
    throw KOnexException("K must be positive");
  }
  std::vector<candidate_time_series_t> bestSoFar;
  dist_t warpedDistance = cascadeDistance;
  data_t bestSoFarDist, currentDist;
  int timeSeriesLength = getItemLength();
  int numberTimeSeries = getItemCount();

  TimeSeries PAAQuery(0);
  if (PAABlock > 0)
  {
    PAAQuery = tsPAA(query, PAABlock);
  }

  // iterate through every timeseries
  for (int idx = 0; idx < numberTimeSeries; idx++)
  {
    // iterate through every length of interval
    for (int intervalLength = 2; intervalLength <= timeSeriesLength;
        intervalLength++)
    {
      // iterate through all interval window lengths
      for (int start = 0; start <= timeSeriesLength - intervalLength;
            start++)
      {
        TimeSeries currentTimeSeries = getTimeSeries(idx, start, start + intervalLength);
        if (k > 0) {
          if (PAABlock > 0)
          {
            TimeSeries PAACurrentTimeSeries = tsPAA(currentTimeSeries, PAABlock);
            currentDist = warpedDistance(PAAQuery, PAACurrentTimeSeries, INF);
          }
          else {
            currentDist = warpedDistance(query, currentTimeSeries, INF);
          }
          bestSoFar.push_back(candidate_time_series_t(currentTimeSeries, currentDist));
          k--;
          if (k == 0) {
            // Heapify exactly once when the heap is filled.
            std::make_heap(bestSoFar.begin(), bestSoFar.end());
          }
        }
        else
        {
          bestSoFarDist = bestSoFar.front().dist;
          if (PAABlock > 0)
          {
            TimeSeries PAACurrentTimeSeries = tsPAA(currentTimeSeries, PAABlock);
            currentDist = warpedDistance(PAAQuery, PAACurrentTimeSeries, bestSoFarDist);
          }
          else {
            currentDist = warpedDistance(query, currentTimeSeries, bestSoFarDist);
          }
          if (currentDist < bestSoFarDist)
          {
            bestSoFar.push_back(candidate_time_series_t(currentTimeSeries, currentDist));
            std::push_heap(bestSoFar.begin(), bestSoFar.end());
            std::pop_heap(bestSoFar.begin(), bestSoFar.end());
            bestSoFar.pop_back();
          }
        }
      }
    }
  }

  if (PAABlock > 0) {
    for (auto i = 0; i < bestSoFar.size(); i++) {
      bestSoFar[i].dist = warpedDistance(query, bestSoFar[i].data, INF);
    }
  }
  std::sort(bestSoFar.begin(), bestSoFar.end());

  return bestSoFar;
}

} // namespace konex


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <graphblas/graphblas.hpp>

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE sparse_ewiseadd_vector_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

/// @todo add tests with accumulate

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

namespace
{
    std::vector<double> v3a_dense = {12, 0, 7};

    std::vector<double> v4a_dense = {0, 0, 12, 7};
    std::vector<double> v4b_dense = {0,  1, 0, 2};
    std::vector<double> v4c_dense = {3,  6, 9, 1};

    std::vector<double> zero3_dense = {0, 0, 0};
    std::vector<double> zero4_dense = {0, 0, 0, 0};

    std::vector<double> twos3_dense = {2, 2, 2};
    std::vector<double> twos4_dense = {2, 2, 2, 2};

    std::vector<double> ans_4atwos4_dense = {2, 2, 14, 9};
    std::vector<double> ans_4a4b_dense = {0, 1, 12, 9};
}

//****************************************************************************
// Tests without mask
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_bad_dimensions)
{
    grb::Vector<double> v(v3a_dense, 0.);
    grb::Vector<double> u(v4a_dense, 0.);

    grb::Vector<double> result(3);

    // incompatible input dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result,
                       grb::NoMask(),
                       grb::NoAccumulate(),
                       grb::Plus<double>(), v, u)),
        grb::DimensionException);

    // incompatible output vector dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result,
                       grb::NoMask(),
                       grb::NoAccumulate(),
                       grb::Plus<double>(), u, u)),
        grb::DimensionException);

    // Additional tests.
    grb::Vector<double> a(v3a_dense, 0.);
    grb::Vector<double> b(v4a_dense, 0.);
    grb::Vector<double> result_a(3);
    grb::Vector<double> result_b(4);

    // w0 m0 u1 v1
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_a,
                       a,
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       b,
                       b)),
        grb::DimensionException);

    // w0 m1 u0 v0
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_a,
                       b,
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       a,
                       a)),
        grb::DimensionException);

    // w1 m0 u0 v1
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_b,
                       a,
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       a,
                       b)),
        grb::DimensionException);

    // w1 m1 u1 v0
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_b,
                       b,
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       b,
                       a)),
        grb::DimensionException);

    // w1 m0 u1 v1
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_b,
                       a,
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       b,
                       b)),
        grb::DimensionException);

    // w0 m1 u1 v1
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_a,
                       b,
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       b,
                       b)),
        grb::DimensionException);

    // w0 no u1 v0
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_a,
                       grb::NoMask(),
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       b,
                       a)),
        grb::DimensionException);

    // w1 no u1 v0
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_b,
                       grb::NoMask(),
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       b,
                       a)),
        grb::DimensionException);

    // w1 no u0 v0
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result_b,
                       grb::NoMask(),
                       grb::NoAccumulate(),
                       grb::Plus<double>(),
                       a,
                       a)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_reg)
{
    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    grb::Vector<double> ans(ans_4atwos4_dense, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(ans_4a4b_dense, 0.);
    grb::eWiseAdd(result,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(v4a_dense, 0.);
    grb::eWiseAdd(result,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_stored_zero_result)
{
    grb::Vector<double> u(v4a_dense); //, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    grb::Vector<double> ans(ans_4atwos4_dense); //, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(ans_4a4b_dense); //, 0.);
    grb::eWiseAdd(result,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(v4a_dense);//, 0.);
    grb::eWiseAdd(result,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
// Tests using a mask with REPLACE
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_masked_replace_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Plus<double>(), v, u,
                       grb::REPLACE)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_masked_replace_reg)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_masked_replace_reg_stored_zero)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_semiring_vector_masked_replace_reg_stored_zero)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
// Tests using a mask with MERGE semantics
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_masked_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Plus<double>(), v, u, grb::MERGE)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_masked_reg)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_masked_reg_stored_zero)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_semiring_vector_masked_reg_stored_zero)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v2, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  mask,
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v3, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
// Tests using a complemented mask with REPLACE semantics
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_scmp_masked_replace_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Plus<double>(), v, u,
                       grb::MERGE)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_scmp_masked_replace_reg)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_scmp_masked_replace_reg_stored_zero)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_semiring_vector_scmp_masked_replace_reg_stored_zero)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
// Tests using a complemented mask (with merge semantics)
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_scmp_masked_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Plus<double>(), v, u, grb::MERGE)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_scmp_masked_reg)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_scmp_masked_reg_stored_zero)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v2, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), u, v3, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_semiring_vector_scmp_masked_reg_stored_zero)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise add with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 2, 14, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise add with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    std::vector<double> answer2 = {0, 1, 12, 0};
    grb::Vector<double> ans2(answer2, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v2, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise add with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    std::vector<double> answer3 = {0, 0, 12, 0};
    grb::Vector<double> ans3(answer3, 0.);
    grb::eWiseAdd(result,
                  grb::complement(mask),
                  grb::NoAccumulate(),
                  add_monoid(grb::ArithmeticSemiring<double>()),
                  u, v3, grb::MERGE);
    BOOST_CHECK_EQUAL(result, ans3);
}

//****************************************************************************
// Tests using an Accumulator (incomplete).
//****************************************************************************

//***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_vector_accum)
{
    grb::Vector<double> v(v3a_dense, 0.);
    grb::Vector<double> u(twos3_dense, 0.);

    grb::Vector<double> result1({1, 1, 1}, 0.);
    grb::Vector<double> ans1({15, 3, 10}, 0.);
    grb::eWiseAdd(result1,
                  grb::NoMask(),
                  grb::Plus<double>(),
                  grb::Plus<double>(), u, v);
    BOOST_CHECK_EQUAL(result1, ans1);

    grb::Vector<double> result2({1, 1, 1}, 0.);
    grb::Vector<double> ans2({-13, -1, -8}, 0.);
    grb::eWiseAdd(result2,
                  grb::NoMask(),
                  grb::Minus<double>(),
                  grb::Plus<double>(), u, v);
    BOOST_CHECK_EQUAL(result2, ans2);
}

BOOST_AUTO_TEST_SUITE_END()


// -*-coding: mule-utf-8-unix; fill-column: 58; -*-

/**
 * @file
 * The generic scrypt implementations.
 *
 * @author Sergei Lodyagin <serg@kogorta.dp.ua>
 */

#ifndef SCRYPT_HPP
#define SCRYPT_HPP

#include <boost/detail/endian.hpp>
#include "constexpr_math.h"
#include "scrypt.h"

namespace meta {

//! The little-endian constexpr detector. C++11 sucks, use boost,
constexpr bool is_little_endian()
{
#ifdef BOOST_LITTLE_ENDIAN
  return true;
#else
  return false;
#endif
}

}

namespace hash {
namespace scrypt {

void
PBKDF2_SHA256(const uint8_t *passwd, size_t passwdlen, const uint8_t *salt,
    size_t saltlen, uint64_t c, uint8_t *buf, size_t dkLen);

inline uint32_t integerify(uint32_t a)
{
  return a;
}

#ifdef USE_SSE2
uint32_t integerify(__m128i a);
#endif

// The current SSE2 salsa20 implementation needs word
// rearrangement

template<unsigned r>
inline void rearrange_before(
  std::array<generic::SalsaBlock, 2*r>& x)
{
}

template<unsigned r>
inline void rearrange_after(std::array<generic::SalsaBlock, 2*r>& x)
{
}

#ifdef USE_SSE2
void rearrange_before(sse2::SalsaBlock& x);

template<unsigned r>
void rearrange_before(std::array<sse2::SalsaBlock, 2*r>& x)
{
  for (int k = 0; k < x.size(); k++)
    rearrange_before(x[k]);
}

void rearrange_after(sse2::SalsaBlock& x);

template<unsigned r>
void rearrange_after(std::array<sse2::SalsaBlock, 2*r>& x)
{
  for (int k = 0; k < x.size(); k++)
    rearrange_after(x[k]);
}
#endif

//! It is the Colin Percival's BlockMix implementation
template<
  unsigned r,
  class SalsaBlockT
>
std::array<SalsaBlockT, 2*r> block_mix(const std::array<SalsaBlockT, 2*r>& b)
{
  std::array<SalsaBlockT, 2*r> b1;

  // X <- B[2r-1]
  SalsaBlockT x(b[2*r - 1]);
  for (unsigned i = 0; i < r; i++) {
    // X <- H(X (+) B[2i])
    xor_salsa8(x, b[2*i]);
    // B'[i] <- X
    b1[i] = x;
    // X <- H(X (+) B[2i+1])
    xor_salsa8(x, b[2*i+1]);
    // B'[r+i] <- X
    b1[r+i] = x;
  }
  return b1;
}

//! It is the Colin Percival's ROMix implementation
template<
  uint32_t N,
  unsigned r,
  unsigned p,
  class SalsaBlockT,
  std::array<SalsaBlockT, 2*r> (*H)(const std::array<SalsaBlockT, 2*r>& b)
>
void romix
  (std::array<SalsaBlockT, 2*r>& x,    //< the input/output vector
   std::array<std::array<SalsaBlockT, 2*r>, N>& v   //< the work area
   )
{
  using namespace constexpr_math;

  // The current SSE2 salsa20 implementation needs word rearrangement
  rearrange_before<r>(x);

  for (uint32_t i = 0; i < N; i++) {
    v[i] = x;
    x = H(x);
  }
  for (uint32_t i = 0; i < N; i++) {
    static_assert(pow2x<log2x<N>::value>::value == N, "N must be pow of 2");
    // NB is N is pow 2 than we can integrify in mod 2^32-1
    // and then mod N
    const uint32_t j = integerify(x[2*r-1][0]) % N;
    x = H(x ^= v[j]);
  }

  rearrange_after<r>(x);
}

template<
  size_t N,   //< the number of cells to ROMix (the real number of
              //< used bytes is 1024*N*r/8 = 128*N*r.
              //< See (*) below. Due to this line N must be pow of 2
              //< and <= 2^32
  unsigned r, //< the size parameter to BlockMix (use r cells and
              //< salsa20/8 each, r = 2n, n >= 1, the BlockMix block
              //< size is 1024*r bits = 128*r bytes
  unsigned p    ,  //< the number of parallel processes, it is hardcoded as
                   //< 1 here, you need change the program for change
                   //< this parameter, do not try pass different value as the
                   //< template argument
  class SalsaBlockT,
  class Password, //< the input sequence - password
  class Salt, //< the input sequence - salt
  class Output //< the output sequence
>
void scrypt_256_sp_templ
  (
   const Password& password,
   const Salt& salt,
         Output& output,
         Scratchpad<N, r, p, SalsaBlockT>& scratchpad
  )
{
  std::array<std::array<SalsaBlockT, 2*r>, p> B;

  static_assert(sizeof(B) == 2 * r * p * sizeof(SalsaBlockT),
                "Invalid types definition");

  PBKDF2_SHA256(
    reinterpret_cast<const uint8_t*>(password.data()),
    password.size(),
    reinterpret_cast<const uint8_t*>(salt.data()),
    salt.size(),
    1,
    reinterpret_cast<uint8_t*>(B.data()),
    sizeof(B));

  static_assert(meta::is_little_endian(),
    "The big endian version of scrypt is not implemented yet");

  static_assert(p == 1,
    "The case p != 1 (multithreaded) is not implemented");
  romix<N, r, p, SalsaBlockT, block_mix<r, SalsaBlockT>>(B[0], scratchpad[0]);

  PBKDF2_SHA256
   (reinterpret_cast<const uint8_t*>(password.data()),
    password.size(),
    reinterpret_cast<uint8_t*>(B.data()),
    sizeof(B),
    1,
    reinterpret_cast<uint8_t*>(output.data()),
    output.size());
}

} // scrypt
} // hash

#endif


///////////////////////////////////////////////////////////////////////////////
// ars::proposal_sampler.hpp                                                 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 BOOST_STATISTICS_DETAIL_ARS_PROPOSAL_SAMPLER_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_PROPOSAL_SAMPLER_HPP_ER_2009
#include <cmath>
#include <vector>
#include <algorithm>
#include <functional>
#include <string>
#include <iterator>
#include <numeric>
#include <limits>

#include <boost/static_assert.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/type_traits/is_base_of.hpp>

#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/bind.hpp>
#include <boost/random.hpp>
#include <boost/range.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/function.hpp>
#include <boost/format.hpp>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/sign.hpp>

#include <boost/ars/constant.hpp>
#include <boost/ars/parameter.hpp>
#include <boost/ars/error.hpp>
#include <boost/ars/point.hpp>
#include <boost/ars/function/signature.hpp>
#include <boost/ars/detail/data.hpp>
#include <boost/ars/detail/area.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

// This class maintains an approximation to the cumulative density of interest
// and generate draws from it
//
// Requirements:
// Cont holds the data and is required to be bi-directional and is to be used
// in conjunction with lower_bound and insert, so deque is asymptotically the
// best choice. In practice, though, vector is probably the best choice.
template<
    typename T,
    template<typename,typename> class Cont = std::vector,
    template<typename> class Alloc = std::allocator
>
class proposal_sampler{

    BOOST_STATIC_ASSERT(std::numeric_limits<T>::has_infinity);

    protected:
    typedef std::string                                         str_t;
    typedef constant<T>                                         const_;
    typedef parameter<T>                                        param_;
    typedef point<T>                                            point_t;
    typedef tangent_intersection<T>                             tang_t;
    typedef data<T>                                             data_t;
    typedef Alloc<data_t>                                       alloc_t;
    typedef Cont<data_t,alloc_t>                                datas_t;
    typedef typename boost::range_difference<datas_t>::type     diff_t;
    typedef typename boost::range_iterator<datas_t>::type       iter_t;
    typedef boost::uniform_real<T>                              runif_t;

    public:
    typedef typename ars::function::signature<T>::type          signature;
    typedef boost::function<signature>                          delegate_type;

    typedef typename boost::range_size<datas_t>::type           size_type;
    typedef T                                                   result_type;
    typedef typename runif_t::input_type                        input_type;

    proposal_sampler();
    proposal_sampler(const proposal_sampler& that);
    proposal_sampler& operator=(const proposal_sampler& that);

    // x_min must be constant<T>::inf_ if unbounded. Likewise x_max
    // fun's syntax is fun(x,y,dy) writes to x and dy
    void set_function(T x_min,T x_max,const delegate_type& fun);
    template<typename R> //Range
    void initialize(const R& initial_data);
    void initialize(T x_0,T x_1);
    // Increasing this number improves precision but may cause
    // an infinite area (TODO investigate). In doubt, don't use it
    void set_max_log(T v);

    // Returns true if the draw is accepted, false otherwise
    template<typename U>
    bool sample(U &urng,T& draw) const;

    delegate_type function()const{ return fun_; }
    // Maximum number of evalutions
    size_type max_data_count()const{ return max_data_count_; }

    const T& x_min() const { return x_min_; }
    const T& x_max() const { return x_max_; }
    const datas_t& datas() const { return datas_; }

    T max_log()const{ return max_log_; }
    T max_tangent()const{ return max_tangent_; }
    T cum_sum()const{ return cum_sum_; }
    size_type size_data()const{ return size_datas_; }
    T inverse_cum_sums(const T& u) const;

    T offset()const; //> exp(t-offset)<inf

    //TODO (RandomDistribution)
    //os << x;
    //is >> u;
    // min()
    // max()

    std::ostream& description(std::ostream& stream)const;

    protected:
    template <typename U> iter_t inv_cum_sums_impl(T u,T& q);
    void update_cum_sums(iter_t iter) const;
    void insert(const point_t& p,iter_t iter)const;
    // Returns first iter such that iter->cum_sum_>u
    iter_t inv_cum_sums_impl(const T& u,T& draw)const;

    delegate_type fun_;
    T x_min_;
    mutable T t_min_;
    T x_max_;
    bool has_function_,is_initialized_;
    mutable bool do_update_;
    size_type max_data_count_;
    mutable T max_log_;
    mutable T max_tangent_;
    mutable T cum_sum_;
    mutable datas_t datas_;
    // Calling size(...) is not necessarily O(1) so saving it is prefarable
    mutable size_type size_datas_;
    mutable point_t pending_p_;
    mutable diff_t pending_d_; // distance to where p should be inserted
};

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
std::ostream&
operator<<(std::ostream& out, const proposal_sampler<T,Cont,Alloc>& s){
    return s.description(out);
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
std::ostream&
proposal_sampler<T,Cont,Alloc>::description(std::ostream& out) const
{
    typedef data<T> data_t;
    //TODO any other relevant info?
    boost::format f("x_min = %1%, t_min = %2%|");
    f%x_min()%t_min_;
    out << f.str();
    std::copy(
        boost::begin(datas()),
        boost::end(datas()),
        std::ostream_iterator<data_t>(out, "|")
    );
    out << ", mt = " << max_tangent() << ",  offset = " << offset();

    return out;
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
proposal_sampler<T,Cont,Alloc>::proposal_sampler()
:
fun_(),
x_min_(const_::inf_),
t_min_(const_::inf_),
x_max_(const_::inf_),
has_function_(false),
is_initialized_(false),
do_update_(false),
max_data_count_(param_::max_data_count),
max_log_(param_::max_log),
max_tangent_(const_::zero_),
cum_sum_(const_::zero_),
datas_(),
size_datas_(0),
pending_p_(),
pending_d_(0)
{
    datas_.reserve(param_::reserve);
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
proposal_sampler<T,Cont,Alloc>::proposal_sampler(
    const proposal_sampler& that)
:
fun_( that.fun_ ),
x_min_( that.x_min_ ),
t_min_( that.t_min_ ),
x_max_( that.x_max_ ),
has_function_( that.has_function_ ),
is_initialized_( that.is_initialized_ ),
do_update_( that.do_update_ ),
max_data_count_( that.max_data_count_ ),
max_log_( that.max_log_ ),
max_tangent_( that.max_tangent_ ),
cum_sum_( that.cum_sum_ ),
datas_( that.datas_ ),
size_datas_( that.size_datas_ ),
pending_p_( that.pending_p_ ),
pending_d_(that.pending_d_)
{}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
proposal_sampler<T,Cont,Alloc>&
proposal_sampler<T,Cont,Alloc>::operator=(const proposal_sampler& that){
    if(&that!=this){
        fun_    = that.fun_;
        x_min_  = that.x_min_;
        t_min_  = that.t_min_;
        x_max_  = that.x_max_;
        has_function_ = that.has_function_;
        is_initialized_ = that.is_initialized_;
        do_update_ = that.do_update_;
        max_data_count_ = that.max_data_count_;
        max_log_ = that.max_log_;
        max_tangent_ = that.max_tangent_;
        cum_sum_ = that.cum_sum_;
        datas_ = that.datas_;
        size_datas_ = that.size_datas_;
        pending_p_ = that.pending_p_;
        pending_d_ = that.pending_d_;
    }
    return *this;
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
T proposal_sampler<T,Cont,Alloc>::offset()const{
    return max_tangent()-max_log();
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
void proposal_sampler<T,Cont,Alloc>::set_max_log(T v){
    max_log_ = v;
    update_cum_sums(boost::begin(datas_)); //TODO anything else?
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
void proposal_sampler<T,Cont,Alloc>::set_function(
    T x_min,
    T x_max,
    const delegate_type& fun
)
{
    static const char* method = "ars::proposal_sampler::set_function";
    x_min_ = x_min;
    x_max_ = x_max;
    // removing this-> would create a name conflict
    if(!boost::math::isinf( this->x_min() )){
        if(!boost::math::isinf( this->x_max() )){
            if(!( this->x_min() < this->x_max() )){
                throw exception(
                    method,
                    "x_min > x_max",
                    *this
               );
            }
        }
    }
    fun_ = fun;
    has_function_ = true;
    is_initialized_ = false;
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
void proposal_sampler<T,Cont,Alloc>::initialize(T x_0,T x_1){

    static const char* method = "ars::proposal_sampler::initialize(%1%,%2%)";
    typedef std::vector<point_t> vec_t;
    static vec_t vec;

    if(!has_function_){
        boost::format f(method); f%x_0%x_1;
        throw exception(
            f.str(),
            "no func",
            *this
        );
    }
    vec.clear();
    vec.push_back( create_point<T,delegate_type>(x_0,fun_) );
    vec.push_back( create_point<T,delegate_type>(x_1,fun_) );

    return initialize(vec);
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
template<typename R> //Range
void proposal_sampler<T,Cont,Alloc>::initialize(const R& initial_datas){
    static const char* method
        = "ars::proposal_sampler::initialize(const R& initial_datas)";
    {
        typedef typename range_value<R>::type value_t;
        BOOST_MPL_ASSERT(
            (is_base_of<point_t,value_t>)
        );
    }
    if(size(initial_datas)<2){
        throw exception(
            method,
            "size(initial_datas)<2",
            *this
        );
    }

// NB: Gilks also has:
//      IF ((.NOT.lb).AND.(hpx(iwv(1)).LT.eps)) ifault=3
//      IF ((.NOT.ub).AND.(hpx(iwv(2)).GT.-eps)) ifault=4

    if(
        math::isinf(x_min())
    ){
        T dy = boost::begin(initial_datas)->dy();
        if(!
            (
                dy > const_::zero_
            )
        ){
            throw exception(
                method,
                (boost::format("!dy = %1%>0")%dy).str(),
                *this
            );
        }
        t_min_ = const_::quiet_nan_;
    }else{
        t_min_ = tangent(*boost::begin(initial_datas),x_min());
        max_tangent_ = t_min_;
    }

    datas_.resize( size(initial_datas) );
    // Let i,j = 0,...,n-1 iterators for data and initial_data.
    // At i,j = n-1, *i = *j
    // for i,j = n-2,...,0, *i = tangent_intersection(*j,*j+1)
    std::adjacent_difference(
        make_reverse_iterator(boost::end(initial_datas)),
        make_reverse_iterator(boost::begin(initial_datas)),
        make_reverse_iterator(boost::end(datas_)),
        update_tangent_intersection<T>()
    );

    iter_t back_iter = boost::prior(boost::end(datas_));
    {

    	// Bug prior to 01/08/2010 :
		//T t = max_element(
        //    boost::begin(datas_),back_iter,
        //    bind<const T&>( &tang_t::t, _1 )
        //)->t();

    	typedef const T& cref_;
		typedef cref_(tang_t::*fp)() const;
		typedef boost::function<cref_(const data_t&)> fun_;
        fun_ fun =  &tang_t::t;

        T t = *std::max_element(
            boost::make_transform_iterator(boost::begin(datas_),fun),
            boost::make_transform_iterator(back_iter,fun)
        );
        max_tangent_ = (max_tangent()<t)? t : max_tangent();
    }
    const point_t& back_p = static_cast<const point_t&>(*back_iter);
    tang_t& back_ti = static_cast<tang_t&>(*back_iter);

    if(boost::math::isinf(x_max())){
        T dy = back_iter->dy();
        if(
            !(dy < const_::zero_)
        ){
            boost::format f("!dy = %1% <0"); f%dy;
            throw exception(method,f.str(),*this);
        }
        back_ti = tang_t(const_::quiet_nan_,const_::quiet_nan_);
    }else{
        back_ti =  tang_t(x_max(),tangent(back_p,x_max()));
        max_tangent_
            = (max_tangent()<back_ti.t())? back_ti.t() : max_tangent();
    }

    update_cum_sums(datas_.begin());

    // Hupper hull > function, the RHS of which is integrable. If the LHS
    // is not integrable in some segment, a midpoint (or reflection point)
    // is added.
    while( boost::math::isinf( cum_sum() ) ) {
        if(size_data()>max_data_count()){
            throw exception(
                method,
                (boost::format("size_data() = > max_data_count() = %2%")
                    %size_data()%max_data_count()).str(),
                *this
            );
        }
        typedef boost::function<const T&(const data_t&)> fun2_t;

        iter_t iter = std::find_if(
            boost::begin( datas_ ),
            boost::end( datas_ ),
            pred_isinf()
        );

        T new_x;
        if(boost::next(iter)!=boost::end(datas_)){
            new_x = iter->z();
        }else{
            T x_0, x_1;
            x_1 = iter->x();
            if(boost::math::isinf(x_max())){
                x_0 = boost::prior(iter)->x();
                //reflection
                new_x = x_1 + (x_1-x_0); //TODO what if = inf?
            }else{
                T x_2 = x_max();
                new_x = ( x_1 + x_2 )/const_::two_;
                if((new_x <= x_1)|| (new_x >= x_2))
                {
                    throw exception(
                        method,
                        "(new_x <= x_1)|| (new_x >= x_2)",
                        *this
                    );
                }
            }
        }
        point_t p = create_point(new_x,fun_);
        insert(p,iter);
    }
    size_datas_ = size(datas_);
    do_update_ = false;
    is_initialized_ = true;
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
T proposal_sampler<T,Cont,Alloc>::inverse_cum_sums(const T& u) const{
    T q;
    inv_cum_sums_impl(u,q);
    return q;
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
typename proposal_sampler<T,Cont,Alloc>::iter_t
proposal_sampler<T,Cont,Alloc>::inv_cum_sums_impl(
    const T& u, // in [0,cum_sum_)
    T& q
)const
{
    static const char* method = "ars::proposal_sampler::inv_cum_sums_impl";
// What TODO about two consecutives segments with same cum_sum due
// to roundoff? Is there a case for std::upper_bound vs std::lower_bound
// upper_bound :  for every iterator j in [first, i), *j <= value
// lower_bound : for every iterator j in [first, i), *j < value.
    if(u>cum_sum()){
        throw exception(
            method,
            "u>cum_sum()",
            *this
        );
    }
    tang_t value;
    value.cum_sum_ = u;
    iter_t iter =
        std::lower_bound(
            boost::begin(datas_),
            boost::end(datas_),
            value
        );

    //TODO delete ->
    BOOST_ASSERT(iter!=boost::end(datas_));
    BOOST_ASSERT(iter->cum_sum_>u);
    if(iter!=boost::begin(datas_)){
        BOOST_ASSERT(boost::prior(iter)->cum_sum_<u);
    }
    // <-
    bool skip = false;
    T lz, lt, x, y, dy, du;
    dump(*iter,x,y,dy);
    if( iter != boost::begin(datas_) ){
        du = u - boost::prior(iter)->cum_sum_;
        lz = boost::prior(iter)->z();
        lt = boost::prior(iter)->t();
    }else{
        if(math::isinf(x_min())){
            // with y[i] <- y[i] - offset
            // solves int[-inf,q]exp(y[i]+dy[i](x-x[i])) dx = u,
            // <=> y[i]+dy[i](q-x[i]) = log(u * dy[i])
            q = ( log( dy * u )-( y - offset() ) + x * dy ) / dy;
            skip = true;
        }else{
            du = u;
            lz = x_min();
            lt = t_min_;
        }
    }
    if(!skip){
        // with t[i-1] <- t[i-1] - offset
        lt -= offset();
        if(dy < const_::lmin_){
            // solves int[z[i-1],q] exp(t[i-1]) dx = u - area[i-1]
            q = lz + du * exp(-lt);
        }else{
            // solves int[z[i-1],q] exp(t[i-1]+dy[i](x-z[i-1])) dx = du
            // <=> q = z[i-1] + log(1 + du * dy[i] / exp(t[i-1]) ) / dy[i]
            // alt: du * dy[i] = exp( log(du) + log(|dy[i]|) ) * sign(dy[i])
            T a = log(du) + log(fabs(dy)) - lt;
            if(a<const_::lmax_){
                T sign = boost::math::sign(dy);
                a = exp(a);
                q = lz + boost::math::log1p( sign * a ) / dy;
            }else{
                q = lz + a / dy;
            }
        }
    }

    //TODO create a local enum

    if(boost::math::isnan(q)){
        throw exception(method,"isnan(q)",*this);
    }
    if(iter!=boost::begin(datas_)){
        if(q<boost::prior(iter)->z()){
            throw exception(method,"q<prior z",*this);
        }
    }else{
        if(!math::isinf(x_min())){
            if(q < x_min()){
                throw exception(method,"q<x_min",*this);
            }
        }
    }
    if(
        ( iter!=boost::end(datas_))
    ){
        if(q>iter->z()){
            throw exception(method,"q>z",*this);
        }
    }else{
        if(q>iter->z()){
            if( !boost::math::isinf(x_max()) ){
                throw exception(method,"q>z",*this);
            }
        }
    }
    return iter;
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
template <typename U>
bool proposal_sampler<T,Cont,Alloc>::sample(U &urng,T& draw) const
{
    static const char* method = "ars::proposal_sampler::sample";
    if(!is_initialized_){
        throw exception(method,"no init",*this);
    }
    if(do_update_){
        insert(
            pending_p_,
            boost::next(
                boost::begin(datas_),
                pending_d_
            )
        );
        size_datas_ = size(datas_);
    }
    bool adaptive = (size_data()<max_data_count());

    //Sample from the upper hull a.k.a the enveloppe
    T u1, log_u2, upper;
    {
        runif_t runif(const_::zero_, cum_sum());
        u1 = runif(urng);
    }
    iter_t iter = inv_cum_sums_impl(u1,draw);
    upper = tangent(*iter,draw);
    {
        runif_t unif(const_::zero_, const_::one_);
        log_u2 = log(unif(urng));
    }
    // Squeezing test
    bool accept = false;
    if(adaptive){
        T lower;
        // lower is -inf outside (x[1],x[n])
        if(
            ( (iter!=boost::begin(datas_)) || (draw>(iter->x())))
            && ( (iter!=boost::prior(boost::end(datas_))) || (draw<(iter->x())))
        ){
            if(draw<=(iter->x())){
                lower = linearly_interpolate(*boost::prior(iter),*iter,draw);
            }else{
                lower = linearly_interpolate(*iter,*boost::next(iter),draw);
            }
            accept = ( log_u2 <= (lower-upper) );
        }else{
            // lower is -inf outside (x[1],x[n])
        }
    }
    // Rejection test
    if(!accept){
        pending_p_ = create_point(draw,fun_);
        pending_d_ = std::distance(boost::begin(datas_),iter);
        if(draw>iter->x()){
            ++pending_d_;
        }
        accept =  ( log_u2 <= (pending_p_.y() - upper) );
        do_update_ = adaptive;
    }
    return accept;
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
void proposal_sampler<T,Cont,Alloc>::update_cum_sums(iter_t iter) const
{
    static const char* method = "ars::proposal_sampler::update_cum_sums";
    //    x1  x2  x3        point abscissae
    //   / \ / \ / \
    //  z0  z1  z2  z3      tangent abscissae
    //
    // z0 = x_min
    // zn = x_max
    //
    // Here we compute for each i>=j,
    // the area under the tangent at x[i] over (z[i-1],z[i])
    // If z0 = -inf, that's exp(t[1]-offset)/dy[1]
    // It zn = inf, that's -exp(t[n-1]-offset)/dy[1]

    //bool is_begin = false;
    T area;
    tang_t a;
    if(iter != boost::begin(datas_)) {
        a = *boost::prior(iter);
        cum_sum_ = boost::prior(iter)->cum_sum_;
    }else{
        cum_sum_ = const_::zero_;
        if(
            boost::math::isinf(x_min())
        ){
            T dy = iter->dy();
            if(dy<const_::lmin_){
                throw; //TODO
            }else{
                area = area_left_tail(*iter,offset());
            }
        }else{
            a = tang_t(x_min(),t_min_);
            area = area_segment_safeguarded(a,*iter,offset());
        }
        if(area<const_::zero_){
            boost::format f("area[%1%] = %2% <0");
            f%std::distance(boost::begin(datas_),iter)%area;
            throw exception(method,f.str(),*this);
        }
        cum_sum_ += area;
        iter->cum_sum_ = cum_sum_;
        a = *iter;
        ++iter;
    }

    iter_t j = boost::prior(boost::end(datas_));
    // TODO adjacent_difference?
    while(iter!=j){
        T area = area_segment_safeguarded(a,*iter,offset());
        if(area<const_::zero_){
            boost::format f("area[%1%] = %2% < 0");
            f%std::distance(boost::begin(datas_),iter)%area;
            throw exception(method,f.str(),*this);
        }
        cum_sum_ += area;
        iter->cum_sum_ = cum_sum_;
        a = *iter;
        ++iter;
    }

    if(
        math::isinf(x_max())
    ){
        area = area_right_tail(a,*iter,offset());
    }else{
        area = area_segment_safeguarded(a,*iter,offset());
    }
    if(area<const_::zero_){
        boost::format f("area[n] = %1%<0");
        f%area;
        throw exception(method,f.str(),*this);
    }
    cum_sum_ += area;
    iter->cum_sum_ = cum_sum_;
    ++iter;
    BOOST_ASSERT(!(iter!=boost::end(datas_)));

    {
    	// Replaces version below to accomodate MSVC
		//http://stackoverflow.com/questions/262000/c-best-algorithm-to-check-if-a-vector-is-sorted
    	using namespace boost::lambda;
    	BOOST_ASSERT(
			std::adjacent_find(
        		boost::begin(datas_),
        		boost::end(datas_),
                boost::lambda::bind(
                	std::less<tang_t>(),
            		_2,
                    _1
                )
    		) == boost::end(datas_)
    	);
    }

	// Previously:
    //BOOST_ASSERT(
    //    is_sorted(
    //        begin(datas_),
    //        boost::end(datas_),
    //        bind<bool>(
    //            std::less<tang_t>(),
    //            _1,
    //            _2
    //        )
    //    )
    //);
}

template<
    typename T,
    template<typename,typename> class Cont,
    template<typename> class Alloc
>
void proposal_sampler<T,Cont,Alloc>::insert(
    const point_t& p,
    iter_t iter
)const
{
    static const char* method = "ars::proposal_sampler::insert";

    if(iter!=boost::end(datas_)){
        if(!std::less<point_t>()(p,*iter)){
            throw exception(method,"p>next",*this);
        }
    }else{
        if(!math::isinf(x_max())){
            if(!(p.x()<x_max())){
                throw exception(method,"p>max",*this);
            }
        }
    }

    if(iter!=boost::begin(datas_)){
        if(!std::less<point_t>()(*boost::prior(iter),p)){
            throw exception(method,"p<prior",*this);
        }
    }else{
        if(!math::isinf(x_min())){
            if(!(p.x()>x_min())){
                throw exception(method,"p<x_min",*this);
            }
        }
    }

    iter = datas_.insert(iter,p);
    T m = math::tools::min_value<T>();

    struct local{
        static T impl(
            data_t& a,
            const data_t& b,
            const proposal_sampler& s
        ){
            static const char* method = "ars::proposal_sampler::insert";
            const point_t& a_p = a;
            const point_t& b_p = b;
            if(!(a_p<b_p)){
                throw exception(method,"a>b",s);
            }
            if(!is_non_increasing_dy(a_p,b_p)){
                throw exception(method,"dy non increasing",s);
            }
            if(!is_concave(a,b)){
                throw exception(method,"not concave",s);
            }
            {
                tang_t& a_ti = a;
                a_ti = tang_t(a,b);
            }
            if(a.z()<a.x()){
                throw exception(method,"z<x",s);
            }
            if(a.z()>b.x()){
                throw exception(method,"z>next x",s);
            }
            return a.t();
        }
    };

    if(iter!=boost::begin(datas_)){
        T t = local::impl(*boost::prior(iter),*iter,*this);
        m = (m<t)? t : m;
    }else{
        if(!boost::math::isinf(x_min())){
            t_min_ = tangent(*iter,x_min());
        }
    }
    if(boost::next(iter)!=boost::end(datas_)){
        T t = local::impl(*iter,*boost::next(iter),*this);
        m = (m<t)? t : m;
    }else{
        const point_t& p = (*iter);
        tang_t& ti = (*iter);
        if(boost::math::isinf(x_max())){
            ti = tang_t(const_::quiet_nan_, const_::quiet_nan_);
        }else{
            ti = tang_t(x_max(),tangent(p,x_max()));
            T t = ti.t();
            m = (m<t)? t : m;
        }
    }

    if(max_tangent()<m){
        max_tangent_ = m;
        iter = boost::begin(datas_);
    }else{
        if(iter!=boost::begin(datas_)){
            iter = boost::prior(iter);
        }
    }

    {
    	// Replaces version below to accomodate MSVC
		//http://stackoverflow.com/questions/262000/c-best-algorithm-to-check-if-a-vector-is-sorted
    	using namespace boost::lambda;
    	BOOST_ASSERT(
			std::adjacent_find(
        		boost::begin(datas_),
        		boost::end(datas_),
                boost::lambda::bind(
                	std::less<point_t>(),
            		_2,
                    _1
                )
    		) == boost::end(datas_)
    	);
    }

	// Previously:
    // BOOST_ASSERT(
    //    is_sorted(
    //        boost::begin(datas_),
    //        boost::end(datas_),
    //        boost::bind<bool>(
    //            std::less<point_t>(),
    //            _1,
    //            _2
    //        )
    //    )
    //);

    update_cum_sums(iter);
}

}// ars
}// detail
}// statistics
}// boost

#endif


//
// Created by yalavrinenko on 09.06.2021.
//
#include "greedy_engine.hpp"
#include <unordered_set>
#include <ranges>
#include <boost/format.hpp>

trs::solution trs::greedy_engine::solve(double target_distance, const std::unordered_map<size_t, check_point> &checks,
                                        size_t start, distance_function distance) const {
  solution sol{.distance = 0};
  sol.trace.emplace_back(start);

  std::unordered_set<size_t> visited{start};

  auto cutoff = 500.0;
  auto current = checks.at(start);

  auto nearest_filter = [&cutoff, &current](auto const& kv) {
    auto sqr = [](auto x) { return x * x; };
    return cutoff * cutoff > sqr(current.position.first - kv.second.position.first) + sqr(current.position.second - kv.second.position.second);
  };

  auto is_visited_filter = [&visited](auto const& kv) { return !visited.contains(kv.first); };

  auto start_distance_filter = [&distance, start, &sol, &target_distance](auto const &kv){
    return distance(kv.first, start) + sol.distance <= target_distance;
  };

  auto evaluate_distance = [&distance, &current, start](auto const& kv){
    auto d = distance(current.uid, kv.first);
    auto value = kv.first;
    while (value > 10)
      value /= 10;

    return std::pair{kv.first, static_cast<double>(value) / d};
  };

  do {
    auto nearest = checks | std::views::filter(is_visited_filter) | std::views::filter(nearest_filter) | std::views::filter(start_distance_filter);
    auto scores = nearest | std::views::transform(evaluate_distance) | std::views::common;

    if (!std::ranges::empty(scores)){
      auto max_score = std::ranges::max_element(scores, {}, &std::pair<size_t, double>::second);

      auto const &max_uid = (*max_score).first;
      visited.insert(max_uid);
      sol.trace.push_back(max_uid);
      sol.distance += distance(current.uid, max_uid);

      current = checks.at(max_uid);
      std::cout << (boost::format("\t\t*********Add check point %1% to solution distance left %2%") % current.uid %
                    (target_distance - sol.distance))
                << std::endl;
    } else {
      sol.trace.push_back(start);
      break;
    }
  } while (sol.distance < target_distance);

  return sol;
}


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

#include <boost/hana/orderable/less_mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/orderable.hpp>
#include <boost/hana/logical/logical.hpp>
using namespace boost::hana;

template <typename mcd>
void test() {
    constexpr auto orderable = detail::minimal::orderable<mcd>;

    BOOST_HANA_CONSTEXPR_ASSERT(less_equal(orderable(0), orderable(1)));
    BOOST_HANA_CONSTEXPR_ASSERT(less_equal(orderable(0), orderable(0)));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less_equal(orderable(1), orderable(0))));

    // operators
    BOOST_HANA_CONSTEXPR_ASSERT(orderable(0) <= orderable(1));
    BOOST_HANA_CONSTEXPR_ASSERT(orderable(0) <= orderable(0));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(orderable(1) <= orderable(0)));
}

int main() {
    test<Orderable::less_mcd>();
}