/*
 * cuConebeamProjectionOperator.cpp
 *
 *  Created on: Feb 18, 2015
 *      Author: u051747
 */

#include "cuConebeamProjectionOperator.h"
#include "conebeam_projection.h"
#include <boost/math/constants/constants.hpp>
#include "hoNDArray_math.h"
using boost::math::constants::pi;
namespace Gadgetron {

cuConebeamProjectionOperator::cuConebeamProjectionOperator() {
	// TODO Auto-generated constructor stub

	samples_per_pixel_ = 1.5f;
	allow_offset_correction_override_=true;

}
void cuConebeamProjectionOperator
::offset_correct(cuNDArray<float>* projections){

	if( !preprocessed_ ){
		throw std::runtime_error( "Error: cuConebeamProjectionOperator::offset_correct: setup not performed");
	}
	float SDD = acquisition_->get_geometry()->get_SDD();
	float SAD = acquisition_->get_geometry()->get_SAD();
	floatd2 ps_dims_in_mm = acquisition_->get_geometry()->get_FOV();
	apply_offset_correct( projections,acquisition_->get_geometry()->get_offsets(),ps_dims_in_mm, SDD, SAD);
}

cuConebeamProjectionOperator::~cuConebeamProjectionOperator() {
	// TODO Auto-generated destructor stub
}

void cuConebeamProjectionOperator::mult_M(cuNDArray<float>* input,
		cuNDArray<float>* output, bool accumulate) {

	auto dims = *input->get_dimensions();
	std::vector<size_t> dims3d(dims);

	if (dims3d.size() ==4) dims3d.pop_back();
	auto outdims = *output->get_dimensions();
	std::vector<size_t> outbindims(outdims);
	float* input_ptr = input->get_data_ptr();
	float* output_ptr = output->get_data_ptr();
	for (int bin = 0; bin < binning_->get_number_of_bins(); bin++){
		//Check for empty bins
		if (binning_->get_bin(bin).size() == 0)
			continue;
		cuNDArray<float> input_view(dims3d,input_ptr);
		outbindims.back() = angles[bin].size();
		auto  output_view = boost::make_shared<cuNDArray<float>>(outbindims,output_ptr);
		auto output_view2 = output_view;
		/*
		if (use_offset_correction_ && accumulate){
			output_view2 = boost::make_shared<cuNDArray<float>>(outbindims);
			clear(output_view2.get());
		}
		 */

		conebeam_forwards_projection(output_view2.get(),&input_view,angles[bin],offsets[bin],samples_per_pixel_,is_dims_in_mm_,acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD(),accumulate);

		/*
		if (use_offset_correction_){
			apply_offset_correct(output_view2.get(),offsets[bin],acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD());
			if (accumulate)
				*output_view += *output_view2;
		}
		 */

		input_ptr += input_view.get_number_of_elements();
		output_ptr += output_view->get_number_of_elements();

	}

}

void cuConebeamProjectionOperator::mult_MH(cuNDArray<float>* input,
		cuNDArray<float>* output, bool accumulate) {
	auto dims = *output->get_dimensions();
	std::vector<size_t> dims3d = dims;
	if (dims3d.size() ==4) dims3d.pop_back();

	auto indims = *input->get_dimensions();
	std::vector<size_t> inbindims(indims);
	float* input_ptr = input->get_data_ptr();
	float* output_ptr = output->get_data_ptr();
	for (int bin = 0; bin < binning_->get_number_of_bins(); bin++){
		//Check for empty bins
		if (binning_->get_bin(bin).size() == 0)
			continue;

		cuNDArray<float> output_view(dims3d,output_ptr);
		inbindims.back() = angles[bin].size();
		auto input_view = boost::make_shared<cuNDArray<float>>(inbindims,input_ptr);
		auto input_view2 = input_view;
		if(use_offset_correction_)
			input_view2 = boost::make_shared<cuNDArray<float>>(*input_view);

		vector_td<int,3> is_dims_in_pixels{dims3d[0], dims3d[1],dims3d[2]};

		conebeam_backwards_projection(input_view2.get(),&output_view, angles[bin],offsets[bin],is_dims_in_pixels, is_dims_in_mm_,acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD(),use_offset_correction_,accumulate);

		input_ptr += input_view->get_number_of_elements();
		output_ptr += output_view.get_number_of_elements();

	}

	if (mask)
		apply_mask(output,mask.get());

}

boost::shared_ptr<cuNDArray<bool>> cuConebeamProjectionOperator::calculate_mask( cuNDArray<float>* projections,float limit)
{
	auto dims = *this->get_domain_dimensions();
	std::vector<size_t> dims3d(dims.begin(),dims.end()-1);
	auto indims = *projections->get_dimensions();
	std::vector<size_t> inbindims(indims);
	float* input_ptr = projections->get_data_ptr();
	auto mask = boost::make_shared<cuNDArray<bool>>(dims);
	bool* mask_ptr = mask->get_data_ptr();
	for (int bin = 0; bin < binning_->get_number_of_bins(); bin++){
		//Check for empty bins
		if (binning_->get_bin(bin).size() == 0)
			continue;

		cuNDArray<bool> mask_view(dims3d,mask_ptr);
		inbindims.back() = angles[bin].size();
		auto input_view = boost::make_shared<cuNDArray<float>>(inbindims,input_ptr);

		vector_td<int,3> is_dims_in_pixels{dims3d[0], dims3d[1],dims3d[2]};

		conebeam_spacecarver(input_view.get(),&mask_view, angles[bin],offsets[bin],is_dims_in_pixels, is_dims_in_mm_,acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD(),limit);

		input_ptr += input_view->get_number_of_elements();
		mask_ptr += mask_view.get_number_of_elements();

	}

	return mask;

}
void Gadgetron::cuConebeamProjectionOperator::setup( boost::shared_ptr<CBCT_acquisition> acquisition,
		floatd3 is_dims_in_mm, bool transform_angles )
{
	acquisition_ = acquisition;
	is_dims_in_mm_ = is_dims_in_mm;

	// Determine the minimum and maximum angles scanned and transform array angles from [0;max_angle_].
	//

	std::vector<float> &all_angles = acquisition->get_geometry()->get_angles();
	if (transform_angles){
		float min_value = *std::min_element(all_angles.begin(), all_angles.end() );
		transform(all_angles.begin(), all_angles.end(), all_angles.begin(), bind2nd(std::minus<float>(), min_value));
	}
	// Are we in a short scan setup?
	// - we say yes if we have covered less than PI+3*delta radians
	//

	float angle_span = *std::max_element(all_angles.begin(), all_angles.end() );
	floatd2 ps_dims_in_mm = acquisition_->get_geometry()->get_FOV();
	float SDD = acquisition_->get_geometry()->get_SDD();
	float delta = std::atan(ps_dims_in_mm[0]/(2.0f*SDD)); // Fan angle

	if( angle_span*pi<float>()/180.0f > pi<float>()+3.0f*delta )
		short_scan_ = false;
	else
		short_scan_ = true;

	std::vector<floatd2> all_offsets = acquisition_->get_geometry()->get_offsets();
	floatd2 mean_offset = std::accumulate(all_offsets.begin(),all_offsets.end(),floatd2(0,0))/float(all_offsets.size());
	if( allow_offset_correction_override_ && std::abs(mean_offset[0]) > ps_dims_in_mm[0]*0.1f )
		use_offset_correction_ = true;

	std::cout << "Mean offset " << mean_offset << " Use offset correct: " << (use_offset_correction_ ? "true" : "false") << std::endl;
	preprocessed_ = true;
	if (!binning_){
		std::vector<unsigned int> bins(angles.size());
		std::iota(bins.begin(),bins.end(),0);
		binning_ = boost::make_shared<CBCT_binning>(std::vector<std::vector<unsigned int>>(1,bins));
	}

	angles = std::vector<std::vector<float>>();
	offsets = std::vector<std::vector<floatd2>>();
	for (int bin =0; bin < binning_->get_number_of_bins(); bin++){
		auto binvec = binning_->get_bin(bin);
		angles.push_back(std::vector<float>());
		offsets.push_back(std::vector<floatd2>());
		for (auto index : binvec){
			angles.back().push_back(all_angles[index]);
			offsets.back().push_back(all_offsets[index]);
		}

	}
	auto permutations = new_order(binning_->get_bins());
	auto proj = acquisition->get_projections();
	if (proj)
		*proj =	*permute_projections(proj,permutations);

}

void Gadgetron::cuConebeamProjectionOperator::setup( boost::shared_ptr<CBCT_acquisition> acquisition,
		boost::shared_ptr<CBCT_binning> binning,floatd3 is_dims_in_mm, 		bool transform_angles)

{

	binning_ = binning;
	setup( acquisition, is_dims_in_mm,transform_angles );
}

std::vector<unsigned int> Gadgetron::cuConebeamProjectionOperator::new_order(std::vector<std::vector<unsigned int>> bins){
	std::vector<unsigned int> result;
	for (auto & b : bins)
		for (auto p : b)
			result.push_back(p);
	return result;
}

boost::shared_ptr<hoCuNDArray<float> > Gadgetron::cuConebeamProjectionOperator::permute_projections(
		boost::shared_ptr<hoCuNDArray<float> > projections,
		std::vector<unsigned int>  & permutations) {

	std::vector<size_t> new_proj_dims = {projections->get_size(0),projections->get_size(1),permutations.size()};

	auto result = boost::make_shared<hoCuNDArray<float>>(new_proj_dims);

	size_t nproj = permutations.size();
	size_t proj_size = projections->get_size(0)*projections->get_size(1);

	float * res_ptr = result->get_data_ptr();
	float * proj_ptr = projections->get_data_ptr();

	for (unsigned int i = 0; i < nproj; i++){
		cudaMemcpy(res_ptr+i*proj_size,proj_ptr+proj_size*permutations[i],proj_size*sizeof(float),cudaMemcpyHostToHost);
	}
	return result;

}

} /* namespace Gadgetron */


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_DETAILS_CUM_HELPER_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_DETAILS_CUM_HELPER_HPP_INCLUDED

#include <boost/simd/include/functions/simd/shuffle.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/mpl/int.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace details
{
  template<std::size_t N,  std::size_t PASS = 1>
  struct cum_helper
  {
    template<typename I, typename C>
    struct apply : boost::mpl::int_<((I::value == 0) ? 0 : I::value-1) >{};

  };
  template<std::size_t N>
  struct cum_helper<N, 2>
  {
    template<typename I, typename C>
    struct apply : boost::mpl::int_<I::value == 0 ? 0:(I::value < 7 ? ((I::value-1)/3 ? 4:1):(I::value-2))>{};

  };
  template<std::size_t N>
  struct cum_helper<N, 4>
  {
    template<typename I, typename C>
    struct apply : boost::mpl::int_<I::value < 4 ? I::value : (I::value < 7 ? 3 :(I::value-4))>{};

  };
  template<std::size_t N>
  struct cum_helper<N, 8>
  {
    template<typename I, typename C>
    struct apply : boost::mpl::int_<I::value < 7 ? I::value : 7>{};

  };
} } }

#endif


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "laplace_3d_hypersingular_boundary_operator.hpp"

#include "blas_quadrature_helper.hpp"
#include "context.hpp"
#include "general_elementary_local_operator_imp.hpp"
#include "general_hypersingular_integral_operator_imp.hpp"
#include "laplace_3d_single_layer_boundary_operator.hpp"
#include "synthetic_integral_operator.hpp"

#include "../common/boost_make_shared_fwd.hpp"

#include "../fiber/explicit_instantiation.hpp"

#include "../fiber/laplace_3d_single_layer_potential_kernel_functor.hpp"
#include "../fiber/laplace_3d_hypersingular_off_diagonal_kernel_functor.hpp"
#include "../fiber/surface_curl_3d_functor.hpp"
#include "../fiber/scalar_function_value_functor.hpp"
#include "../fiber/simple_test_scalar_kernel_trial_integrand_functor.hpp"
#include "../fiber/single_component_test_trial_integrand_functor.hpp"

#include "../fiber/default_collection_of_kernels.hpp"
#include "../fiber/default_collection_of_basis_transformations.hpp"
#include "../fiber/default_test_kernel_trial_integral.hpp"

#include "general_elementary_singular_integral_operator_imp.hpp"
#include "../fiber/typical_test_scalar_kernel_trial_integral.hpp"

#include <boost/type_traits/is_complex.hpp>

namespace Bempp {

template <typename BasisFunctionType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
laplace3dSyntheticHypersingularBoundaryOperator(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    std::string label, int internalSymmetry,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp) {
  typedef typename ScalarTraits<BasisFunctionType>::RealType CoordinateType;

  typedef Fiber::ScalarFunctionValueFunctor<CoordinateType> ValueFunctor;
  typedef Fiber::SurfaceCurl3dFunctor<CoordinateType> CurlFunctor;
  typedef Fiber::SingleComponentTestTrialIntegrandFunctor<
      BasisFunctionType, ResultType> IntegrandFunctor;

  typedef GeneralElementaryLocalOperator<BasisFunctionType, ResultType> LocalOp;
  typedef SyntheticIntegralOperator<BasisFunctionType, ResultType> SyntheticOp;

  if (!domain || !range || !dualToRange)
    throw std::invalid_argument(
        "laplace3dSyntheticHypersingularBoundaryOperator(): "
        "domain, range and dualToRange must not be null");

  shared_ptr<const Space<BasisFunctionType>> newDomain = domain;
  shared_ptr<const Space<BasisFunctionType>> newDualToRange = dualToRange;

  bool isBarycentric =
      (domain->isBarycentric() || dualToRange->isBarycentric());

  if (isBarycentric) {
    newDomain = domain->barycentricSpace(domain);
    newDualToRange = dualToRange->barycentricSpace(dualToRange);
  }

  shared_ptr<const Context<BasisFunctionType, ResultType>> internalContext,
      auxContext;
  SyntheticOp::getContextsForInternalAndAuxiliaryOperators(
      context, internalContext, auxContext);
  shared_ptr<const Space<BasisFunctionType>> internalTrialSpace =
      newDomain->discontinuousSpace(newDomain);
  shared_ptr<const Space<BasisFunctionType>> internalTestSpace =
      newDualToRange->discontinuousSpace(newDualToRange);

  // Note: we don't really need to care about ranges and duals to domains of
  // the internal operator. The only range space that matters is that of the
  // leftmost operator in the product.

  const char xyz[] = "xyz";
  const size_t dimWorld = 3;

  if (label.empty())
    label =
        AbstractBoundaryOperator<BasisFunctionType, ResultType>::uniqueLabel();

  BoundaryOperator<BasisFunctionType, ResultType> slp;

  if (!externalSlp.isInitialized()) {

    slp = laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
        internalContext, internalTrialSpace,
        internalTestSpace /* or whatever */, internalTestSpace,
        "(" + label + ")_internal_SLP", internalSymmetry);

  } else {

    slp = externalSlp;
  }

  std::vector<BoundaryOperator<BasisFunctionType, ResultType>>
  trialCurlComponents;
  std::vector<BoundaryOperator<BasisFunctionType, ResultType>>
  testCurlComponents;
  testCurlComponents.resize(3);
  for (size_t i = 0; i < dimWorld; ++i)
    testCurlComponents[i] = BoundaryOperator<BasisFunctionType, ResultType>(
        auxContext, boost::make_shared<LocalOp>(
                        internalTestSpace, range, newDualToRange,
                        ("(" + label + ")_test_curl_") + xyz[i], NO_SYMMETRY,
                        CurlFunctor(), ValueFunctor(), IntegrandFunctor(i, 0)));
  int syntheseSymmetry = 0; // symmetry of the decomposition
  if (newDomain == newDualToRange && internalTrialSpace == internalTestSpace)
    syntheseSymmetry =
        HERMITIAN | (boost::is_complex<BasisFunctionType>() ? 0 : SYMMETRIC);
  else {
    trialCurlComponents.resize(3);
    for (size_t i = 0; i < dimWorld; ++i)
      trialCurlComponents[i] = BoundaryOperator<BasisFunctionType, ResultType>(
          auxContext,
          boost::make_shared<LocalOp>(
              newDomain, internalTrialSpace /* or whatever */,
              internalTrialSpace, ("(" + label + ")_trial_curl_") + xyz[i],
              NO_SYMMETRY, ValueFunctor(), CurlFunctor(),
              IntegrandFunctor(0, i)));
  }

  return BoundaryOperator<BasisFunctionType, ResultType>(
      context, boost::make_shared<SyntheticOp>(testCurlComponents, slp,
                                               trialCurlComponents, label,
                                               syntheseSymmetry));
}

template <typename BasisFunctionType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
laplace3dHypersingularBoundaryOperator(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    const std::string &label, int symmetry,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp) {
  const AssemblyOptions &assemblyOptions = context->assemblyOptions();
  if ((assemblyOptions.assemblyMode() == AssemblyOptions::ACA &&
       assemblyOptions.acaOptions().mode == AcaOptions::LOCAL_ASSEMBLY) ||
      (externalSlp.isInitialized()))
    return laplace3dSyntheticHypersingularBoundaryOperator(
        context, domain, range, dualToRange, label, symmetry, externalSlp);

  typedef typename ScalarTraits<BasisFunctionType>::RealType KernelType;
  typedef typename ScalarTraits<BasisFunctionType>::RealType CoordinateType;

  typedef Fiber::Laplace3dSingleLayerPotentialKernelFunctor<KernelType>
  KernelFunctor;
  typedef Fiber::SurfaceCurl3dFunctor<CoordinateType> TransformationFunctor;
  typedef Fiber::SimpleTestScalarKernelTrialIntegrandFunctorExt<
      BasisFunctionType, KernelType, ResultType, 3> IntegrandFunctor;

  typedef Fiber::Laplace3dHypersingularOffDiagonalKernelFunctor<KernelType>
  OffDiagonalKernelFunctor;
  typedef Fiber::ScalarFunctionValueFunctor<CoordinateType>
  OffDiagonalTransformationFunctor;
  typedef Fiber::SimpleTestScalarKernelTrialIntegrandFunctorExt<
      BasisFunctionType, KernelType, ResultType, 1> OffDiagonalIntegrandFunctor;

  typedef GeneralHypersingularIntegralOperator<BasisFunctionType, KernelType,
                                               ResultType> Op;

  shared_ptr<Fiber::TestKernelTrialIntegral<BasisFunctionType, KernelType,
                                            ResultType>> integral,
      offDiagonalIntegral;
  if (shouldUseBlasInQuadrature(assemblyOptions, *domain, *dualToRange)) {
    integral.reset(new Fiber::TypicalTestScalarKernelTrialIntegral<
        BasisFunctionType, KernelType, ResultType>());
    offDiagonalIntegral = integral;
  } else {
    integral.reset(new Fiber::DefaultTestKernelTrialIntegral<IntegrandFunctor>(
        IntegrandFunctor()));
    offDiagonalIntegral.reset(
        new Fiber::DefaultTestKernelTrialIntegral<OffDiagonalIntegrandFunctor>(
            OffDiagonalIntegrandFunctor()));
  }

  shared_ptr<Op> newOp(
      new Op(domain, range, dualToRange, label, symmetry, KernelFunctor(),
             TransformationFunctor(), TransformationFunctor(), integral,
             OffDiagonalKernelFunctor(), OffDiagonalTransformationFunctor(),
             OffDiagonalTransformationFunctor(), offDiagonalIntegral));

  return BoundaryOperator<BasisFunctionType, ResultType>(context, newOp);
}

template <typename BasisFunctionType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
laplace3dHypersingularBoundaryOperator(
    const ParameterList& parameterList,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    const std::string &label, int symmetry,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp){

  shared_ptr<const Context<BasisFunctionType, ResultType>> context(
      new Context<BasisFunctionType, ResultType>(parameterList));
  return laplace3dHypersingularBoundaryOperator(context, domain, range,
                                              dualToRange, label, symmetry,
                                              externalSlp);

}

#define INSTANTIATE_NONMEMBER_CONSTRUCTOR(BASIS, RESULT)                       \
  template BoundaryOperator<BASIS, RESULT>                                     \
  laplace3dHypersingularBoundaryOperator(                                      \
      const ParameterList&,                                                    \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &, int,        \
      const BoundaryOperator<BASIS, RESULT> &);                                \
  template BoundaryOperator<BASIS, RESULT>                                     \
  laplace3dHypersingularBoundaryOperator(                                      \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &, int,        \
      const BoundaryOperator<BASIS, RESULT> &)
FIBER_ITERATE_OVER_BASIS_AND_RESULT_TYPES(INSTANTIATE_NONMEMBER_CONSTRUCTOR);

} // namespace Bempp


#include "pcraster_multicore/python/local/maximum.h"

#include <boost/python.hpp>

// PCRaster
#include "calc_spatial.h"
#include "calc_nonspatial.h"

// Field wrapper
#include "pcraster_multicore/wrapper/datatype_customization_points/multicore_spatial.h"
#include "pcraster_multicore/wrapper/datatype_traits/multicore_spatial.h"
#include "pcraster_multicore/wrapper/argument_traits/multicore_spatial.h"

#include "pcraster_multicore/wrapper/datatype_customization_points/multicore_nonspatial.h"
#include "pcraster_multicore/wrapper/datatype_traits/multicore_nonspatial.h"
#include "pcraster_multicore/wrapper/argument_traits/multicore_nonspatial.h"

#include "pcraster_multicore/python/execution_policy.h"
#include "pcraster_multicore/python/local/utils.h"
#include "pcraster_multicore/python/type_conversion/scalar.h"

// Fern
#include "fern/algorithm/policy/policies.h"
#include "fern/algorithm/statistic/binary_max.h"

namespace fa = fern::algorithm;

namespace pcraster_multicore {
namespace python {
namespace detail {

template<class T>
void maximum_nonspatial(
         fa::ExecutionPolicy& epol,
         multicore_field::Spatial<T>& res,
         multicore_field::Nonspatial<T> const& arg2) {

  using InputNoDataPolicy = fa::InputNoDataPolicies<SpatialDetectNoData<T>,
        NonspatialDetectNoData<T>>;
  InputNoDataPolicy input_no_data_policy{{res}, {arg2}};

  SpatialSetNoData<T> output_no_data_policy(res);

  fa::statistic::binary_max(input_no_data_policy,
    output_no_data_policy, epol, res, arg2, res);
}

template<class T>
void maximum_spatial(
         fa::ExecutionPolicy& epol,
         multicore_field::Spatial<T>& res,
         multicore_field::Spatial<T> const& arg2){

  using InputNoDataPolicy = fa::InputNoDataPolicies<SpatialDetectNoData<T>,
        SpatialDetectNoData<T>>;

  InputNoDataPolicy input_no_data_policy{{res}, {arg2}};

  SpatialSetNoData<T> output_no_data_policy(res);

  fa::statistic::binary_max(input_no_data_policy,
    output_no_data_policy, epol, res, arg2, res);
}

template<class T>
calc::Field* maximum(std::vector<calc::Field*> const&  field_arguments){

  calc::Field* res_field = (field_arguments.at(0))->createClone();

  multicore_field::Spatial<T> res(res_field);

  fa::ExecutionPolicy epol = execution_policy();

  for(size_t idx = 1; idx < field_arguments.size(); ++idx){
    // we assume that the first field is always spatial
    // i.e. spatial argument, or spatial result of previous maximum
    if(field_arguments.at(idx)->isSpatial() == true){
      const multicore_field::Spatial<T> arg2(field_arguments.at(idx));
      maximum_spatial<T>(epol, res, arg2);
    }
    else{
      const multicore_field::Nonspatial<T> arg2(field_arguments.at(idx));
      maximum_nonspatial<T>(epol, res, arg2);

    }
  }
  return res.getField();
}

} // namespace detail

calc::Field* maximum(boost::python::list const& arguments){

  size_t nr_args = boost::python::len(arguments);

  if(nr_args == 0){
    throw std::runtime_error("at least 1 argument required, 0 given\n");
  }

  if(nr_args == 1){
    return (static_cast<calc::Field*>(boost::python::extract<calc::Field*>(arguments[0])))->createClone();
  }

  std::vector<calc::Field *> field_arguments;

  // Casting of POD to a PCRaster data type
  // To determine the intended datatype, we check the spatial arguments
  // as POD are just mapped to nominal or scalars in the Python wrapper.
  // Depending on the spatial data types, we cast int where appropriate
  bool is_ordinal = false;
  bool is_scalar = false;
  bool all_nonspatial = true;

  for(size_t idx = 0; idx < nr_args; ++idx){
    field_arguments.push_back(boost::python::extract<calc::Field*>(arguments[idx]));
    // arguments must have same extent as clone
    assert_equal_location_attributes(*field_arguments.at(idx));

    // Just test the spatials
    if(field_arguments.at(idx)->isSpatial() == true){
      if(ordinal_valuescale(*field_arguments.at(idx))){
        is_ordinal = true;
        all_nonspatial = false;
      }
      else if(scalar_valuescale(*field_arguments.at(idx))) {
        is_scalar = true;
        all_nonspatial = false;
      }

      if(directional_valuescale(*field_arguments.at(idx))){
        throw std::runtime_error("argument " + std::to_string(idx + 1) + " is of type 'directional'; allowed type is 'scalar' or 'ordinal'");
      }
      if(ldd_valuescale(*field_arguments.at(idx))){
        throw std::runtime_error("argument " + std::to_string(idx + 1) + " is of type 'ldd'; allowed type is 'scalar' or 'ordinal'");
      }
      if(nominal_valuescale(*field_arguments.at(idx))){
        throw std::runtime_error("argument " + std::to_string(idx + 1) + " is of type 'nominal'; allowed type is 'scalar' or 'ordinal'");
      }
      if(boolean_valuescale(*field_arguments.at(idx))){
        throw std::runtime_error("argument " + std::to_string(idx + 1) + " is of type 'boolean'; allowed type is 'scalar' or 'ordinal'");
      }
    }
  }

  // In case someone runs into this: modify the wrapper and forward this to the PCRaster max
  if(all_nonspatial == true){
    throw std::runtime_error("non-spatial result type is not implemented; use at least one spatial argument");
  }

  if(is_ordinal == true){
    for(size_t idx = 0; idx < nr_args; ++idx){
      // Data type mixing is not allowed
      if(scalar_valuescale(*field_arguments.at(idx))){
        throw std::runtime_error("argument " + std::to_string(idx + 1) + " is of type 'scalar'; allowed type is 'ordinal'");
       }
    }
    return detail::maximum<INT4>(field_arguments);
  }
  else if(is_scalar == true){
    for(size_t idx = 0; idx < nr_args; ++idx){
      // Data type mixing is not allowed
      if(ordinal_valuescale(*field_arguments.at(idx))){
        throw std::runtime_error("argument " + std::to_string(idx + 1) + " is of type 'ordinal'; allowed type is 'scalar'");
      }
      // In case we got an integer (nominal) as argument we can cast to scalar
      if(nominal_valuescale(*field_arguments.at(idx))){
        field_arguments.at(idx) = to_scalar(field_arguments.at(idx));
      }
    }

    return detail::maximum<REAL4>(field_arguments);
  }

  assert(false);
  return nullptr;
}

} // namespace python
} // namespace pcraster_multicore


#pragma once

#include <fstream>
#include <mutex>

#include <basalt/utils/ba_utils.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <basalt/linearization/landmark_block.hpp>
#include <basalt/utils/sophus_utils.hpp>

namespace basalt {

template <typename Scalar, int POSE_SIZE>
class LandmarkBlockAbsDynamic : public LandmarkBlock<Scalar> {
 public:
  using Options = typename LandmarkBlock<Scalar>::Options;
  using State = typename LandmarkBlock<Scalar>::State;

  inline bool isNumericalFailure() const override {
    return state == State::NumericalFailure;
  }

  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;

  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;

  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
  using RowMatX =
      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

  virtual inline void allocateLandmark(
      Keypoint<Scalar>& lm,
      const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
                                         RelPoseLin<Scalar>>& relative_pose_lin,
      const Calibration<Scalar>& calib, const AbsOrderMap& aom,
      const Options& options,
      const std::map<TimeCamId, size_t>* rel_order = nullptr) override {
    // some of the logic assumes the members are at their initial values
    BASALT_ASSERT(state == State::Uninitialized);

    UNUSED(rel_order);

    lm_ptr = &lm;
    options_ = &options;
    calib_ = &calib;

    // TODO: consider for VIO that we have a lot of 0 columns if we just use aom
    // --> add option to use different AOM with reduced size and/or just
    // involved poses --> when accumulating results, check which case we have;
    // if both aom are identical, we don't have to do block-wise operations.
    aom_ = &aom;

    pose_lin_vec.clear();
    pose_lin_vec.reserve(lm.obs.size());
    pose_tcid_vec.clear();
    pose_tcid_vec.reserve(lm.obs.size());

    // LMBs without host frame should not be created
    BASALT_ASSERT(aom.abs_order_map.count(lm.host_kf_id.frame_id) > 0);

    for (const auto& [tcid_t, pos] : lm.obs) {
      size_t i = pose_lin_vec.size();

      auto it = relative_pose_lin.find(std::make_pair(lm.host_kf_id, tcid_t));
      BASALT_ASSERT(it != relative_pose_lin.end());

      if (aom.abs_order_map.count(tcid_t.frame_id) > 0) {
        pose_lin_vec.push_back(&it->second);
      } else {
        // Observation droped for marginalization
        pose_lin_vec.push_back(nullptr);
      }
      pose_tcid_vec.push_back(&it->first);

      res_idx_by_abs_pose_[it->first.first.frame_id].insert(i);   // host
      res_idx_by_abs_pose_[it->first.second.frame_id].insert(i);  // target
    }

    // number of pose-jacobian columns is determined by oam
    padding_idx = aom_->total_size;

    num_rows = pose_lin_vec.size() * 2 + 3;  // residuals and lm damping

    size_t pad = padding_idx % 4;
    if (pad != 0) {
      padding_size = 4 - pad;
    }

    lm_idx = padding_idx + padding_size;
    res_idx = lm_idx + 3;
    num_cols = res_idx + 1;

    // number of columns should now be multiple of 4 for good memory alignment
    // TODO: test extending this to 8 --> 32byte alignment for float?
    BASALT_ASSERT(num_cols % 4 == 0);

    storage.resize(num_rows, num_cols);

    damping_rotations.clear();
    damping_rotations.reserve(6);

    state = State::Allocated;
  }

  // may set state to NumericalFailure --> linearization at this state is
  // unusable. Numeric check is only performed for residuals that were
  // considered to be used (valid), which depends on
  // use_valid_projections_only setting.
  virtual inline Scalar linearizeLandmark() override {
    BASALT_ASSERT(state == State::Allocated ||
                  state == State::NumericalFailure ||
                  state == State::Linearized || state == State::Marginalized);

    // storage.setZero(num_rows, num_cols);
    storage.setZero();
    damping_rotations.clear();
    damping_rotations.reserve(6);

    bool numerically_valid = true;

    Scalar error_sum = 0;

    size_t i = 0;
    for (const auto& [tcid_t, obs] : lm_ptr->obs) {
      std::visit(
          [&, obs = obs](const auto& cam) {
            // TODO: The pose_lin_vec[i] == nullptr is intended to deal with
            // dropped measurements during marginalization. However, dropped
            // measurements should only occur for the remaining frames, not for
            // the marginalized frames. Maybe these are observations bewtween
            // two marginalized frames, if more than one is marginalized at the
            // same time? But those we would not have to drop... Double check if
            // and when this happens and possibly resolve by fixing handling
            // here, or else updating the measurements in lmdb before calling
            // linearization. Otherwise, check where else we need a `if
            // (pose_lin_vec[i])` check or `pose_lin_vec[i] != nullptr` assert
            // in this class.

            if (pose_lin_vec[i]) {
              size_t obs_idx = i * 2;
              size_t abs_h_idx =
                  aom_->abs_order_map.at(pose_tcid_vec[i]->first.frame_id)
                      .first;
              size_t abs_t_idx =
                  aom_->abs_order_map.at(pose_tcid_vec[i]->second.frame_id)
                      .first;

              Vec2 res;
              Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
              Eigen::Matrix<Scalar, 2, 3> d_res_d_p;

              using CamT = std::decay_t<decltype(cam)>;
              bool valid = linearizePoint<Scalar, CamT>(
                  obs, *lm_ptr, pose_lin_vec[i]->T_t_h, cam, res, &d_res_d_xi,
                  &d_res_d_p);

              if (!options_->use_valid_projections_only || valid) {
                numerically_valid = numerically_valid &&
                                    d_res_d_xi.array().isFinite().all() &&
                                    d_res_d_p.array().isFinite().all();

                const Scalar res_squared = res.squaredNorm();
                const auto [weighted_error, weight] =
                    compute_error_weight(res_squared);
                const Scalar sqrt_weight =
                    std::sqrt(weight) / options_->obs_std_dev;

                error_sum += weighted_error /
                             (options_->obs_std_dev * options_->obs_std_dev);

                storage.template block<2, 3>(obs_idx, lm_idx) =
                    sqrt_weight * d_res_d_p;
                storage.template block<2, 1>(obs_idx, res_idx) =
                    sqrt_weight * res;

                d_res_d_xi *= sqrt_weight;
                storage.template block<2, 6>(obs_idx, abs_h_idx) +=
                    d_res_d_xi * pose_lin_vec[i]->d_rel_d_h;
                storage.template block<2, 6>(obs_idx, abs_t_idx) +=
                    d_res_d_xi * pose_lin_vec[i]->d_rel_d_t;
              }
            }

            i++;
          },
          calib_->intrinsics[tcid_t.cam_id].variant);
    }

    if (numerically_valid) {
      state = State::Linearized;
    } else {
      state = State::NumericalFailure;
    }

    return error_sum;
  }

  virtual inline void performQR() override {
    BASALT_ASSERT(state == State::Linearized);

    // Since we use dense matrices Householder QR might be better:
    // https://mathoverflow.net/questions/227543/why-householder-reflection-is-better-than-givens-rotation-in-dense-linear-algebr

    if (options_->use_householder) {
      performQRHouseholder();
    } else {
      performQRGivens();
    }

    state = State::Marginalized;
  }

  // Sets damping and maintains upper triangular matrix for landmarks.
  virtual inline void setLandmarkDamping(Scalar lambda) override {
    BASALT_ASSERT(state == State::Marginalized);
    BASALT_ASSERT(lambda >= 0);

    if (hasLandmarkDamping()) {
      BASALT_ASSERT(damping_rotations.size() == 6);

      // undo dampening
      for (int n = 2; n >= 0; n--) {
        for (int m = n; m >= 0; m--) {
          storage.applyOnTheLeft(num_rows - 3 + n - m, n,
                                 damping_rotations.back().adjoint());
          damping_rotations.pop_back();
        }
      }
    }

    if (lambda == 0) {
      storage.template block<3, 3>(num_rows - 3, lm_idx).diagonal().setZero();
    } else {
      BASALT_ASSERT(Jl_col_scale.array().isFinite().all());

      storage.template block<3, 3>(num_rows - 3, lm_idx)
          .diagonal()
          .setConstant(sqrt(lambda));

      BASALT_ASSERT(damping_rotations.empty());

      // apply dampening and remember rotations to undo
      for (int n = 0; n < 3; n++) {
        for (int m = 0; m <= n; m++) {
          damping_rotations.emplace_back();
          damping_rotations.back().makeGivens(
              storage(n, lm_idx + n),
              storage(num_rows - 3 + n - m, lm_idx + n));
          storage.applyOnTheLeft(num_rows - 3 + n - m, n,
                                 damping_rotations.back());
        }
      }
    }
  }

  // lambda < 0 means computing exact model cost change
  virtual inline void backSubstitute(const VecX& pose_inc,
                                     Scalar& l_diff) override {
    BASALT_ASSERT(state == State::Marginalized);

    // For now we include all columns in LMB
    BASALT_ASSERT(pose_inc.size() == signed_cast(padding_idx));

    const auto Q1Jl = storage.template block<3, 3>(0, lm_idx)
                          .template triangularView<Eigen::Upper>();

    const auto Q1Jr = storage.col(res_idx).template head<3>();
    const auto Q1Jp = storage.topLeftCorner(3, padding_idx);

    Vec3 inc = -Q1Jl.solve(Q1Jr + Q1Jp * pose_inc);

    // We want to compute the model cost change. The model function is
    //
    //     L(inc) = F(x) + inc^T J^T r + 0.5 inc^T J^T J inc
    //
    // and thus the expected decrease in cost for the computed increment is
    //
    //     l_diff = L(0) - L(inc)
    //            = - inc^T J^T r - 0.5 inc^T J^T J inc
    //            = - inc^T J^T (r + 0.5 J inc)
    //            = - (J inc)^T (r + 0.5 (J inc)).
    //
    // Here we have J = [Jp, Jl] under the orthogonal projection Q = [Q1, Q2],
    // i.e. the linearized system (model cost) is
    //
    //    L(inc) = 0.5 || J inc + r ||^2 = 0.5 || Q^T J inc + Q^T r ||^2
    //
    // and below we thus compute
    //
    //    l_diff = - (Q^T J inc)^T (Q^T r + 0.5 (Q^T J inc)).
    //
    // We have
    //             | Q1^T |            | Q1^T Jp   Q1^T Jl |
    //    Q^T J =  |      | [Jp, Jl] = |                   |
    //             | Q2^T |            | Q2^T Jp      0    |.
    //
    // Note that Q2 is the nullspace of Jl, and Q1^T Jl == R. So with inc =
    // [incp^T, incl^T]^T we have
    //
    //                | Q1^T Jp incp + Q1^T Jl incl |
    //    Q^T J inc = |                             |
    //                | Q2^T Jp incp                |
    //

    // undo damping before we compute the model cost difference
    setLandmarkDamping(0);

    // compute "Q^T J incp"
    VecX QJinc = storage.topLeftCorner(num_rows - 3, padding_idx) * pose_inc;

    // add "Q1^T Jl incl" to the first 3 rows
    QJinc.template head<3>() += Q1Jl * inc;

    auto Qr = storage.col(res_idx).head(num_rows - 3);
    l_diff -= QJinc.transpose() * (Scalar(0.5) * QJinc + Qr);

    // TODO: detect and handle case like ceres, allowing a few iterations but
    // stopping eventually
    if (!inc.array().isFinite().all() ||
        !lm_ptr->direction.array().isFinite().all() ||
        !std::isfinite(lm_ptr->inv_dist)) {
      std::cerr << "Numerical failure in backsubstitution\n";
    }

    // Note: scale only after computing model cost change
    inc.array() *= Jl_col_scale.array();

    lm_ptr->direction += inc.template head<2>();
    lm_ptr->inv_dist = std::max(Scalar(0), lm_ptr->inv_dist + inc[2]);
  }

  virtual inline size_t numReducedCams() const override {
    BASALT_LOG_FATAL("check what we mean by numReducedCams for absolute poses");
    return pose_lin_vec.size();
  }

  inline void addQ2JpTQ2Jp_mult_x(VecX& res,
                                  const VecX& x_pose) const override {
    UNUSED(res);
    UNUSED(x_pose);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual inline void addQ2JpTQ2r(VecX& res) const override {
    UNUSED(res);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual inline void addJp_diag2(VecX& res) const override {
    BASALT_ASSERT(state == State::Linearized);

    for (const auto& [frame_id, idx_set] : res_idx_by_abs_pose_) {
      const int pose_idx = aom_->abs_order_map.at(frame_id).first;
      for (const int i : idx_set) {
        const auto block = storage.block(2 * i, pose_idx, 2, POSE_SIZE);

        res.template segment<POSE_SIZE>(pose_idx) +=
            block.colwise().squaredNorm();
      }
    }
  }

  virtual inline void addQ2JpTQ2Jp_blockdiag(
      BlockDiagonalAccumulator<Scalar>& accu) const override {
    UNUSED(accu);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual inline void scaleJl_cols() override {
    BASALT_ASSERT(state == State::Linearized);

    // ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
    // we use 1.0 / (eps + sqrt(SquaredColumnNorm))
    Jl_col_scale =
        (options_->jacobi_scaling_eps +
         storage.block(0, lm_idx, num_rows - 3, 3).colwise().norm().array())
            .inverse();

    storage.block(0, lm_idx, num_rows - 3, 3) *= Jl_col_scale.asDiagonal();
  }

  virtual inline void scaleJp_cols(const VecX& jacobian_scaling) override {
    BASALT_ASSERT(state == State::Marginalized);

    // we assume we apply scaling before damping (we exclude the last 3 rows)
    BASALT_ASSERT(!hasLandmarkDamping());

    storage.topLeftCorner(num_rows - 3, padding_idx) *=
        jacobian_scaling.asDiagonal();
  }

  inline bool hasLandmarkDamping() const { return !damping_rotations.empty(); }

  virtual inline void printStorage(const std::string& filename) const override {
    std::ofstream f(filename);

    Eigen::IOFormat CleanFmt(4, 0, " ", "\n", "", "");

    f << "Storage (state: " << state
      << ", damping: " << (hasLandmarkDamping() ? "yes" : "no")
      << " Jl_col_scale: " << Jl_col_scale.transpose() << "):\n"
      << storage.format(CleanFmt) << std::endl;

    f.close();
  }
#if 0
  virtual inline void stage2(
      Scalar lambda, const VecX* jacobian_scaling, VecX* precond_diagonal2,
      BlockDiagonalAccumulator<Scalar>* precond_block_diagonal,
      VecX& bref) override {
    // 1. scale jacobian
    if (jacobian_scaling) {
      scaleJp_cols(*jacobian_scaling);
    }

    // 2. dampen landmarks
    setLandmarkDamping(lambda);

    // 3a. compute diagonal preconditioner (SCHUR_JACOBI_DIAGONAL)
    if (precond_diagonal2) {
      addQ2Jp_diag2(*precond_diagonal2);
    }

    // 3b. compute block diagonal preconditioner (SCHUR_JACOBI)
    if (precond_block_diagonal) {
      addQ2JpTQ2Jp_blockdiag(*precond_block_diagonal);
    }

    // 4. compute rhs of reduced camera normal equations
    addQ2JpTQ2r(bref);
  }
#endif

  inline State getState() const override { return state; }

  virtual inline size_t numQ2rows() const override { return num_rows - 3; }

 protected:
  inline void performQRGivens() {
    // Based on "Matrix Computations 4th Edition by Golub and Van Loan"
    // See page 252, Algorithm 5.2.4 for how these two loops work
    Eigen::JacobiRotation<Scalar> gr;
    for (size_t n = 0; n < 3; n++) {
      for (size_t m = num_rows - 4; m > n; m--) {
        gr.makeGivens(storage(m - 1, lm_idx + n), storage(m, lm_idx + n));
        storage.applyOnTheLeft(m, m - 1, gr);
      }
    }
  }

  inline void performQRHouseholder() {
    VecX tempVector1(num_cols);
    VecX tempVector2(num_rows - 3);

    for (size_t k = 0; k < 3; ++k) {
      size_t remainingRows = num_rows - k - 3;

      Scalar beta;
      Scalar tau;
      storage.col(lm_idx + k)
          .segment(k, remainingRows)
          .makeHouseholder(tempVector2, tau, beta);

      storage.block(k, 0, remainingRows, num_cols)
          .applyHouseholderOnTheLeft(tempVector2, tau, tempVector1.data());
    }
  }

  inline std::tuple<Scalar, Scalar> compute_error_weight(
      Scalar res_squared) const {
    // Note: Definition of cost is 0.5 ||r(x)||^2 to be in line with ceres

    if (options_->huber_parameter > 0) {
      // use huber norm
      const Scalar huber_weight =
          res_squared <= options_->huber_parameter * options_->huber_parameter
              ? Scalar(1)
              : options_->huber_parameter / std::sqrt(res_squared);
      const Scalar error =
          Scalar(0.5) * (2 - huber_weight) * huber_weight * res_squared;
      return {error, huber_weight};
    } else {
      // use squared norm
      return {Scalar(0.5) * res_squared, Scalar(1)};
    }
  }

  void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
                          size_t start_idx) const override {
    Q2r.segment(start_idx, num_rows - 3) =
        storage.col(res_idx).tail(num_rows - 3);

    BASALT_ASSERT(Q2Jp.cols() == signed_cast(padding_idx));

    Q2Jp.block(start_idx, 0, num_rows - 3, padding_idx) =
        storage.block(3, 0, num_rows - 3, padding_idx);
  }

  void get_dense_Q2Jp_Q2r_rel(
      MatX& Q2Jp, VecX& Q2r, size_t start_idx,
      const std::map<TimeCamId, size_t>& rel_order) const override {
    UNUSED(Q2Jp);
    UNUSED(Q2r);
    UNUSED(start_idx);
    UNUSED(rel_order);
    BASALT_LOG_FATAL("not implemented");
  }

  void add_dense_H_b(DenseAccumulator<Scalar>& accum) const override {
    UNUSED(accum);
    BASALT_LOG_FATAL("not implemented");
  }

  void add_dense_H_b(MatX& H, VecX& b) const override {
    const auto r = storage.col(res_idx).tail(num_rows - 3);
    const auto J = storage.block(3, 0, num_rows - 3, padding_idx);

    H.noalias() += J.transpose() * J;
    b.noalias() += J.transpose() * r;
  }

  void add_dense_H_b_rel(
      MatX& H_rel, VecX& b_rel,
      const std::map<TimeCamId, size_t>& rel_order) const override {
    UNUSED(H_rel);
    UNUSED(b_rel);
    UNUSED(rel_order);
    BASALT_LOG_FATAL("not implemented");
  }

  const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
      const override {
    BASALT_LOG_FATAL("not implemented");
  }

  Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
      const std::map<TimeCamId, size_t>& rel_order) const override {
    UNUSED(rel_order);
    BASALT_LOG_FATAL("not implemented");
  }

  void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const override {
    UNUSED(H_rel);
    UNUSED(b_rel);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual TimeCamId getHostKf() const override { return lm_ptr->host_kf_id; }

 private:
  // Dense storage for pose Jacobians, padding, landmark Jacobians and
  // residuals [J_p | pad | J_l | res]
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
      storage;

  Vec3 Jl_col_scale = Vec3::Ones();
  std::vector<Eigen::JacobiRotation<Scalar>> damping_rotations;

  std::vector<const RelPoseLin<Scalar>*> pose_lin_vec;
  std::vector<const std::pair<TimeCamId, TimeCamId>*> pose_tcid_vec;
  size_t padding_idx = 0;
  size_t padding_size = 0;
  size_t lm_idx = 0;
  size_t res_idx = 0;

  size_t num_cols = 0;
  size_t num_rows = 0;

  const Options* options_ = nullptr;

  State state = State::Uninitialized;

  Keypoint<Scalar>* lm_ptr = nullptr;
  const Calibration<Scalar>* calib_ = nullptr;
  const AbsOrderMap* aom_ = nullptr;

  std::map<int64_t, std::set<int>> res_idx_by_abs_pose_;
};

}  // namespace basalt


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

#ifndef BOOST_ASTRONOMY_COORDINATE_BASE_COORDINATE_SYSTEM_HPP
#define BOOST_ASTRONOMY_COORDINATE_BASE_COORDINATE_SYSTEM_HPP

#include <tuple>
#include <cmath>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/static_assert.hpp>

#include <boost/astronomy/coordinate/representation.hpp>
#include <boost/astronomy/coordinate/differential.hpp>
#include <boost/astronomy/detail/is_base_template_of.hpp>
#include <boost/astronomy/coordinate/arithmetic.hpp>

namespace boost { namespace astronomy { namespace coordinate {

typedef boost::geometry::degree degree;
typedef boost::geometry::radian radian;

namespace bu = boost::units;
namespace bg = boost::geometry;

template <typename Representation, typename Differential>
struct base_frame
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
        <boost::astronomy::coordinate::base_representation, Representation>::value),
        "First template argument is expected to be a representation class");
    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
        <boost::astronomy::coordinate::base_differential, Differential>::value),
        "Second template argument is expected to be a differential class");
    ///@endcond

protected:
    Representation data;
    Differential motion;

public:
    typedef Representation representation;
    typedef Differential differential;

    //!returns 2d propermotion
    std::tuple
    <
        typename Differential::quantity1,
        typename Differential::quantity2
    > get_proper_motion() const
    {
        return std::make_tuple
        (
            Differential::quantity1::from_value(bg::get<0>(motion.get_differential())),
            Differential::quantity2::from_value(bg::get<1>(motion.get_differential()))
        );
    }

    //!returns radial velocity (line of sight velocity) of the object
    typename Differential::quantity3 get_radial_velocity() const
    {
        return Differential::quantity3::from_value(bg::get<2>(motion.get_differential()));
    }

    /*!returns differential data in form of boost::geometry::model::point
    having components (pm_lat, pm_lon, radial_velocity) including cos(lat) component*/
    Differential get_differential() const
    {
        return motion;
    }

    //!set differential for the motion of the object
    void set_differential(Differential const& other)
    {
        this->motion = other;
    }

    /*!returns coordinate data in form of boost::geometry::model::point
    having components (lat, lon, distance)*/
    Representation get_data() const
    {
        return data;
    }

    //!returns data in specified subclass of base_representation
    //template <typename ReturnType>
    //ReturnType represent_as() const
    //{
    //    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
    //        <boost::astronomy::coordinate::base_representation, ReturnType>::value),
    //        "return type is expected to be a representation class");

    //    return ReturnType(data);
    //}

    //!angular separation between two coordinates in radians
    bu::quantity<bu::si::plane_angle> get_angular_separation(base_frame const& other) const
    {
        return bu::quantity<bu::si::plane_angle>::from_value(
            std::acos((dot(this->data, other.get_data()) /
                (magnitude(this->data) * magnitude(other.get_data()))).value())
        );
    }

};
}}} //namespace boost::astronomy::coordinate
#endif  // !BOOST_ASTRONOMY_COORDINATE_BASE_COORDINATE_SYSTEM_HPP


/*! \file hydrodynamics_2d.hpp
  \brief Various manipulations of hydrodynamic variables
  \author Almog Yalinewich
*/

#ifndef HYDRODYNAMICS_2D_HPP
#define HYDRODYNAMICS_2D_HPP 1

#include "spatial_distribution2d.hpp"
#include "../common/equation_of_state.hpp"
#include "point_motion.hpp"
#include "spatial_reconstruction.hpp"
#include "SourceTerm.hpp"
#include "../../misc/utils.hpp"
#include "../../misc/lazy_list.hpp"
#include "../common/hydrodynamics.hpp"
#include "../../misc/universal_error.hpp"
#include "../../tessellation/ConvexHull.hpp"
#include "../../tessellation/polygon_overlap_area.hpp"
#include <boost/scoped_ptr.hpp>
#include "../../tessellation/EdgeLengthCorrect.hpp"
#include "physical_geometry.hpp"
#include "time_step_function.hpp"
#include "cache_data.hpp"
#include "../common/riemann_solver.hpp"

/*! \brief Rotates primitive variables to align with edge
  \param n Normal directions
  \param p Parallel direction
  \param cell Primitive variables
  \return Rotated cell
 */
Primitive RotatePrimitive(const Vector2D& n,
	const Vector2D& p,
	const Primitive& cell);

/*! \brief Rotates flux from the edge frame back to the lab frame
  \param c Flux
  \param n Normal direction
  \param p Parallel direction
  \return Rotated flux
 */
Conserved RotateFluxBack(const Conserved& c,
	const Vector2D& n,
	const Vector2D& p);

/*! \brief Given an edge and an index of one neighbor, returns the index of another neighbor
  \param edge Voronoi edge
  \param index Index of one neighbor
  \return Index of the other neighbor
 */
int get_other_index(const Edge& edge,
	const int index);

/*! \brief Initialize computational cells
  \param density Density distribution
  \param pressure Pressure distribution
  \param xvelocity Distribution of the x component of the velocity
  \param yvelocity Distribution of the y component of the velocity
  \param eos Equation of state
  \param tess Tessellation
  \param CMvalue Determines whether to evaluate the spatial distributions in the mesh generating points or the center of mass
  \return List of primitive variables
*/
vector<Primitive> InitialiseCells
(SpatialDistribution const& density,
	SpatialDistribution const& pressure,
	SpatialDistribution const& xvelocity,
	SpatialDistribution const& yvelocity,
	EquationOfState const& eos,
	Tessellation const& tess, bool CMvalue = true);

/*! \brief Calculates the intensive conserved variables
  \param cells Hydrodynamical cells
  \return List of conserved variables
*/
vector<Conserved> CalcConservedIntensive
(vector<Primitive> const& cells);

/*! \brief Calculates the extensive conserved variables
  \param cons_int Conserved intensive variables
  \param tess Tessellation
  \param pg Physical geometry
  \return List of conserved variables
*/
vector<Conserved> CalcConservedExtensive
(const vector<Conserved>& cons_int,
	const Tessellation& tess,
	const PhysicalGeometry& pg);

/*! \brief Calculates the time step for a cell
  \param cell Computational cell
  \param width Cell width
  \param face_velocites Velocities of the edges of the cell
  \return Time step
*/
double TimeStepForCell(Primitive const& cell,
	double width, vector<Vector2D> const& face_velocites);

/*! \brief Move mesh points
  \param pointvelocity Velocities of all mesh points
  \param dt Time step
  \param tessellation Tessellation
  \param oldpoints Possible input for the location of the old points
  \param reorder Should we reorder the points
  \return The indeces of the hilbert order of the points
*/
vector<int> MoveMeshPoints(vector<Vector2D> const& pointvelocity,
	double dt, Tessellation& tessellation, bool reorder,
	vector<Vector2D> oldpoints = vector<Vector2D>());
/*! \brief Move mesh points
  \param pointvelocity Velocities of all mesh points
  \param dt Time step
  \param tessellation The local tessellation
  \param vproc The processor tessellation
  \param oldpoints Possible input for the location of the old points
  \param reorder Should we reorder the points
  \return The indeces of the hilbert order of the points
*/
vector<int> MoveMeshPoints(vector<Vector2D> const& pointvelocity,
	double dt, Tessellation& tessellation, Tessellation const& vproc, bool reorder,
	vector<Vector2D> oldpoints = vector<Vector2D>());

/*! \brief Calculates the intensive conserved variables
  \param tess Tessellation
  \param extensive Extensive conserved variables
  \param pg Physical geometry
  \return List of intensive conserved variables
 */
vector<Conserved> calc_conserved_intensive
(const Tessellation& tess,
	const vector<Conserved>& extensive,
	const PhysicalGeometry& pg);

/*! \brief Updates the intensive conserved variables
  \param tessellation Tessellation
  \param conservedextensive Extensive conserved variables
  \param conservedintensive Intensive conserved variables
*/
void UpdateConservedIntensive(Tessellation const& tessellation,
	vector<Conserved> const& conservedextensive,
	vector<Conserved>& conservedintensive);

/*! \brief Calculates the flux in the bulk of the fluid
  \param normaldir A unit vector normal to the interface
  \param paraldir A unit vector parallel to the interface
  \param left Primitive variables on the left side of the interface
  \param right Primitive variables on the right side of the interface
  \param edge_velocity Velocity of the interface
  \param rs Riemann solver
  \return Flux
 */
Conserved FluxInBulk(Vector2D const& normaldir,
	Vector2D const& paraldir,
	Primitive const& left,
	Primitive const& right,
	Vector2D const& edge_velocity,
	RiemannSolver const& rs);

/*! \brief Adds force contribution to the extensive conserved variables
  \param tess Tessellation
  \param pg Physical geometry
  \param cd Cache data
  \param cells Computational cells
  \param fluxes Fluxes
  \param point_velocities Velocities of the mesh generating points
  \param source Source term
  \param t Time
  \param dt Time step
  \param extensives Extensive variables
  \param tracerstickernames The names of the tracers and stickers
 */
void ExternalForceContribution
(const Tessellation& tess,
	const PhysicalGeometry& pg,
	const CacheData& cd,
	const vector<ComputationalCell>& cells,
	const vector<Extensive>& fluxes,
	const vector<Vector2D>& point_velocities,
	const SourceTerm& source,
	double t,
	double dt,
	vector<Extensive>& extensives,
	TracerStickerNames const& tracerstickernames);

/*! \brief Returns the position of all mesh generating points
  \param tess Tessellation
  \return Position of all mesh generating points
 */
vector<Vector2D> get_all_mesh_points
(Tessellation const& tess);

/*! \brief Changes the energy and sound speed so they would satisfy the equation of state
  \param vp Primitive variables
  \param eos Equation of state
  \return Primitive variable with corrected sound speed and energy
 */
vector<Primitive> make_eos_consistent
(vector<Primitive> const& vp,
	EquationOfState const& eos);

/*! \brief Returns the energies due to external potentials
  \param tess Tessellation
  \param g TBA
  \return TBA
  \todo Add documentation
 */
vector<double> GetForceEnergy(Tessellation const& tess,
	vector<double> const& g);

/*! \brief Calculates extensive tracers
  \param intensive_tracer Intensive tracers
  \param tess Tessellation
  \param cells List of primitive variables
  \param pg Physical geometry
  \return List of extensive tracers
 */
vector<vector<double> > calc_extensive_tracer
(const vector<vector<double> > & intensive_tracer,
	const Tessellation& tess,
	const vector<Primitive>& cells,
	const PhysicalGeometry& pg);

/*! \brief Calculates the extensive tracer
  \param tracer Intensive tracer
  \param tess Tessellation
  \param cells Fluid elements
  \param result List of extensive tracers
 */
void MakeTracerExtensive
(vector<vector<double> > const&tracer,
	Tessellation const& tess, vector<Primitive> const& cells,
	vector<vector<double> > &result);

/*! \brief Makes a list of points to remove
  \param tess Tessellation
  \param point TBA
  \param R Radius?
  \param PointToRemove output
  \param Inner TBA
  \todo Add documentation
 */
void GetPointToRemove(Tessellation const& tess, Vector2D const& point,
	double R, vector<int> & PointToRemove, int Inner);

/*! \brief Applies a correction to the extensive variables due to the change in volume during time step.
  \details This method calculates the change in extensive by calculating the volume swept by an edge and multiplying it by the intensive variables of the respective cell.
  \param extensive Extensive variables
  \param intensive Intensive variables
  \param tessold Old tessellation
  \param tessnew New tessellation
  \param facevelocity Face velocity
  \param dt Time step
  \param pointvelocity Velocities of mesh generating points
 */
void FixAdvection(vector<Conserved>& extensive,
	vector<Conserved> const& intensive,
	Tessellation const& tessold,
	Tessellation const& tessnew,
	vector<Vector2D> const& facevelocity,
	double dt, vector<Vector2D> const& pointvelocity);

/*! \brief Determines the time step
  \param hydro_time_step Time step derived from hydrodynamics
  \param external_dt Time step suggested by user
  \param current_time Current simulation time
  \param end_time Termination time
  \return Time step
 */
double determine_time_step(double hydro_time_step,
	double external_dt,
	double current_time,
	double end_time);

#endif // HYDRODYNAMICS_2D_HPP


/*
SHA-1 C++ conversion

original version:

SHA-1 in C
By Steve Reid <sreid@sea-to-sky.net>
100% Public Domain

changelog at the end of the file.
*/

#include <cstdio>
#include <cstring>

// if you don't want boost
// replace with
// #include <stdint.h>
// typedef uint32_t u32;
// typedef uint8_t u8;

#include <boost/cstdint.hpp>
typedef boost::uint32_t u32;
typedef boost::uint8_t u8;

#include "libtorrent/config.hpp"

namespace libtorrent
{

struct TORRENT_EXPORT sha_ctx
{
	u32 state[5];
	u32 count[2];
	u8 buffer[64];
};

// we don't want these to clash with openssl's libcrypto
TORRENT_EXPORT void SHA1_init(sha_ctx* context);
TORRENT_EXPORT void SHA1_update(sha_ctx* context, u8 const* data, u32 len);
TORRENT_EXPORT void SHA1_final(u8* digest, sha_ctx* context);

namespace
{
	union CHAR64LONG16
	{
		u8 c[64];
		u32 l[16];
	};

#define rol(value, bits) (((value) << (bits)) | ((value) >> (32 - (bits))))

// blk0() and blk() perform the initial expand.
// I got the idea of expanding during the round function from SSLeay
	struct little_endian_blk0
	{
		static u32 apply(CHAR64LONG16* block, int i)
		{
			return block->l[i] = (rol(block->l[i],24)&0xFF00FF00)
				| (rol(block->l[i],8)&0x00FF00FF);
		}
	};

	struct big_endian_blk0
	{
		static u32 apply(CHAR64LONG16* block, int i)
		{
			return  block->l[i];
		}
	};

#define blk(i) (block->l[i&15] = rol(block->l[(i+13)&15]^block->l[(i+8)&15] \
    ^block->l[(i+2)&15]^block->l[i&15],1))

// (R0+R1), R2, R3, R4 are the different operations used in SHA1
#define R0(v,w,x,y,z,i) z+=((w&(x^y))^y)+BlkFun::apply(block, i)+0x5A827999+rol(v,5);w=rol(w,30);
#define R1(v,w,x,y,z,i) z+=((w&(x^y))^y)+blk(i)+0x5A827999+rol(v,5);w=rol(w,30);
#define R2(v,w,x,y,z,i) z+=(w^x^y)+blk(i)+0x6ED9EBA1+rol(v,5);w=rol(w,30);
#define R3(v,w,x,y,z,i) z+=(((w|x)&y)|(w&x))+blk(i)+0x8F1BBCDC+rol(v,5);w=rol(w,30);
#define R4(v,w,x,y,z,i) z+=(w^x^y)+blk(i)+0xCA62C1D6+rol(v,5);w=rol(w,30);

	// Hash a single 512-bit block. This is the core of the algorithm.
	template <class BlkFun>
	void SHA1transform(u32 state[5], u8 const buffer[64])
	{
		using namespace std;
		u32 a, b, c, d, e;

		CHAR64LONG16* block;
		u8 workspace[64];
		block = (CHAR64LONG16*)workspace;
		memcpy(block, buffer, 64);

		// Copy context->state[] to working vars
		a = state[0];
		b = state[1];
		c = state[2];
		d = state[3];
		e = state[4];
		// 4 rounds of 20 operations each. Loop unrolled.
		R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3);
		R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7);
		R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11);
		R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15);
		R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
		R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23);
		R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27);
		R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31);
		R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35);
		R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39);
		R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43);
		R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47);
		R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51);
		R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55);
		R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59);
		R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63);
		R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67);
		R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71);
		R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75);
		R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79);
		// Add the working vars back into context.state[]
		state[0] += a;
		state[1] += b;
		state[2] += c;
		state[3] += d;
		state[4] += e;
	}

#ifdef VERBOSE
	void SHAPrintContext(sha_ctx *context, char *msg)
	{
		using namespace std;
		printf("%s (%d,%d) %x %x %x %x %x\n"
			, msg, (unsigned int)context->count[0]
			, (unsigned int)context->count[1]
			, (unsigned int)context->state[0]
			, (unsigned int)context->state[1]
			, (unsigned int)context->state[2]
			, (unsigned int)context->state[3]
			, (unsigned int)context->state[4]);
	}
#endif

	template <class BlkFun>
	void internal_update(sha_ctx* context, u8 const* data, u32 len)
	{
		using namespace std;
		u32 i, j;	// JHB

#ifdef VERBOSE
		SHAPrintContext(context, "before");
#endif
		j = (context->count[0] >> 3) & 63;
		if ((context->count[0] += len << 3) < (len << 3)) context->count[1]++;
		context->count[1] += (len >> 29);
		if ((j + len) > 63)
		{
			memcpy(&context->buffer[j], data, (i = 64-j));
			SHA1transform<BlkFun>(context->state, context->buffer);
			for ( ; i + 63 < len; i += 64)
			{
				SHA1transform<BlkFun>(context->state, &data[i]);
			}
			j = 0;
		}
		else
		{
			i = 0;
		}
		memcpy(&context->buffer[j], &data[i], len - i);
#ifdef VERBOSE
		SHAPrintContext(context, "after ");
#endif
	}

#if !defined BOOST_BIG_ENDIAN && !defined BOOST_LITTLE_ENDIAN
	bool is_big_endian()
	{
		u32 test = 1;
		return *reinterpret_cast<u8*>(&test) == 0;
	}
#endif
}

// SHA1Init - Initialize new context

void SHA1_init(sha_ctx* context)
{
    // SHA1 initialization constants
    context->state[0] = 0x67452301;
    context->state[1] = 0xEFCDAB89;
    context->state[2] = 0x98BADCFE;
    context->state[3] = 0x10325476;
    context->state[4] = 0xC3D2E1F0;
    context->count[0] = context->count[1] = 0;
}

// Run your data through this.

void SHA1_update(sha_ctx* context, u8 const* data, u32 len)
{
	// GCC standard defines for endianness
	// test with: cpp -dM /dev/null
#if defined BOOST_BIG_ENDIAN
	internal_update<big_endian_blk0>(context, data, len);
#elif defined BOOST_LITTLE_ENDIAN
	internal_update<little_endian_blk0>(context, data, len);
#else
	// select different functions depending on endianess
	// and figure out the endianess runtime
	if (is_big_endian())
		internal_update<big_endian_blk0>(context, data, len);
	else
		internal_update<little_endian_blk0>(context, data, len);
#endif
}

// Add padding and return the message digest.

void SHA1_final(u8* digest, sha_ctx* context)
{
	u8 finalcount[8];

	for (u32 i = 0; i < 8; ++i)
	{
		// Endian independent
		finalcount[i] = static_cast<u8>(
			(context->count[(i >= 4 ? 0 : 1)]
			>> ((3-(i & 3)) * 8) ) & 255);
	}

	SHA1_update(context, (u8 const*)"\200", 1);
	while ((context->count[0] & 504) != 448)
		SHA1_update(context, (u8 const*)"\0", 1);
	SHA1_update(context, finalcount, 8);  // Should cause a SHA1transform()

	for (u32 i = 0; i < 20; ++i)
	{
		digest[i] = static_cast<unsigned char>(
			(context->state[i>>2] >> ((3-(i & 3)) * 8) ) & 255);
	}
}

} // libtorrent namespace

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

-----------------
Modified 7/98
By James H. Brown <jbrown@burgoyne.com>
Still 100% Public Domain

Corrected a problem which generated improper hash values on 16 bit machines
Routine SHA1Update changed from
	void SHA1Update(SHA1_CTX* context, unsigned char* data, unsigned int
len)
to
	void SHA1Update(SHA1_CTX* context, unsigned char* data, unsigned
long len)

The 'len' parameter was declared an int which works fine on 32 bit machines.
However, on 16 bit machines an int is too small for the shifts being done
against
it.  This caused the hash function to generate incorrect values if len was
greater than 8191 (8K - 1) due to the 'len << 3' on line 3 of SHA1Update().

Since the file IO in main() reads 16K at a time, any file 8K or larger would
be guaranteed to generate the wrong hash (e.g. Test Vector #3, a million
"a"s).

I also changed the declaration of variables i & j in SHA1Update to
unsigned long from unsigned int for the same reason.

These changes should make no difference to any 32 bit implementations since
an
int and a long are the same size in those environments.

--
I also corrected a few compiler warnings generated by Borland C.
1. Added #include <process.h> for exit() prototype
2. Removed unused variable 'j' in SHA1Final
3. Changed exit(0) to return(0) at end of main.

ALL changes I made can be located by searching for comments containing 'JHB'
-----------------
Modified 8/98
By Steve Reid <sreid@sea-to-sky.net>
Still 100% public domain

1- Removed #include <process.h> and used return() instead of exit()
2- Fixed overwriting of finalcount in SHA1Final() (discovered by Chris Hall)
3- Changed email address from steve@edmweb.com to sreid@sea-to-sky.net

-----------------
Modified 4/01
By Saul Kravitz <Saul.Kravitz@celera.com>
Still 100% PD
Modified to run on Compaq Alpha hardware.

-----------------
Converted to C++ 6/04
By Arvid Norberg <arvidn@sourceforge.net>
1- made the input buffer const, and made the
   previous SHA1HANDSOFF implicit
2- uses C99 types with size guarantees
   from boost
3- if none of BOOST_BIG_ENDIAN or BOOST_LITTLE_ENDIAN
   are defined, endianess is determined
   at runtime. templates are used to duplicate
   the transform function for each endianess
4- using anonymous namespace to avoid external
   linkage on internal functions
5- using standard C++ includes
6- made API compatible with openssl

still 100% PD
*/

/*
Test Vectors (from FIPS PUB 180-1)
"abc"
  A9993E36 4706816A BA3E2571 7850C26C 9CD0D89D
"abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq"
  84983E44 1C3BD26E BAAE4AA1 F95129E5 E54670F1
A million repetitions of "a"
  34AA973C D4C4DAA4 F61EEB2B DBAD2731 6534016F
*/


/***
* Copyright 2017 Marc Stevens <marc@marc-stevens.nl>, Dan Shumow <danshu@microsoft.com>
* Distributed under the MIT Software License.
* See accompanying file LICENSE.txt or copy at
* https://opensource.org/licenses/MIT
***/

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <vector>
#include <iomanip>
#include <array>

#include <boost/nondet_random.hpp>
#include <boost/random.hpp>
#include <boost/timer.hpp>
#include <boost/progress.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/variance.hpp>

extern "C"
{
#include "sha1.h"
#include "ubc_check.h"

	void ubc_check_verify(const uint32_t W[80], uint32_t dvmask[DVMASKSIZE]);

	void nc_callback(uint64_t byteoffset, const uint32_t ihvin1[5], const uint32_t ihvin2[5], const uint32_t m1[80], const uint32_t m2[80])
	{
		unsigned i;
		printf("Detected near-collision block:\n");
		printf("IHVin1  = { 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x };\n", ihvin1[0], ihvin1[1], ihvin1[2], ihvin1[3], ihvin1[4]);
		printf("IHVin2  = { 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x };\n", ihvin2[0], ihvin2[1], ihvin2[2], ihvin2[3], ihvin2[4]);
		printf("MSGBLK1 = { 0x%08x", m1[0]);
		for (i = 1; i < 16; ++i)
			printf(", 0x%08x", m1[i]);
		printf(" };\n");
		printf("MSGBLK2 = { 0x%08x", m2[0]);
		for (i = 1; i < 16; ++i)
			printf(", 0x%08x", m2[i]);
		printf(" };\n");
	}

	// the SHA-1 context
	typedef struct {
		uint64_t total;
		uint32_t ihv[5];
		unsigned char buffer[64];
		int bigendian;
	} SHA1reg_CTX;

	#define LE32_TO_BE32(X) \
		{X = ((X << 8) & 0xFF00FF00) | ((X >> 8) & 0xFF00FF); X = (X << 16) | (X >> 16);}

	void swap_bytes(uint32_t val[16])
	{
		LE32_TO_BE32(val[0]);
        	LE32_TO_BE32(val[1]);
	        LE32_TO_BE32(val[2]);
        	LE32_TO_BE32(val[3]);
        	LE32_TO_BE32(val[4]);
        	LE32_TO_BE32(val[5]);
        	LE32_TO_BE32(val[6]);
        	LE32_TO_BE32(val[7]);
        	LE32_TO_BE32(val[8]);
        	LE32_TO_BE32(val[9]);
        	LE32_TO_BE32(val[10]);
        	LE32_TO_BE32(val[11]);
        	LE32_TO_BE32(val[12]);
        	LE32_TO_BE32(val[13]);
        	LE32_TO_BE32(val[14]);
        	LE32_TO_BE32(val[15]);
	}

#define Rotate_right(x,n) (((x)>>(n))|((x)<<(32-(n))))
#define Rotate_left(x,n)  (((x)<<(n))|((x)>>(32-(n))))

#define sha1_f1(b,c,d) ((d)^((b)&((c)^(d))))
#define sha1_f2(b,c,d) ((b)^(c)^(d))
#define sha1_f3(b,c,d) (((b) & ((c)|(d))) | ((c)&(d)))
#define sha1_f4(b,c,d) ((b)^(c)^(d))

#define HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, m, t) \
	{ e += Rotate_left(a, 5) + sha1_f1(b,c,d) + 0x5A827999 + m[t]; b = Rotate_left(b, 30); }
#define HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, m, t) \
	{ e += Rotate_left(a, 5) + sha1_f2(b,c,d) + 0x6ED9EBA1 + m[t]; b = Rotate_left(b, 30); }
#define HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, m, t) \
	{ e += Rotate_left(a, 5) + sha1_f3(b,c,d) + 0x8F1BBCDC + m[t]; b = Rotate_left(b, 30); }
#define HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, m, t) \
	{ e += Rotate_left(a, 5) + sha1_f4(b,c,d) + 0xCA62C1D6 + m[t]; b = Rotate_left(b, 30); }

void sha1_compression(uint32_t ihv[5], const uint32_t m[16])
{
	uint32_t W[80];
	uint32_t a,b,c,d,e;
	unsigned i;

	memcpy(W, m, 16 * 4);
	for (i = 16; i < 80; ++i)
		W[i] = Rotate_left(W[i - 3] ^ W[i - 8] ^ W[i - 14] ^ W[i - 16], 1);

	a = ihv[0]; b = ihv[1]; c = ihv[2]; d = ihv[3]; e = ihv[4];

	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 0);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 1);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 2);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 3);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 4);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 5);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 6);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 7);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 8);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 9);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 10);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 11);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 12);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 13);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 14);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 15);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 16);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 17);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 18);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 19);

	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 20);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 21);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 22);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 23);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 24);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 25);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 26);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 27);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 28);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 29);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 30);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 31);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 32);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 33);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 34);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 35);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 36);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 37);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 38);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 39);

	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 40);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 41);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 42);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 43);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 44);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 45);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 46);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 47);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 48);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 49);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 50);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 51);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 52);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 53);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 54);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 55);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 56);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 57);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 58);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 59);

	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 60);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 61);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 62);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 63);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 64);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 65);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 66);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 67);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 68);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 69);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 70);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 71);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 72);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 73);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 74);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 75);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 76);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 77);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 78);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 79);

	ihv[0] += a; ihv[1] += b; ihv[2] += c; ihv[3] += d; ihv[4] += e;
}

void sha1_compression_W(uint32_t ihv[5], const uint32_t W[80])
{
	uint32_t a = ihv[0], b = ihv[1], c = ihv[2], d = ihv[3], e = ihv[4];

	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 0);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 1);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 2);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 3);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 4);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 5);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 6);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 7);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 8);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 9);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 10);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 11);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 12);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 13);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 14);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(a, b, c, d, e, W, 15);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(e, a, b, c, d, W, 16);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(d, e, a, b, c, W, 17);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(c, d, e, a, b, W, 18);
	HASHCLASH_SHA1COMPRESS_ROUND1_STEP(b, c, d, e, a, W, 19);

	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 20);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 21);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 22);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 23);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 24);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 25);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 26);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 27);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 28);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 29);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 30);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 31);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 32);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 33);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 34);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(a, b, c, d, e, W, 35);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(e, a, b, c, d, W, 36);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(d, e, a, b, c, W, 37);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(c, d, e, a, b, W, 38);
	HASHCLASH_SHA1COMPRESS_ROUND2_STEP(b, c, d, e, a, W, 39);

	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 40);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 41);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 42);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 43);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 44);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 45);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 46);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 47);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 48);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 49);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 50);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 51);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 52);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 53);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 54);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(a, b, c, d, e, W, 55);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(e, a, b, c, d, W, 56);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(d, e, a, b, c, W, 57);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(c, d, e, a, b, W, 58);
	HASHCLASH_SHA1COMPRESS_ROUND3_STEP(b, c, d, e, a, W, 59);

	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 60);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 61);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 62);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 63);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 64);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 65);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 66);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 67);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 68);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 69);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 70);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 71);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 72);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 73);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 74);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(a, b, c, d, e, W, 75);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(e, a, b, c, d, W, 76);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(d, e, a, b, c, W, 77);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(c, d, e, a, b, W, 78);
	HASHCLASH_SHA1COMPRESS_ROUND4_STEP(b, c, d, e, a, W, 79);

	ihv[0] += a; ihv[1] += b; ihv[2] += c; ihv[3] += d; ihv[4] += e;
}

	void SHA1reg_Init(SHA1reg_CTX* ctx)
	{
		static const union { unsigned char bytes[4]; uint32_t value; } endianness = { { 0, 1, 2, 3 } };
		static const uint32_t littleendian = 0x03020100;
		ctx->total = 0;
		ctx->ihv[0] = 0x67452301;
		ctx->ihv[1] = 0xEFCDAB89;
		ctx->ihv[2] = 0x98BADCFE;
		ctx->ihv[3] = 0x10325476;
		ctx->ihv[4] = 0xC3D2E1F0;
		ctx->bigendian = (endianness.value != littleendian);
	}

	void SHA1reg_Update(SHA1reg_CTX* ctx, const char* buf, unsigned len)
	{
		unsigned left, fill;
		if (len == 0)
			return;

		left = ctx->total & 63;
		fill = 64 - left;

		if (left && len >= fill)
		{
			ctx->total += fill;
			memcpy(ctx->buffer + left, buf, fill);
			if (!ctx->bigendian)
				swap_bytes((uint32_t*)(ctx->buffer));
			sha1_compression(ctx->ihv, (uint32_t*)(ctx->buffer));
			buf += fill;
			len -= fill;
			left = 0;
		}
		while (len >= 64)
		{
			ctx->total += 64;
			if (!ctx->bigendian)
			{
				memcpy(ctx->buffer, buf, 64);
				swap_bytes((uint32_t*)(ctx->buffer));
				sha1_compression(ctx->ihv, (uint32_t*)(ctx->buffer));
			}
			else
				sha1_compression(ctx->ihv, (uint32_t*)(ctx->buffer));
			buf += 64;
			len -= 64;
		}
		if (len > 0)
		{
			ctx->total += len;
			memcpy(ctx->buffer + left, buf, len);
		}
	}

	static const unsigned char sha1_padding[64] =
	{
		0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
	};

	void SHA1reg_Final(unsigned char output[20], SHA1reg_CTX *ctx)
	{
		uint32_t last = ctx->total & 63;
		uint32_t padn = (last < 56) ? (56 - last) : (120 - last);
		uint64_t total;
		SHA1reg_Update(ctx, (const char*)(sha1_padding), padn);

		total = ctx->total - padn;
		total <<= 3;
		ctx->buffer[56] = (unsigned char)(total >> 56);
		ctx->buffer[57] = (unsigned char)(total >> 48);
		ctx->buffer[58] = (unsigned char)(total >> 40);
		ctx->buffer[59] = (unsigned char)(total >> 32);
		ctx->buffer[60] = (unsigned char)(total >> 24);
		ctx->buffer[61] = (unsigned char)(total >> 16);
		ctx->buffer[62] = (unsigned char)(total >> 8);
		ctx->buffer[63] = (unsigned char)(total);
		if (!ctx->bigendian)
			swap_bytes((uint32_t*)(ctx->buffer));
		sha1_compression(ctx->ihv, (uint32_t*)(ctx->buffer));
		output[0] = (unsigned char)(ctx->ihv[0] >> 24);
		output[1] = (unsigned char)(ctx->ihv[0] >> 16);
		output[2] = (unsigned char)(ctx->ihv[0] >> 8);
		output[3] = (unsigned char)(ctx->ihv[0]);
		output[4] = (unsigned char)(ctx->ihv[1] >> 24);
		output[5] = (unsigned char)(ctx->ihv[1] >> 16);
		output[6] = (unsigned char)(ctx->ihv[1] >> 8);
		output[7] = (unsigned char)(ctx->ihv[1]);
		output[8] = (unsigned char)(ctx->ihv[2] >> 24);
		output[9] = (unsigned char)(ctx->ihv[2] >> 16);
		output[10] = (unsigned char)(ctx->ihv[2] >> 8);
		output[11] = (unsigned char)(ctx->ihv[2]);
		output[12] = (unsigned char)(ctx->ihv[3] >> 24);
		output[13] = (unsigned char)(ctx->ihv[3] >> 16);
		output[14] = (unsigned char)(ctx->ihv[3] >> 8);
		output[15] = (unsigned char)(ctx->ihv[3]);
		output[16] = (unsigned char)(ctx->ihv[4] >> 24);
		output[17] = (unsigned char)(ctx->ihv[4] >> 16);
		output[18] = (unsigned char)(ctx->ihv[4] >> 8);
		output[19] = (unsigned char)(ctx->ihv[4]);
	}

} // extern "C"

using namespace std;
using namespace boost::accumulators;

inline double LogBase2(double x)
{
	return (log(x) / log(2.0));
}

inline uint32_t rotate_left(const uint32_t x, const unsigned n)
{
	return (x << n) | (x >> (32 - n));
}

template<typename RNG>
void gen_W(RNG& rng, uint32_t W[80])
{
	for (unsigned i = 0; i < 16; ++i)
		W[i] = rng();
	for (unsigned i = 16; i < 80; ++i)
		W[i] = rotate_left(W[i - 3] ^ W[i - 8] ^ W[i - 14] ^ W[i - 16], 1);
}

int main(int argc, char** argv)
{
	boost::random::random_device seeder;
	boost::random::mt19937 rng(seeder);

	SHA1_CTX ctx;
	SHA1reg_CTX ctxreg;
	unsigned char hash[20];

	vector<char> buffer;

	boost::timer timer;

	uint32_t dvmask[DVMASKSIZE], dvmask_test[DVMASKSIZE];

	cout << "Verifying ubc_check() against ubc_check_verify():" << endl;
	boost::progress_display pd(1 << 24);
	for (unsigned ll = 0; ll < (1 << 24); ++ll, ++pd)
	{
		uint32_t W[80];

		gen_W(rng, W);

		for (unsigned i = 0; i < DVMASKSIZE; ++i)
		{
			dvmask[i] = 0;
			dvmask_test[i] = ~uint32_t(0);
		}

		ubc_check(W, dvmask);
		ubc_check_verify(W, dvmask_test);

		for (unsigned i = 0; i < DVMASKSIZE; ++i)
			if (dvmask[i] != dvmask_test[i])
			{
				cerr << "Found error:" << endl
					<< "dvmask [" << i << "] = 0x" << hex << std::setw(8) << std::setfill('0') << dvmask[i] << dec << endl
					<< "dvmask2[" << i << "] = 0x" << hex << std::setw(8) << std::setfill('0') << dvmask_test[i] << dec << endl
					;
				return 1;
			}
	}
	cout << "Found no discrepancies between ubc_check() and ubc_check_verify()." << endl << endl;

	const size_t testCnt = 17;
	size_t iterCnt = 1 << 24;

	accumulator_set<double, stats<tag::mean, tag::variance, tag::median> > acc_ubc;
	accumulator_set<double, stats<tag::mean, tag::variance, tag::median> > acc_sha;
	accumulator_set<double, stats<tag::mean, tag::variance, tag::median> > acc_shawnome;

	cout << "Measuring performance of ubc_check, SHA-1 Compress and SHA-1 Compress w/out message expansion." << endl;

	uint32_t x = 0; // variable that accumulates results from inner loops to prevent them from being optimized away

	boost::progress_display perf_pd(testCnt);

	for (size_t k = 0; k < testCnt; k++, ++perf_pd)
	{
		vector< vector<uint32_t> > Wlist;
		for (size_t i = 0; i < (1 << 20); ++i)
		{
			vector<uint32_t> W(80);
			for (unsigned j = 0; j < 80; ++j)
				W[j] = rng();
			Wlist.push_back(W);
		}

		timer.restart();
		for (size_t j = 0; j < (iterCnt >> 20); ++j)
			for (size_t i = 0; i < (1 << 20); ++i)
			{
				ubc_check(&Wlist[i][0], dvmask);
				x += dvmask[0];
			}
		double ubcchecktime = timer.elapsed();

		acc_ubc(iterCnt/ubcchecktime);

		uint32_t IHV[5], M[80];
		for (unsigned i = 0; i < 5; ++i)
			IHV[i] = rng();
		for (unsigned i = 0; i < 80; ++i)
			M[i] = rng();

		timer.restart();
		for (size_t i = 0; i < iterCnt; ++i)
			sha1_compression(IHV, M);
		double shatime = timer.elapsed();

		acc_sha(iterCnt/shatime);

		timer.restart();
		for (size_t i = 0; i < (iterCnt); ++i)
			sha1_compression_W(IHV, M);
		double shawometime = timer.elapsed();

		x += IHV[0] + IHV[1] + IHV[2] + IHV[3] + IHV[4]; // prevent l
		acc_shawnome(iterCnt/shawometime);
	}

	cout << "SHA-1 compress performance: ";
	cout << "median 2^" << LogBase2(median(acc_sha)) << " sha1 compress/s ";
	cout << "mean 2^" << LogBase2(mean(acc_sha)) << " sha1 compress/s ";
	cout << "variance " << variance(acc_sha) << endl;

	cout << "UBC Check performance: ";
	cout << "median 2^" << LogBase2(median(acc_ubc)) << " ubc_check/s (" << median(acc_ubc) / mean(acc_sha) << ") ";
	cout << "mean 2^" << LogBase2(mean(acc_ubc)) << " ubc_check/s (" << mean(acc_ubc) / mean(acc_sha) << ") ";
	cout << "variance " << variance(acc_ubc) << " DVMASK:[" << x << "]" << endl;

	cout << "SHA-1 compress w/o msgexp performance: ";
	cout << "median 2^" << LogBase2(median(acc_shawnome)) << " sha1 compress no ME/s (" << median(acc_shawnome) / mean(acc_sha) << ") ";
	cout << "mean 2^" << LogBase2(mean(acc_shawnome)) << " sha1 compress no ME/s (" << mean(acc_shawnome) / mean(acc_sha) << ") ";
	cout << "variance " << variance(acc_shawnome) << endl;

	size_t testcount = 128;
	size_t mindata = 1 << 20;

	for (size_t i = 11; i < 12; ++i)
	{
		buffer.resize(mindata);

		size_t minloop = mindata / (1<<i);
		if (minloop == 0)
			minloop = 1;

		double elap_noubc = 0;
		double elap_ubc = 0;
		double elap_reg = 0;

		accumulator_set<double, stats<tag::mean, tag::variance, tag::median> > acc_shafullcd;
		accumulator_set<double, stats<tag::mean, tag::variance, tag::median> > acc_shaubc;
		accumulator_set<double, stats<tag::mean, tag::variance, tag::median> > acc_shareg;

		for (size_t n = 0; n < testCnt; n++)
		{
			for (size_t r = 0; r < testcount; ++r)
			{
				for (size_t j = 0; j < buffer.size(); ++j)
					buffer[j] = rng();

				timer.restart();
				for (size_t k = 0; k < minloop; ++k)
				{
					SHA1DCInit(&ctx);
					SHA1DCSetCallback(&ctx, nc_callback);
					SHA1DCSetUseDetectColl(&ctx, 0);
					SHA1DCUpdate(&ctx, &buffer[k * (1<<i)], 1<<i);
					SHA1DCFinal(hash, &ctx);
					x += hash[0];
				}
				elap_reg += timer.elapsed();

				timer.restart();
				for (size_t k = 0; k < minloop; ++k)
				{
					SHA1DCInit(&ctx);
					SHA1DCSetCallback(&ctx, nc_callback);
					SHA1DCSetUseUBC(&ctx, 0);
					SHA1DCUpdate(&ctx, &buffer[k * (1<<i)], 1<<i);
					SHA1DCFinal(hash, &ctx);
					x += hash[0];
				}
				elap_noubc += timer.elapsed();

				timer.restart();
				for (size_t k = 0; k < minloop; ++k)
				{
					SHA1DCInit(&ctx);
					SHA1DCSetCallback(&ctx, nc_callback);
	//				SHA1DCSetUseUBC(&ctx, 1);
					SHA1DCUpdate(&ctx, &buffer[k*(1<<i)], 1<<i);
					SHA1DCFinal(hash, &ctx);
					x += hash[0];
				}
				elap_ubc += timer.elapsed();

			}

			elap_noubc /= double(minloop)*double(testcount);
			elap_ubc /= double(minloop)*double(testcount);
			elap_reg /= double(minloop)*double(testcount);

			acc_shafullcd(elap_noubc);
			acc_shaubc(elap_ubc);
			acc_shareg(elap_reg);

		}

		cout << (1 << i) << "\t : SHA Regular Median: " << median(acc_shareg) << "s \tMean: " << mean(acc_shareg) << "s \t Variance: " << variance(acc_shareg) << "s" << endl;
		cout << (1 << i) << "\t : SHA Collision Detection w/out UBC Median: " << median(acc_shafullcd) << "s " << median(acc_shafullcd) / median(acc_shareg) << "(no ubc/reg) \tMean: " << mean(acc_shafullcd) << "s " << mean(acc_shafullcd) / mean(acc_shareg) << "(no ubc/reg) \t Variance: " << variance(acc_shafullcd) << "s" << endl;
		cout << (1 << i) << "\t : SHA Collision Detection w/ UBC Median: " << median(acc_shaubc) << "s " << median(acc_shaubc) / median(acc_shareg) << "(no ubc/reg) \tMean: " << mean(acc_shaubc) << "s " << mean(acc_shaubc) / mean(acc_shareg) << "(no ubc/reg) \t Variance: " << variance(acc_shaubc) << "s" << endl;
	}
	// finally act on x to prevent this variable to be optimized away
	if (x)
		cout << " " << flush;
	else
		cout << " ";
	return 0;

	cout << "Performing endurance test..." << endl;
	uint64_t total = 0;
	SHA1DCInit(&ctx);
	SHA1DCSetCallback(&ctx, nc_callback);
	buffer.resize(1 << 30);
	timer.restart();
	while (true)
	{
		for (size_t i = 0; i < buffer.size(); i += 4)
			(*reinterpret_cast<uint32_t*>(&buffer[i])) = rng();
		SHA1DCUpdate(&ctx, &buffer[0], buffer.size());
		total += buffer.size();
		if (timer.elapsed() > 60)
		{
			cout << "Hashed " << (total >> 30) << " GB..." << endl;
			timer.restart();
		}
	}

}