/***************************************************************************
 * Copyright 1998-2017 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxRender.                                       *
 *                                                                         *
 * Licensed under the Apache License, Version 2.0 (the "License");         *
 * you may not use this file except in compliance with the License.        *
 * You may obtain a copy of the License at                                 *
 *                                                                         *
 *     http://www.apache.org/licenses/LICENSE-2.0                          *
 *                                                                         *
 * Unless required by applicable law or agreed to in writing, software     *
 * distributed under the License is distributed on an "AS IS" BASIS,       *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.*
 * See the License for the specific language governing permissions and     *
 * limitations under the License.                                          *
 ***************************************************************************/

#include <boost/lexical_cast.hpp>

#include "luxrays/core/color/color.h"
#include "slg/samplers/sampler.h"
#include "slg/samplers/sobol.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// SobolSamplerSharedData
//------------------------------------------------------------------------------

SobolSamplerSharedData::SobolSamplerSharedData(RandomGenerator *rndGen) : SamplerSharedData() {
	rng0 = rndGen->floatValue();
	rng1 = rndGen->floatValue();
	pass = SOBOL_STARTOFFSET;
}

SamplerSharedData *SobolSamplerSharedData::FromProperties(const Properties &cfg,
		RandomGenerator *rndGen, Film *film) {
	return new SobolSamplerSharedData(rndGen);
}

//------------------------------------------------------------------------------
// Sobol sampler
//
// This sampler is based on Blender Cycles Sobol implementation.
//------------------------------------------------------------------------------

SobolSampler::SobolSampler(RandomGenerator *rnd, Film *flm,
		const FilmSampleSplatter *flmSplatter,
		SobolSamplerSharedData *samplerSharedData) : Sampler(rnd, flm, flmSplatter),
		sharedData(samplerSharedData), directions(NULL) {
}

SobolSampler::~SobolSampler() {
	delete directions;
}

void SobolSampler::RequestSamples(const u_int size) {
	directions = new u_int[size * SOBOL_BITS];
	SobolGenerateDirectionVectors(directions, size);

	passBase = sharedData->pass.fetch_add(SOBOL_THREAD_WORK_SIZE);
	passOffset = 0;
}

u_int SobolSampler::SobolDimension(const u_int index, const u_int dimension) const {
	const u_int offset = dimension * SOBOL_BITS;
	u_int result = 0;
	u_int i = index;

	for (u_int j = 0; i; i >>= 1, j++) {
		if (i & 1)
			result ^= directions[offset + j];
	}

	return result;
}

float SobolSampler::GetSample(const u_int index) {
	const u_int iResult = SobolDimension(passBase + passOffset, index);
	const float fResult = iResult * (1.f / 0xffffffffu);

	// Cranley-Patterson rotation to reduce visible regular patterns
	const float shift = (index & 1) ? sharedData->rng0 : sharedData->rng1;
	const float val = fResult + shift;

	return val - floorf(val);
}

void SobolSampler::NextSample(const vector<SampleResult> &sampleResults) {
	film->AddSampleCount(1.0);
	AddSamplesToFilm(sampleResults);

	++passOffset;
	if (passOffset >= SOBOL_THREAD_WORK_SIZE) {
		passBase = sharedData->pass.fetch_add(SOBOL_THREAD_WORK_SIZE);
		passOffset = 0;
	}
}

//------------------------------------------------------------------------------
// Static methods used by SamplerRegistry
//------------------------------------------------------------------------------

Properties SobolSampler::ToProperties(const Properties &cfg) {
	return Properties() <<
			cfg.Get(GetDefaultProps().Get("sampler.type"));
}

Sampler *SobolSampler::FromProperties(const Properties &cfg, RandomGenerator *rndGen,
		Film *film, const FilmSampleSplatter *flmSplatter, SamplerSharedData *sharedData) {
	return new SobolSampler(rndGen, film, flmSplatter, (SobolSamplerSharedData *)sharedData);
}

slg::ocl::Sampler *SobolSampler::FromPropertiesOCL(const Properties &cfg) {
	slg::ocl::Sampler *oclSampler = new slg::ocl::Sampler();

	oclSampler->type = slg::ocl::SOBOL;

	return oclSampler;
}

const Properties &SobolSampler::GetDefaultProps() {
	static Properties props = Properties() <<
			Sampler::GetDefaultProps() <<
			Property("sampler.type")(GetObjectTag());

	return props;
}


#pragma once

#include <list>
#include <mutex>

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include "rcvio/macros.h"

namespace rcvio
{
    struct ImuData
    {
        Eigen::Vector3d wm_;
        Eigen::Vector3d am_;
        double ts_;
        double dt_;

        ImuData()
        {
            wm_.setZero();
            am_.setZero();
            ts_ = 0;
            dt_ = 0;
        }

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    struct ImageData
    {
        cv::Mat image_;
        double ts_;

        ImageData()
        {
            image_ = cv::Mat();
            ts_ = 0;
        }
    };

    class InputBuffer
    {
    public:
        POINTER_TYPEDEFS(InputBuffer);

        InputBuffer() = default;

        void pushImuData(ImuData *data);
        void pushImageData(ImageData *data);

        bool getMeasurements(double time_offset, std::pair<ImageData *, std::list<ImuData *>> &measurements);

    protected:
        std::list<ImuData *> imu_fifo_;
        std::list<ImageData *> image_fifo_;

        std::mutex mutex_;
    };
}


#ifndef pear_int_hpp
#define pear_int_hpp

#include <Eigen/Core>
#include <Eigen/Sparse>

// TYPE DEFINITIONS
typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd Vec;
typedef Eigen::MatrixXi MatI;
typedef Eigen::VectorXi VecI;
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SpMat;

namespace pear {
Mat int_func(Vec &xp, Vec &yp, MatI &t);
Mat int_func_block(Vec &xp, Vec &yp);
double triangle_area(Vec &xp, Vec &yp);
} // namespace pear

#endif


//
//  mat_io.h
//  SequentialRandomForest
//
//  Created by jimmy on 2017-07-01.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#ifndef __SequentialRandomForest__mat_io__
#define __SequentialRandomForest__mat_io__

// matlab input output
#include <stdio.h>
#include <Eigen/Dense>
#include <string>
#include <vector>

namespace matio {

    // suppport Eigen::MatrixXd, Eigen::MatrixXf and Eigen::MatrixXi
    template<class matrixT>
    bool readMatrix(const char *file_name, const char *var_name, matrixT & data, bool verbose = true);

    // support Eigen::MatrixXd, Eigen::MatrixXf
    // In .mat file, both are in double format
    template<class matrixT>
    bool writeMatrix(const char *file_name, const char *var_name, const matrixT& data);

    template<class matrixT>
    bool writeMultipleMatrix(const char *file_name,
                             const std::vector<std::string>& var_name,
                             const std::vector<matrixT>& data);

} // name space matio

#endif /* defined(__SequentialRandomForest__mat_io__) */


/*!
 * Copyright (c) 2020 Microsoft Corporation. All rights reserved.
 * Licensed under the MIT License. See LICENSE file in the project root for license information.
 */
#include "linear_tree_learner.h"

#include <algorithm>

#ifndef LGB_R_BUILD
#include <Eigen/Dense>
#endif  // !LGB_R_BUILD

namespace LightGBM {

void LinearTreeLearner::Init(const Dataset* train_data, bool is_constant_hessian) {
  SerialTreeLearner::Init(train_data, is_constant_hessian);
  LinearTreeLearner::InitLinear(train_data, config_->num_leaves);
}

void LinearTreeLearner::InitLinear(const Dataset* train_data, const int max_leaves) {
  leaf_map_ = std::vector<int>(train_data->num_data(), -1);
  contains_nan_ = std::vector<int8_t>(train_data->num_features(), 0);
  // identify features containing nans
#pragma omp parallel for schedule(static)
  for (int feat = 0; feat < train_data->num_features(); ++feat) {
    auto bin_mapper = train_data_->FeatureBinMapper(feat);
    if (bin_mapper->bin_type() == BinType::NumericalBin) {
      const float* feat_ptr = train_data_->raw_index(feat);
      for (int i = 0; i < train_data->num_data(); ++i) {
        if (std::isnan(feat_ptr[i])) {
          contains_nan_[feat] = 1;
          break;
        }
      }
    }
  }
  for (int feat = 0; feat < train_data->num_features(); ++feat) {
    if (contains_nan_[feat]) {
      any_nan_ = true;
      break;
    }
  }
  // preallocate the matrix used to calculate linear model coefficients
  int max_num_feat = std::min(max_leaves, train_data_->num_numeric_features());
  XTHX_.clear();
  XTg_.clear();
  for (int i = 0; i < max_leaves; ++i) {
    // store only upper triangular half of matrix as an array, in row-major order
    // this requires (max_num_feat + 1) * (max_num_feat + 2) / 2 entries (including the constant terms of the regression)
    // we add another 8 to ensure cache lines are not shared among processors
    XTHX_.push_back(std::vector<float>((max_num_feat + 1) * (max_num_feat + 2) / 2 + 8, 0));
    XTg_.push_back(std::vector<float>(max_num_feat + 9, 0.0));
  }
  XTHX_by_thread_.clear();
  XTg_by_thread_.clear();
  int max_threads = omp_get_max_threads();
  for (int i = 0; i < max_threads; ++i) {
    XTHX_by_thread_.push_back(XTHX_);
    XTg_by_thread_.push_back(XTg_);
  }
}

Tree* LinearTreeLearner::Train(const score_t* gradients, const score_t *hessians, bool is_first_tree) {
  Common::FunctionTimer fun_timer("SerialTreeLearner::Train", global_timer);
  gradients_ = gradients;
  hessians_ = hessians;
  int num_threads = OMP_NUM_THREADS();
  if (share_state_->num_threads != num_threads && share_state_->num_threads > 0) {
    Log::Warning(
        "Detected that num_threads changed during training (from %d to %d), "
        "it may cause unexpected errors.",
        share_state_->num_threads, num_threads);
  }
  share_state_->num_threads = num_threads;

  // some initial works before training
  BeforeTrain();

  auto tree = std::unique_ptr<Tree>(new Tree(config_->num_leaves, true, true));
  auto tree_ptr = tree.get();
  constraints_->ShareTreePointer(tree_ptr);

  // root leaf
  int left_leaf = 0;
  int cur_depth = 1;
  // only root leaf can be splitted on first time
  int right_leaf = -1;

  int init_splits = ForceSplits(tree_ptr, &left_leaf, &right_leaf, &cur_depth);

  for (int split = init_splits; split < config_->num_leaves - 1; ++split) {
    // some initial works before finding best split
    if (BeforeFindBestSplit(tree_ptr, left_leaf, right_leaf)) {
      // find best threshold for every feature
      FindBestSplits(tree_ptr);
    }
    // Get a leaf with max split gain
    int best_leaf = static_cast<int>(ArrayArgs<SplitInfo>::ArgMax(best_split_per_leaf_));
    // Get split information for best leaf
    const SplitInfo& best_leaf_SplitInfo = best_split_per_leaf_[best_leaf];
    // cannot split, quit
    if (best_leaf_SplitInfo.gain <= 0.0) {
      Log::Warning("No further splits with positive gain, best gain: %f", best_leaf_SplitInfo.gain);
      break;
    }
    // split tree with best leaf
    Split(tree_ptr, best_leaf, &left_leaf, &right_leaf);
    cur_depth = std::max(cur_depth, tree->leaf_depth(left_leaf));
  }

  bool has_nan = false;
  if (any_nan_) {
    for (int i = 0; i < tree->num_leaves() - 1 ; ++i) {
      if (contains_nan_[tree_ptr->split_feature_inner(i)]) {
        has_nan = true;
        break;
      }
    }
  }

  GetLeafMap(tree_ptr);

  if (has_nan) {
    CalculateLinear<true>(tree_ptr, false, gradients_, hessians_, is_first_tree);
  } else {
    CalculateLinear<false>(tree_ptr, false, gradients_, hessians_, is_first_tree);
  }

  Log::Debug("Trained a tree with leaves = %d and max_depth = %d", tree->num_leaves(), cur_depth);
  return tree.release();
}

Tree* LinearTreeLearner::FitByExistingTree(const Tree* old_tree, const score_t* gradients, const score_t *hessians) const {
  auto tree = SerialTreeLearner::FitByExistingTree(old_tree, gradients, hessians);
  bool has_nan = false;
  if (any_nan_) {
    for (int i = 0; i < tree->num_leaves() - 1 ; ++i) {
      if (contains_nan_[train_data_->InnerFeatureIndex(tree->split_feature(i))]) {
        has_nan = true;
        break;
      }
    }
  }
  GetLeafMap(tree);
  if (has_nan) {
    CalculateLinear<true>(tree, true, gradients, hessians, false);
  } else {
    CalculateLinear<false>(tree, true, gradients, hessians, false);
  }
  return tree;
}

Tree* LinearTreeLearner::FitByExistingTree(const Tree* old_tree, const std::vector<int>& leaf_pred,
                                           const score_t* gradients, const score_t *hessians) const {
  data_partition_->ResetByLeafPred(leaf_pred, old_tree->num_leaves());
  return LinearTreeLearner::FitByExistingTree(old_tree, gradients, hessians);
}

void LinearTreeLearner::GetLeafMap(Tree* tree) const {
  std::fill(leaf_map_.begin(), leaf_map_.end(), -1);
  // map data to leaf number
  const data_size_t* ind = data_partition_->indices();
#pragma omp parallel for schedule(dynamic)
  for (int i = 0; i < tree->num_leaves(); ++i) {
    data_size_t idx = data_partition_->leaf_begin(i);
    for (int j = 0; j < data_partition_->leaf_count(i); ++j) {
      leaf_map_[ind[idx + j]] = i;
    }
  }
}

#ifdef LGB_R_BUILD
template<bool HAS_NAN>
void LinearTreeLearner::CalculateLinear(Tree* tree, bool is_refit, const score_t* gradients, const score_t* hessians, bool is_first_tree) const {
  Log::Fatal("Linear tree learner does not work with R package.");
}
#else
template<bool HAS_NAN>
void LinearTreeLearner::CalculateLinear(Tree* tree, bool is_refit, const score_t* gradients, const score_t* hessians, bool is_first_tree) const {
  tree->SetIsLinear(true);
  int num_leaves = tree->num_leaves();
  int num_threads = OMP_NUM_THREADS();
  if (is_first_tree) {
    for (int leaf_num = 0; leaf_num < num_leaves; ++leaf_num) {
      tree->SetLeafConst(leaf_num, tree->LeafOutput(leaf_num));
    }
    return;
  }

  // calculate coefficients using the method described in Eq 3 of https://arxiv.org/pdf/1802.05640.pdf
  // the coefficients vector is given by
  // - (X_T * H * X + lambda) ^ (-1) * (X_T * g)
  // where:
  // X is the matrix where the first column is the feature values and the second is all ones,
  // H is the diagonal matrix of the hessian,
  // lambda is the diagonal matrix with diagonal entries equal to the regularisation term linear_lambda
  // g is the vector of gradients
  // the subscript _T denotes the transpose

  // create array of pointers to raw data, and coefficient matrices, for each leaf
  std::vector<std::vector<int>> leaf_features;
  std::vector<int> leaf_num_features;
  std::vector<std::vector<const float*>> raw_data_ptr;
  int max_num_features = 0;
  for (int i = 0; i < num_leaves; ++i) {
    std::vector<int> raw_features;
    if (is_refit) {
      raw_features = tree->LeafFeatures(i);
    } else {
      raw_features = tree->branch_features(i);
    }
    std::sort(raw_features.begin(), raw_features.end());
    auto new_end = std::unique(raw_features.begin(), raw_features.end());
    raw_features.erase(new_end, raw_features.end());
    std::vector<int> numerical_features;
    std::vector<const float*> data_ptr;
    for (size_t j = 0; j < raw_features.size(); ++j) {
      int feat = train_data_->InnerFeatureIndex(raw_features[j]);
      auto bin_mapper = train_data_->FeatureBinMapper(feat);
      if (bin_mapper->bin_type() == BinType::NumericalBin) {
        numerical_features.push_back(feat);
        data_ptr.push_back(train_data_->raw_index(feat));
      }
    }
    leaf_features.push_back(numerical_features);
    raw_data_ptr.push_back(data_ptr);
    leaf_num_features.push_back(numerical_features.size());
    if (static_cast<int>(numerical_features.size()) > max_num_features) {
      max_num_features = numerical_features.size();
    }
  }
  // clear the coefficient matrices
#pragma omp parallel for schedule(static)
  for (int i = 0; i < num_threads; ++i) {
    for (int leaf_num = 0; leaf_num < num_leaves; ++leaf_num) {
      int num_feat = leaf_features[leaf_num].size();
      std::fill(XTHX_by_thread_[i][leaf_num].begin(), XTHX_by_thread_[i][leaf_num].begin() + (num_feat + 1) * (num_feat + 2) / 2, 0);
      std::fill(XTg_by_thread_[i][leaf_num].begin(), XTg_by_thread_[i][leaf_num].begin() + num_feat + 1, 0);
    }
  }
#pragma omp parallel for schedule(static)
  for (int leaf_num = 0; leaf_num < num_leaves; ++leaf_num) {
    int num_feat = leaf_features[leaf_num].size();
    std::fill(XTHX_[leaf_num].begin(), XTHX_[leaf_num].begin() + (num_feat + 1) * (num_feat + 2) / 2, 0);
    std::fill(XTg_[leaf_num].begin(), XTg_[leaf_num].begin() + num_feat + 1, 0);
  }
  std::vector<std::vector<int>> num_nonzero;
  for (int i = 0; i < num_threads; ++i) {
    if (HAS_NAN) {
      num_nonzero.push_back(std::vector<int>(num_leaves, 0));
    }
  }
  OMP_INIT_EX();
#pragma omp parallel if (num_data_ > 1024)
  {
    std::vector<float> curr_row(max_num_features + 1);
    int tid = omp_get_thread_num();
#pragma omp for schedule(static)
    for (int i = 0; i < num_data_; ++i) {
      OMP_LOOP_EX_BEGIN();
      int leaf_num = leaf_map_[i];
      if (leaf_num < 0) {
        continue;
      }
      bool nan_found = false;
      int num_feat = leaf_num_features[leaf_num];
      for (int feat = 0; feat < num_feat; ++feat) {
        if (HAS_NAN) {
          float val = raw_data_ptr[leaf_num][feat][i];
          if (std::isnan(val)) {
            nan_found = true;
            break;
          }
          num_nonzero[tid][leaf_num] += 1;
          curr_row[feat] = val;
        } else {
          curr_row[feat] = raw_data_ptr[leaf_num][feat][i];
        }
      }
      if (HAS_NAN) {
        if (nan_found) {
          continue;
        }
      }
      curr_row[num_feat] = 1.0;
      double h = hessians[i];
      double g = gradients[i];
      int j = 0;
      for (int feat1 = 0; feat1 < num_feat + 1; ++feat1) {
        double f1_val = curr_row[feat1];
        XTg_by_thread_[tid][leaf_num][feat1] += f1_val * g;
        f1_val *= h;
        for (int feat2 = feat1; feat2 < num_feat + 1; ++feat2) {
          XTHX_by_thread_[tid][leaf_num][j] += f1_val * curr_row[feat2];
          ++j;
        }
      }
      OMP_LOOP_EX_END();
    }
  }
  OMP_THROW_EX();
  auto total_nonzero = std::vector<int>(tree->num_leaves());
  // aggregate results from different threads
  for (int tid = 0; tid < num_threads; ++tid) {
#pragma omp parallel for schedule(static)
    for (int leaf_num = 0; leaf_num < num_leaves; ++leaf_num) {
      int num_feat = leaf_features[leaf_num].size();
      for (int j = 0; j < (num_feat + 1) * (num_feat + 2) / 2; ++j) {
        XTHX_[leaf_num][j] += XTHX_by_thread_[tid][leaf_num][j];
      }
      for (int feat1 = 0; feat1 < num_feat + 1; ++feat1) {
        XTg_[leaf_num][feat1] += XTg_by_thread_[tid][leaf_num][feat1];
      }
      if (HAS_NAN) {
        total_nonzero[leaf_num] += num_nonzero[tid][leaf_num];
      }
    }
  }
  if (!HAS_NAN) {
    for (int leaf_num = 0; leaf_num < num_leaves; ++leaf_num) {
      total_nonzero[leaf_num] = data_partition_->leaf_count(leaf_num);
    }
  }
  double shrinkage = tree->shrinkage();
  double decay_rate = config_->refit_decay_rate;
  // copy into eigen matrices and solve
#pragma omp parallel for schedule(static)
  for (int leaf_num = 0; leaf_num < num_leaves; ++leaf_num) {
    if (total_nonzero[leaf_num] < static_cast<int>(leaf_features[leaf_num].size()) + 1) {
      if (is_refit) {
        double old_const = tree->LeafConst(leaf_num);
        tree->SetLeafConst(leaf_num, decay_rate * old_const + (1.0 - decay_rate) * tree->LeafOutput(leaf_num) * shrinkage);
        tree->SetLeafCoeffs(leaf_num, std::vector<double>(leaf_features[leaf_num].size(), 0));
        tree->SetLeafFeaturesInner(leaf_num, leaf_features[leaf_num]);
      } else {
        tree->SetLeafConst(leaf_num, tree->LeafOutput(leaf_num));
      }
      continue;
    }
    int num_feat = leaf_features[leaf_num].size();
    Eigen::MatrixXd XTHX_mat(num_feat + 1, num_feat + 1);
    Eigen::MatrixXd XTg_mat(num_feat + 1, 1);
    int j = 0;
    for (int feat1 = 0; feat1 < num_feat + 1; ++feat1) {
      for (int feat2 = feat1; feat2 < num_feat + 1; ++feat2) {
        XTHX_mat(feat1, feat2) = XTHX_[leaf_num][j];
        XTHX_mat(feat2, feat1) = XTHX_mat(feat1, feat2);
        if ((feat1 == feat2) && (feat1 < num_feat)) {
          XTHX_mat(feat1, feat2) += config_->linear_lambda;
        }
        ++j;
      }
      XTg_mat(feat1) = XTg_[leaf_num][feat1];
    }
    Eigen::MatrixXd coeffs = - XTHX_mat.fullPivLu().inverse() * XTg_mat;
    std::vector<double> coeffs_vec;
    std::vector<int> features_new;
    std::vector<double> old_coeffs = tree->LeafCoeffs(leaf_num);
    for (size_t i = 0; i < leaf_features[leaf_num].size(); ++i) {
      if (is_refit) {
        features_new.push_back(leaf_features[leaf_num][i]);
        coeffs_vec.push_back(decay_rate * old_coeffs[i] + (1.0 - decay_rate) * coeffs(i) * shrinkage);
      } else {
        if (coeffs(i) < -kZeroThreshold || coeffs(i) > kZeroThreshold) {
          coeffs_vec.push_back(coeffs(i));
          int feat = leaf_features[leaf_num][i];
          features_new.push_back(feat);
        }
      }
    }
    // update the tree properties
    tree->SetLeafFeaturesInner(leaf_num, features_new);
    std::vector<int> features_raw(features_new.size());
    for (size_t i = 0; i < features_new.size(); ++i) {
      features_raw[i] = train_data_->RealFeatureIndex(features_new[i]);
    }
    tree->SetLeafFeatures(leaf_num, features_raw);
    tree->SetLeafCoeffs(leaf_num, coeffs_vec);
    if (is_refit) {
      double old_const = tree->LeafConst(leaf_num);
      tree->SetLeafConst(leaf_num, decay_rate * old_const + (1.0 - decay_rate) * coeffs(num_feat) * shrinkage);
    } else {
      tree->SetLeafConst(leaf_num, coeffs(num_feat));
    }
  }
}
#endif  // LGB_R_BUILD
}  // namespace LightGBM


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

#include <boost/property_tree/ptree.hpp>

#include "clotho/cuda/crossover/crossover_kernels.hpp"
#include "clotho/cuda/helpers/scaled_mean_helper.hpp"

#include "clotho/cuda/curand_state_pool.hpp"

#include "clotho/cuda/distributions/poisson_distribution.hpp"
#include "clotho/recombination/recombination_rate_parameter.hpp"

#ifndef CROSSOVER_VERSION
#define CROSSOVER_VERSION 1
#endif  // CROSSOVER_VERSION

template < class PopulationType >
class CrossoverGenerator {
public:
    typedef PopulationType                                                      population_type;
    typedef typename population_type::allele_space_type::device_space_type      allele_space_type;

    typedef typename population_type::real_type             real_type;
    typedef typename population_type::int_type              int_type;
    typedef typename population_type::order_tag_type        order_tag_type;

    typedef clotho::cuda::curand_state_pool                 state_pool_type;

    typedef recombination_rate_parameter< real_type >       recombination_rate_type;

    typedef poisson_cdf< real_type, 32 >                    poisson_type;
    typedef scaled_mean_helper< real_type, order_tag_type > scaled_mean_type;

    typedef clotho::utility::algo_version< CROSSOVER_VERSION > algorithm_version_type;
    typedef kernel_exec< xover_config< order_tag_type, CROSSOVER_VERSION > > exec_type;

    CrossoverGenerator( boost::property_tree::ptree & config ) :
        m_recombination_rate( config )
        , ver( NULL )
        , m_exec( config )
        , m_blocks( 1, 1, 1)
        , m_threads( 1, 1, 1 )
    {
        parse_configuration( config );

        initialize();
    }

    void operator()( population_type * pop ) {
        CHECK_LAST_KERNEL_EXEC

//        std::cout << "Blocks: {" << m_blocks.x << ", " << m_blocks.y << ", " << m_blocks.z << "}" << std::endl;
//        std::cout << "Threads: {" << m_threads.x << ", " << m_threads.y << ", " << m_threads.z << "}" << std::endl;
        run_crossover_kernel( pop, ver );

        CHECK_LAST_KERNEL_EXEC
    }

    void operator()( population_type * pop, device_event_pool_space< real_type, int_type > * event_pool ) {
        CHECK_LAST_KERNEL_EXEC

        build_crossover_mask_kernel<<< m_blocks, m_threads >>>( pop->alleles.get_device_space(),
            pop->sequences.get_device_space(),
            event_pool );

        CHECK_LAST_KERNEL_EXEC
    }

    virtual ~CrossoverGenerator() {
        cudaFree( dPoisCDF );
    }

protected:

    void parse_configuration( boost::property_tree::ptree & config ) {
        state_pool_type::getInstance()->initialize( config );

        m_blocks.x = state_pool_type::getInstance()->get_max_blocks();
        m_threads.x = state_pool_type::getInstance()->get_max_threads();

        m_exec( m_blocks, m_threads );
    }

    template < unsigned char V >
    void run_crossover_kernel( population_type * pop, clotho::utility::algo_version< V > * v ) {
        crossover_kernel<<< m_blocks, m_threads >>>( state_pool_type::getInstance()->get_device_states()
                                                , pop->alleles.get_device_space()
                                                , pop->free_space
                                                , dPoisCDF
                                                , pop->sequences.get_device_space()
                                                , v );

    }

    void run_crossover_kernel( population_type * pop, clotho::utility::algo_version< 5 > * v ) {
        m_blocks.x = state_pool_type::getInstance()->get_total_states();
        m_blocks.y = 1;
        m_blocks.z = 1;

        m_threads.x = 32;
        m_threads.y = 32;
        m_threads.z = 1;

        crossover_kernel<<< m_blocks, m_threads >>>( state_pool_type::getInstance()->get_device_states()
                                                , pop->alleles.get_device_space()
                                                , pop->free_space
                                                , dPoisCDF
                                                , pop->sequences.get_device_space()
                                                , v);
    }

    void initialize() {

        assert( cudaMalloc( (void **) &dPoisCDF, sizeof( poisson_type) ) == cudaSuccess );

        real_type lambda_adj = compute_lambda( m_recombination_rate.m_rho, (order_tag_type *) NULL, (clotho::utility::algo_version< CROSSOVER_VERSION > *) NULL );

        make_poisson_cdf_maxk32<<< 1, 32 >>>( dPoisCDF, lambda_adj, m_recombination_rate.m_rho );
    }

    template < class Tag0, class Tag1 >
    real_type compute_lambda( real_type mean, Tag0 * t0, Tag1 * t1 ) {
        return mean;
    }

    template < unsigned char V >
    real_type compute_lambda( real_type mean, unordered_tag * p, clotho::utility::algo_version< V > * v ) {
        std::cerr << "Scaling mean by 1/" << allele_space_type::ALIGNMENT_SIZE << std::endl;
        return (mean / (real_type) allele_space_type::ALIGNMENT_SIZE);
    }

    real_type compute_lambda( real_type mean, unordered_tag * p, clotho::utility::algo_version< 2 > * v ) {
        return (mean / 32.0);
    }

    real_type compute_lambda( real_type mean, unordered_tag * p, clotho::utility::algo_version< 3 > * v ) {
        return (mean / 32.0);
    }

    template < unsigned char V >
    real_type compute_lambda( real_type mean, unit_ordered_tag< int_type > * p, clotho::utility::algo_version< V > * ver ) {
        return (mean / 32.0);
    }

    poisson_type * dPoisCDF;
    recombination_rate_type     m_recombination_rate;
    algorithm_version_type      * ver;
    exec_type                   m_exec;
    dim3                        m_blocks, m_threads;
};

#endif  // CROSSOVER_GENERATOR_HPP_


#ifndef __SMT_HPP_DEFINED__
#define __SMT_HPP_DEFINED__

#include <boost/shared_ptr.hpp>
#include <ilasynth/ast.hpp>
#include <ilasynth/ast/hash.hpp>
#include <ilasynth/exception.hpp>
#include <ilasynth/util.hpp>
#include <unordered_map>
#include <z3++.h>

namespace ilasynth {
// A function object that converts nodes into Z3 expressions.
class Z3ExprAdapter {
public:
  // Define types.
  typedef std::unordered_map<const Node*, z3::expr, decltype(&nodeHash),
                             decltype(&nodeEqual)>
      expr_map_t;
  // simplify flag.
  bool simplify;

protected:
  // These are used during memoization.
  expr_map_t exprmap;
  expr_map_t cnstmap;

  // The underlying Z3 context.
  z3::context& c;
  // The "suffix" (used only for synthesis operators).
  std::string suffix;

  /// Added to every name
  std::string name_suffix;

  // Utility function extracts the Z3 expression corresponding
  // to the i-th argument of the Node n.
  z3::expr _getArg(const expr_map_t& m, const Node* n, int i);
  // This extracts expressions from exprmap.
  z3::expr getArgExpr(const Node* n, int i) { return _getArg(exprmap, n, i); }
  // This extracts expressions from cnstmap.
  z3::expr getArgCnst(const Node* n, int i) { return _getArg(cnstmap, n, i); }

  // These functions populate exprmap and cnstmap.
  void _populateExprMap(const Node* n);
  void _populateCnstMap(const Node* n);

  // Convert a boolean variable into a Z3 expression.
  virtual z3::expr getBoolVarExpr(const BoolVar* bv);
  // Convert a boolean op into a Z3 expression.
  virtual z3::expr getBoolOpExpr(const BoolOp* op);
  // for booleans
  virtual z3::expr getChoiceExpr(const BoolChoice* op);

  // Convert a bitvector variable into a Z3 expression.
  virtual z3::expr getBitvectorVarExpr(const BitvectorVar* bvv);
  // Convert a bitvector op into a Z3 expression.
  virtual z3::expr getBvOpExpr(const BitvectorOp* op);
  // Convert a choice op into a Z3 expression. (bitvectors)
  virtual z3::expr getChoiceExpr(const BitvectorChoice* op);
  // Convert a in-range expression to Z3.
  virtual z3::expr getBVInRangeExpr(const BVInRange* op);
  // Constraints for the in-range operator.
  virtual z3::expr getBVInRangeCnst(const BVInRange* op);

  // Convert a memory var into a Z3 expression.
  virtual z3::expr getMemVarExpr(const MemVar* mv);
  // Convert a mem write expression into Z3.
  virtual z3::expr getMemOpExpr(const MemOp* mw);
  // for memories.
  virtual z3::expr getChoiceExpr(const MemChoice* op);

  // Convert a function variable into a Z3 expression.
  virtual z3::expr getFuncVarExpr(const FuncVar* fv);

public:
  // Constructors.
  Z3ExprAdapter(z3::context& c, const std::string& suffix);
  Z3ExprAdapter(z3::context& c, const char* suffix);
  // Destructor.
  virtual ~Z3ExprAdapter();

  // This is used by depthFirstVisit.
  virtual void operator()(const Node* n);

  // This will call node->depthFirstVisit.
  z3::expr getExpr(const Node* n);
  // As will this.
  z3::expr getCnst(const Node* n);

  // return the context.
  z3::context& ctx() const { return c; }

  // clear the memo.
  void clear() {
    exprmap.clear();
    cnstmap.clear();
  }

  // Extract a string representation of the numeric value
  // of node r in the model m.
  std::string extractNumeralString(z3::model& m, const Node* r);
  // Extract the integer value of node r in model m.
  int getNumeralInt(z3::model& m, const Node* r);
  // Extract the arbitrary precision int value of node r in model m.
  mp_int_t getNumeralCppInt(z3::model& m, const Node* r);
  // Extract the boolean value of node r in model m.
  bool getBoolValue(z3::model& m, const Node* r);
  // Return the value of the ith choice boolean
  template <typename T>
  bool getChoiceBool(z3::model& m, const ChoiceExpr<T>* op, int i) {
    using namespace z3;
    std::string name = op->getChoiceVarName(i) + suffix;
    expr ci = c.bool_const(name.c_str());
    z3::expr mi = m.eval(ci, true);
    Z3_lbool bi = Z3_get_bool_value(c, mi);
    // FIXME avoid macro in template
    // ILA_ASSERT(bi != Z3_L_UNDEF, "Unable to extract bool from model.");
    return bi == Z3_L_TRUE;
  }

  void setNameSuffix(const std::string& ns) { name_suffix = ns; }
  const std::string& getNameSuffix() const { return name_suffix; }

private:
  // Helper to convert choices into z3 expressions.
  template <typename T> z3::expr _getChoiceExpr(const ChoiceExpr<T>* op) {
    using namespace z3;

    expr vi_ = getArgExpr(op, 0);
    unsigned nargs = op->nArgs();
    for (unsigned i = 1; i != nargs; i++) {
      std::string name = op->getChoiceVarName(i - 1) + suffix;
      expr ci = c.bool_const(name.c_str());
      expr vi = getArgExpr(op, i);
      expr vi_next = ite(ci, vi, vi_);
      vi_ = vi_next;
    }
    return vi_;
  }
};

class DistInput;

// The function object we use during synthesis to rewrite expressions.
class Z3ExprRewritingAdapter : public Z3ExprAdapter {
protected:
  // The distinguishing input this is based on.
  const DistInput* distInp;
  // Convert a boolean variable into a Z3 expression.
  virtual z3::expr getBoolVarExpr(const BoolVar* bv);
  // Convert a bitvector variable into a Z3 expression.
  virtual z3::expr getBitvectorVarExpr(const BitvectorVar* bvv);
  // Convert a mem var into Z3.
  virtual z3::expr getMemVarExpr(const MemVar* mv);

public:
  // Constructors.
  Z3ExprRewritingAdapter(z3::context& c, const DistInput* di,
                         const std::string& suffix);
  Z3ExprRewritingAdapter(z3::context& c, const DistInput* di,
                         const char* suffix);
  // Destructor.
  ~Z3ExprRewritingAdapter();
  // wrapper over getExpr and getCnst
  z3::expr getIOCnst(const Node* n, const py::object& result);
};

class Z3FixedpointAdapter : public Z3ExprAdapter {
public:
  Z3FixedpointAdapter(z3::context& c, const std::string& suffix)
      : Z3ExprAdapter(c, suffix) {}

  ~Z3FixedpointAdapter() {}
};
} // namespace ilasynth

#endif


//
// Created by joshua on 9/29/17.
//
#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <mps/planner/pushing/OraclePushPlanner.h>
#include <mps/planner/pushing/oracle/DataGenerator.h>
#include <mps/planner/util/Random.h>
#include <mps/planner/util/yaml/OracleParsing.h>
#include <ompl/base/samplers/UniformValidStateSampler.h>

using namespace mps::planner::pushing::oracle;

DataGenerator::Parameters::Parameters()
{
    obj_orientation_sigma = boost::math::float_constants::pi / 18.0f; // 10 degrees
    obj_position_sigma = 0.01; // 1 cm
    mass_sigma = 0.002;
    friction_sigma = 0.004;
}

DataGenerator::DataGenerator(::ompl::control::SpaceInformationPtr si,
    mps::planner::ompl::control::SimEnvStatePropagatorPtr state_prop,
    sim_env::WorldPtr world,
    const std::string& robot_name,
    const std::string& obj_name,
    const Parameters& params)
    : _world(world)
    , _space_information(si)
    , _state_propagator(state_prop)
{
    _robot = world->getRobot(robot_name);
    _object = world->getObject(obj_name);
    auto state_space = std::dynamic_pointer_cast<ompl::state::SimEnvWorldStateSpace>(si->getStateSpace());
    int robot_id = state_space->getObjectIndex(_robot->getName());
    assert(robot_id >= 0);
    _robot_id = (unsigned int)robot_id;
    int object_id = state_space->getObjectIndex(_object->getName());
    assert(object_id >= 0);
    _object_id = (unsigned int)object_id;
    assert(_robot);
    assert(_object);
    computeMaxDistance();
    _state_sigma.resize(_object->getNumActiveDOFs());
    _state_sigma.setZero();
    assert(_state_sigma.size() >= 3);
    _state_sigma[0] = params.obj_position_sigma;
    _state_sigma[1] = params.obj_position_sigma;
    _state_sigma[2] = params.obj_orientation_sigma;
    _mass_stddev = params.mass_sigma;
    _friction_stddev = params.friction_sigma;
}

DataGenerator::~DataGenerator() = default;

void DataGenerator::generateData(const std::string& file_name,
    unsigned int num_samples,
    const std::string& annotation,
    bool deterministic,
    unsigned int num_noise_samples)
{
    ::ompl::control::ControlSamplerPtr control_sampler = _space_information->allocControlSampler();
    auto state_space = std::dynamic_pointer_cast<mps::planner::ompl::state::SimEnvWorldStateSpace>(_space_information->getStateSpace());
    auto state_sampler = _space_information->getStateSpace()->allocStateSampler();
    auto validity_checker = _space_information->getStateValidityChecker();
    ::ompl::base::State* mean_state = _space_information->allocState();
    ::ompl::base::State* noisy_state = _space_information->allocState();
    ::ompl::base::State* new_state = _space_information->allocState();
    ::ompl::control::Control* control = _space_information->allocControl();
    mps::planner::util::serialize::OracleDataDumper data_dumper;
    data_dumper.setFile(file_name);
    data_dumper.openFile();
    if (deterministic) {
        num_noise_samples = 1;
    }
    unsigned int i = 0;
    while (i < num_samples) {
        //        _world->getLogger()->logInfo(boost::format("Sampling sample %i") % i, "[DataGenerator]");
        bool has_state = sampleValidState(mean_state, state_sampler, validity_checker);
        if (not has_state) {
            _world->getLogger()->logWarn("Failed to sample a valid state, we might end up in a infinite loop here!");
            continue;
        }
        control_sampler->sample(control);
        _world->getLogger()->logDebug("Sampled state");
        _space_information->copyState(noisy_state, mean_state);
        for (unsigned int j = 0; j < num_noise_samples; ++j) {
            if (not deterministic) {
                applyNoise(mean_state, noisy_state);
                modifyDynamics();
            }
            auto world_state = static_cast<mps::planner::ompl::state::SimEnvWorldState*>(noisy_state);
            state_space->setToState(_world, world_state);
            if (!_world->isPhysicallyFeasible()) {
                _world->getLogger()->logErr("The start state is now physically infeasible....", "[Fuck this]");
                continue;
            }
            bool success = _state_propagator->propagate(noisy_state, control, new_state);
            if (not deterministic) {
                restoreDynamics();
            }
            _world->getLogger()->logDebug("Propagated state");
            if (not success) {
                _world->getLogger()->logDebug("State propagation failed, skipping");
                continue;
            }
            data_dumper.saveData(noisy_state, new_state, control, annotation);
        }
        ++i;
    }
    data_dumper.closeFile();
    _space_information->freeState(mean_state);
    _space_information->freeState(new_state);
    _space_information->freeState(noisy_state);
    _space_information->freeControl(control);
}

void DataGenerator::evaluateOracle(mps::planner::ompl::state::goal::RelocationGoalSpecification goal,
    mps::planner::pushing::oracle::OracleControlSamplerPtr oracle_sampler,
    mps::planner::ompl::state::SimEnvWorldStateSpacePtr state_space,
    const std::string& file_name,
    unsigned int num_samples,
    const std::string& annotation)
{
    ::ompl::control::ControlSamplerPtr control_sampler = _space_information->allocControlSampler();
    auto state_sampler = _space_information->getStateSpace()->allocStateSampler();
    auto validity_checker = _space_information->getStateValidityChecker();
    ::ompl::base::State* start_state = _space_information->allocState();
    ::ompl::base::State* feasible_state = _space_information->allocState();
    ::ompl::base::State* goal_state = _space_information->allocState();
    ::ompl::base::State* final_state = _space_information->allocState();
    ::ompl::control::Control* control = _space_information->allocControl();
    mps::planner::util::serialize::OracleDataDumper data_dumper;
    data_dumper.setFile(file_name);
    data_dumper.openFile();
    unsigned int i = 0;
    while (i < num_samples) {
        bool has_states;
        has_states = sampleValidState(start_state, state_sampler, validity_checker);
        if (not has_states) {
            _world->getLogger()->logWarn("Failed to sample a valid state, we might end up in a infinite loop here!");
            continue;
        }
        has_states = sampleValidState(goal_state, state_sampler, validity_checker);
        if (not has_states) {
            _world->getLogger()->logWarn("Failed to sample a valid state, we might end up in a infinite loop here!");
            continue;
        }

        bool success = false;

        int target_id = state_space->getObjectIndex(goal.object_name);

        // Sample feasible state and try to move robot there
        oracle_sampler->sampleFeasibleState(feasible_state, goal_state, target_id);
        std::vector<::ompl::control::Control const*> oracle_controls_robot;
        success = oracle_sampler->steerRobot(oracle_controls_robot, start_state, feasible_state);
        if (not success) {
            _world->getLogger()->logWarn("Sampling feasible state produced no controls, trying push");
        } else {
            for (auto* control_const : oracle_controls_robot) {
                auto* control = const_cast<::ompl::control::Control*>(control_const);
                success = _state_propagator->propagate(start_state, control, start_state);
                if (not success) {
                    _world->getLogger()->logErr("State propagation to feasible state failed!!!");
                }
            }
        }

        // TODO change to steer from achieved feasible state
        std::vector<::ompl::control::Control const*> oracle_controls_push;
        success = oracle_sampler->steerPush(oracle_controls_push, start_state, goal_state, target_id);
        if (not success) {
            _world->getLogger()->logWarn("Could not sample pushing controls! Skipping");
            continue;
        } else {
            for (auto* control_const : oracle_controls_push) {
                auto* control = const_cast<::ompl::control::Control*>(control_const);
                success = _state_propagator->propagate(start_state, control, start_state);
                if (not success) {
                    _world->getLogger()->logErr("State propagation with pushing failed!!!");
                }
            }
        }

        // Start state is now incrementally changed to the resulting state
        data_dumper.saveData(goal_state, start_state, control, annotation);
        ++i;
    }
    _space_information->freeState(start_state);
    _space_information->freeState(feasible_state);
    _space_information->freeState(goal_state);
    _space_information->freeState(final_state);
    _space_information->freeControl(control);
}

bool DataGenerator::sampleValidState(::ompl::base::State* state,
    ::ompl::base::StateSamplerPtr state_sampler,
    ::ompl::base::StateValidityCheckerPtr validity_checker)
{
    unsigned int num_tries = 100;
    bool valid_sample = false;
    for (unsigned int i = 0; i < num_tries; ++i) {
        state_sampler->sampleUniform(state);
        auto sim_env_state = dynamic_cast<ompl::state::SimEnvWorldState*>(state);
        assert(sim_env_state);
        auto robot_state = sim_env_state->getObjectState(_robot_id);
        Eigen::VectorXf config;
        robot_state->getConfiguration(config);
        config.setZero();
        robot_state->setConfiguration(config);
        // check for validity
        valid_sample = validity_checker->isValid(state);
        auto object_state = sim_env_state->getObjectState(_object_id);
        object_state->getConfiguration(config);
        float distance = config.head(2).norm();
        valid_sample = valid_sample and distance < _max_distance;
        if (valid_sample)
            break;
    }
    return valid_sample;
}

void DataGenerator::applyNoise(const ::ompl::base::State* mean_state, ::ompl::base::State* noisy_state)
{
    _space_information->copyState(noisy_state, mean_state);
    auto sim_env_state = noisy_state->as<ompl::state::SimEnvWorldState>();
    assert(sim_env_state);
    auto object_state = sim_env_state->getObjectState(_object_id);
    Eigen::VectorXf config;
    object_state->getConfiguration(config);
    auto rng = util::random::getDefaultRandomGenerator();
    for (unsigned int idx = 0; idx < config.size(); ++idx) {
        auto value = rng->gaussian(0.0, _state_sigma[idx]);
        config[idx] += value;
    }
    object_state->setConfiguration(config);
}

void DataGenerator::modifyDynamics()
{
    _original_dynamics.mass = _object->getMass();
    _original_dynamics.friction_coeff = _object->getBaseLink()->getGroundFrictionCoefficient();
    auto rng = util::random::getDefaultRandomGenerator();
    float new_mass = std::max((float)(rng->gaussian(_original_dynamics.mass, _mass_stddev)), 0.000001f);
    float new_friction = std::max((float)rng->gaussian(_original_dynamics.friction_coeff, _friction_stddev), 0.00000001f);
    _object->getBaseLink()->setMass(new_mass);
    _object->getBaseLink()->setGroundFrictionCoefficient(new_friction);
}

void DataGenerator::restoreDynamics()
{
    _object->getBaseLink()->setMass(_original_dynamics.mass);
    _object->getBaseLink()->setGroundFrictionCoefficient(_original_dynamics.friction_coeff);
}

void DataGenerator::computeMaxDistance()
{
    auto control_space = std::dynamic_pointer_cast<ompl::control::RampVelocityControlSpace>(_space_information->getControlSpace());
    Eigen::Array2f duration_limits;
    Eigen::VectorXf velocity_limits;
    Eigen::VectorXf acceleration_limits;
    control_space->getDurationLimits(duration_limits);
    control_space->getVelocityLimits(velocity_limits);
    control_space->getAccelerationLimits(acceleration_limits);
    float max_acceleration_time = velocity_limits[0] / acceleration_limits[0];
    _max_distance = (max_acceleration_time + duration_limits[1]) * velocity_limits[0];
}

bool DataGenerator::objectMoved(const ::ompl::base::State* initial_state,
    const ::ompl::base::State* result_state) const
{
    auto sim_initial_state = dynamic_cast<const ompl::state::SimEnvWorldState*>(initial_state);
    auto sim_result_state = dynamic_cast<const ompl::state::SimEnvWorldState*>(result_state);
    auto initial_obj_state = sim_initial_state->getObjectState(_object_id);
    auto result_obj_state = sim_result_state->getObjectState(_object_id);
    //    std::cout << "Initial state was ";
    //    sim_initial_state->print(std::cout);
    //    std::cout << " Resulting state was ";
    //    sim_result_state->print(std::cout);
    //    std::cout << std::endl;
    Eigen::VectorXf initial_config;
    initial_obj_state->getConfiguration(initial_config);
    Eigen::VectorXf result_config;
    result_obj_state->getConfiguration(result_config);
    return (initial_config - result_config).norm() > 0.00001f;
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
// #define DO_CONSIDER_EXPLICITLY_SPECIALIZED_TRIDIAGONAL_APPLIES

#include "Tridiagonal.h"
#include "StateVector.h"

#include "Randomized.h"

#include <boost/progress.hpp>

#include <boost/preprocessor/arithmetic/sub.hpp>
#include <boost/preprocessor/comparison/less.hpp>
#include <boost/preprocessor/control/iif.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>

#define RANK 4

using namespace std;

const double totalDim=59049;// 1048576.; // 2^20

const int nRepeat=1000;

typedef quantumoperator::Tridiagonal<RANK> Tridiagonal ;
typedef quantumoperator::Tridiagonal<   1> Tridiagonal1;

typedef quantumdata::StateVector<RANK> StateVector;

typedef Tridiagonal1::Diagonal Diagonal1;

int main()
{
  int dim=mathutils::round(pow(totalDim,1./RANK));

  Diagonal1 zero(dim), minus(dim-2)//, plus(dim-2)
    ;

  {
    using randomized::fillWithRandom;
    fillWithRandom(minus,fillWithRandom(zero));
  }

  const Tridiagonal1 element(zero,2,minus);

#define PRINT_factor(z,n,data) element BOOST_PP_IIF(BOOST_PP_LESS(n, BOOST_PP_SUB(RANK, 1) ) , *, )

  const Tridiagonal tridiag( BOOST_PP_REPEAT(RANK,PRINT_factor,~) );

#undef  PRINT_factor

  cerr<<"Dimensions: "<<element.getTotalDimension()<<' '<<tridiag.getTotalDimension()//<<' '<<minus.size()<<' '<<plus.size()
      <<endl;

  StateVector psi(tridiag.getDimensions()), dpsidt(psi);

  {
    using randomized::fillWithRandom;
    fillWithRandom(psi(),1002);
  }

  {
    boost::progress_timer t;

    for (int i=0; i<nRepeat; i++)
      tridiag.apply(psi(),dpsidt());
  }

  // std::cout<<dpsidt();

}


// #pragma GCC optimize ("O0")

#include <boost/log/trivial.hpp>

#include "rtcmEncoder.hpp"
#include "acsConfig.hpp"
#include "constants.hpp"

void calculateSsrComb(
	GTime 						curTime,
	int 						period,
	SSRMeta 					ssrMeta,
	int 						masterIod,
	map<SatSys, SSROut>&		ssrOutMap)
{
	if (ssrOutMap.empty())
	{
		BOOST_LOG_TRIVIAL(warning) << "No suitable Ephemeris data available.";
		return;
	}

	double commonClockOffset = 0;

	for (auto& [Sat, ssrOut] : ssrOutMap)
	{
		if (ssrOut.clkInput.brdc1.iode != ssrOut.ephInput.brdc1.iode)
		{
			tracepdeex(3, std::cout, "IODE mismatch between clock and ephemeris for %s\n", Sat.id().c_str());
			continue;
		}

		auto& ssrEph		= ssrOut.ssrEph;
		auto& ssrClk		= ssrOut.ssrClk;
		auto& ssrEphInput	= ssrOut.ephInput;
		auto& ssrClkInput	= ssrOut.clkInput;

		ssrEph.t0 					= curTime + period / 2;
		ssrClk.t0 					= curTime + period / 2;
		ssrEph.udi					= period;
		ssrClk.udi					= period;
		ssrEph.iod					= masterIod;
		ssrClk.iod					= masterIod;

		ssrEph.ssrMeta				= ssrMeta;
		ssrClk.ssrMeta				= ssrMeta;

		ssrEph.iode		= ssrEphInput.brdc1.iode;

		// ssrEph.iodcrc				= ??

		Vector3d	diffRAC[2];
		double		diffClock = 0;
		for (int dt : {0, 1})
		{
			double		broadcastEphRatio	= (ssrEph.t0 + dt - ssrEphInput.brdc1.time) / (ssrEphInput.brdc2.time - ssrEphInput.brdc1.time);
			double		broadcastClkRatio	= (ssrClk.t0 + dt - ssrClkInput.brdc1.time) / (ssrClkInput.brdc2.time - ssrClkInput.brdc1.time);
			double		preciseEphRatio		= (ssrEph.t0 + dt - ssrEphInput.prec1.time) / (ssrEphInput.prec2.time - ssrEphInput.prec1.time);
			double		preciseClkRatio		= (ssrClk.t0 + dt - ssrClkInput.prec1.time) / (ssrClkInput.prec2.time - ssrClkInput.prec1.time);

			Vector3d	broadcastPos		= ssrEphInput.brdc1.pos + broadcastEphRatio	* (ssrEphInput.brdc2.pos - ssrEphInput.brdc1.pos);
			Vector3d	broadcastVel		= ssrEphInput.brdc1.vel + broadcastEphRatio	* (ssrEphInput.brdc2.vel - ssrEphInput.brdc1.vel);
			double		broadcastClk		= ssrClkInput.brdc1.clk + broadcastClkRatio	* (ssrClkInput.brdc2.clk - ssrClkInput.brdc1.clk);
			Vector3d	precisePos			= ssrEphInput.prec1.pos + preciseEphRatio	* (ssrEphInput.prec2.pos - ssrEphInput.prec1.pos);
	// 		Vector3d	preciseVel			= ssrEphInput.prec1.vel + preciseEphRatio	* (ssrEphInput.prec2.vel - ssrEphInput.prec1.vel);
			double		preciseClk			= ssrClkInput.prec1.clk + preciseClkRatio	* (ssrClkInput.prec2.clk - ssrClkInput.prec1.clk);

			Vector3d	diffEcef		= broadcastPos - precisePos;
			Vector3d	diffRac			= ecef2rac(broadcastPos, broadcastVel) 	* diffEcef;

			diffRAC[dt]	= diffRac;

			diffClock	= broadcastClk - preciseClk;
// 			std::cout << std::fixed;
// 			std::cout << "BRatio:       " << broadcastEphRatio << std::endl;
// 			std::cout << "brdc1:        "<< ssrEphInput.brdc1.pos.transpose() << std::endl;
// 			std::cout << "brdc2:        "<< ssrEphInput.brdc2.pos.transpose() << std::endl;
// 			std::cout << "broadcastPos: "<< broadcastPos.transpose() << std::endl;
// 			std::cout << "PRatio:       " << broadcastEphRatio << std::endl;
// 			std::cout << "prec1:        "<< ssrEphInput.prec1.pos.transpose() << std::endl;
// 			std::cout << "prec2:        "<< ssrEphInput.prec2.pos.transpose() << std::endl;
// 			std::cout << "precisePos:   "<< precisePos.transpose() << std::endl;
// 			std::cout << "diffEcef:     "<< diffEcef.transpose() << std::endl;
// 			std::cout << "diffRac:      "<< diffRac.transpose() << std::endl;
		}

		ssrEph. deph	= diffRAC[0];
		ssrEph.ddeph	= diffRAC[1] - diffRAC[0];

// 		std::cout << "deph:         "<< ssrEph. deph.transpose() << std::endl;
// 		std::cout << "ddeph:        "<< ssrEph.ddeph.transpose() << std::endl;
		ssrClk.dclk[0]	= diffClock;
		ssrClk.dclk[1]	= 0;	// set to zero (not used)
		ssrClk.dclk[2]	= 0;	// set to zero (not used)

		//adjust all clock corrections so that they remain within the bounds of the outputs
		if (commonClockOffset == 0)
		{
			commonClockOffset = ssrClk.dclk[0];
		}
		ssrClk.dclk[0] -= commonClockOffset;

// break;
	}
// 	std::cout << "Calculated Combined Messages.\n";
}

int RtcmEncoder::getUdiIndex(int udi)
{
	for (int i = 0; i < 16; i++) // 16 from updateInterval[16] above
	{
		if (updateInterval[i] == udi)
		{
			return i;
		}
	}

	BOOST_LOG_TRIVIAL(error) << "Error: udi is not valid :" << udi << ").";

	return -1;
}

void RtcmEncoder::encodeWriteMessages(
	std::ostream& outputStream)
{
	if (outputStream)
	{
		outputStream.write((const char*) &data[0], data.size());
		data.clear();
	}
}

bool RtcmEncoder::encodeWriteMessageToBuffer(
	vector<uint8_t>& buffer)
{
	int i = 0;
	int messLength = buffer.size();

	if (buffer.empty())
	{
		return false;
	}

// 	unsigned char nbuf[messLength+6];
	vector<uint8_t> newbuffer(messLength+6);
	unsigned char* nbuf	= newbuffer.data();
	unsigned char* buf	= buffer.data();

	i = setbituInc(nbuf,i,8,	RTCM_PREAMBLE);
	i = setbituInc(nbuf,i,6,	0);
	i = setbituInc(nbuf,i,10,	messLength);

	memcpy(nbuf+3,buf,sizeof(uint8_t)*messLength);
	i = i + messLength*8;

	const unsigned char* bCrcBuf = (const unsigned char *)nbuf;
	unsigned int crcCalc = crc24q(bCrcBuf, sizeof(char)*(messLength+3));

	unsigned char* bCrcCalc = (unsigned char*)&crcCalc;
	unsigned int b1 = 0;
	unsigned int b2 = 0;
	unsigned int b3 = 0;
	setbituInc((unsigned char *)&b1,0,8,*(bCrcCalc+0));
	setbituInc((unsigned char *)&b2,0,8,*(bCrcCalc+1));
	setbituInc((unsigned char *)&b3,0,8,*(bCrcCalc+2));
	i = setbituInc(nbuf,i,8,b3);
	i = setbituInc(nbuf,i,8,b2);
	i = setbituInc(nbuf,i,8,b1);

	data.insert(data.end(), &nbuf[0], &nbuf[messLength+6]);

	return true;
}

vector<uint8_t> RtcmEncoder::encodeTimeStampRTCM()
{
	// Custom message code, for crcsi maximum length 4096 bits or 512 bytes.
	unsigned int messCode = +RtcmMessageType::CUSTOM;
	unsigned int messType = +E_RTCMSubmessage::TIMESTAMP;

	boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();

	// Number of seconds since 1/1/1970, long is 64 bits and all may be used.
	long int seconds = (now - boost::posix_time::from_time_t(0)).total_seconds();

	//Number of fractional seconds, The largest this can be is 1000 which is 10 bits unsigned.
	boost::posix_time::ptime now_mod_seconds	= boost::posix_time::from_time_t(seconds);
	auto subseconds	= now - now_mod_seconds;
	int milli_sec = subseconds.total_milliseconds();

	unsigned int* var = (unsigned int*) &seconds;

	int i = 0;
	//int byteLen = ceil((12.0+8.0+64.0+10.0)/8.0);
	int byteLen = 12;
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();
	i = setbituInc(buf, i, 12,		messCode);
	i = setbituInc(buf, i, 8,		messType);
	i = setbituInc(buf, i, 32,		var[0]);
	i = setbituInc(buf, i, 32,		var[1]);
	i = setbituInc(buf, i, 10, (int)milli_sec);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrComb(
	map<SatSys, SSROut>& ssrOutMap)
{
	int numSat = ssrOutMap.size();
	if (numSat == 0)
	{
		return vector<uint8_t>();
	}

	auto& [Sat, ssrOut1]	= *ssrOutMap.begin();
	auto& ssrEph			= ssrOut1.ssrEph;
	auto& ssrMeta			= ssrEph.ssrMeta;

	int i		= 0;
	int bitLen	= 0;
	int ni		= 0;
	unsigned int messCode = 0;
	if		(Sat.sys == +E_Sys::GPS) {	messCode = +RtcmMessageType::GPS_SSR_COMB_CORR;		ni= 8; bitLen = 68+numSat*205;}
	else if (Sat.sys == +E_Sys::GAL) {	messCode = +RtcmMessageType::GAL_SSR_COMB_CORR;		ni=10; bitLen = 68+numSat*207;}

	int byteLen = ceil(bitLen/8.0);
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	// Write the header information.
	i = setbituInc(buf,i,12,	messCode);
	i = setbituInc(buf,i,20,	ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,		ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,		ssrMeta.multipleMessage);
	i = setbituInc(buf,i,1,		ssrMeta.referenceDatum);
	i = setbituInc(buf,i,4,		ssrEph.iod);
	i = setbituInc(buf,i,16,	ssrMeta.provider);
	i = setbituInc(buf,i,4,		ssrMeta.solution);
	i = setbituInc(buf,i,6,		numSat);

	for (auto& [Sat, ssrOut] : ssrOutMap)
	{
		auto& ssrEph = ssrOut.ssrEph;
		auto& ssrClk = ssrOut.ssrClk;

		i = setbituInc(buf,i, 6,	Sat.prn);
		i = setbituInc(buf,i, ni,	ssrEph.iode);

		int d;
		d = (int)round(ssrEph.deph[0]		/ 0.1e-3);				i = setbitsInc(buf,i,22,d);
		d = (int)round(ssrEph.deph[1]		/ 0.4e-3);				i = setbitsInc(buf,i,20,d);
		d = (int)round(ssrEph.deph[2]		/ 0.4e-3);				i = setbitsInc(buf,i,20,d);
		d = (int)round(ssrEph.ddeph[0]		/ 0.001e-3);			i = setbitsInc(buf,i,21,d);
		d = (int)round(ssrEph.ddeph[1]		/ 0.004e-3);			i = setbitsInc(buf,i,19,d);
		d = (int)round(ssrEph.ddeph[2]		/ 0.004e-3);			i = setbitsInc(buf,i,19,d);

		d = (int)round(ssrClk.dclk[0]		/ 0.1e-3);				i = setbitsInc(buf,i,22,d);
		d = (int)round(ssrClk.dclk[1]		/ 0.001e-3);			i = setbitsInc(buf,i,21,d);
		d = (int)round(ssrClk.dclk[2]		/ 0.00002e-3);			i = setbitsInc(buf,i,27,d);

		outputSsrEphToJson(ssrEph, Sat);
		outputSsrClkToJson(ssrClk, Sat);

	}

	int bitl = byteLen*8-i;
	if (bitl > 7 )
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding combined.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}
	i = setbituInc(buf, i, bitl, 0);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrPhase(
	SsrPBMap& ssrPBMap)
{
	int numSat = ssrPBMap.size();

	int i = 0;
	int bitLen = 0;

	int totalNbias = 0;
	for (auto& [Sat,ssrPhasBias] : ssrPBMap)
	{
		totalNbias += ssrPhasBias.obsCodeBiasMap.size();
	}

	if (totalNbias == 0)
		return vector<uint8_t>();

	// Write the header information.
	auto s_it = ssrPBMap.begin();
	auto& [Sat, ssrPhasBias] = *s_it;
	SSRMeta& ssrMeta = ssrPhasBias.ssrMeta;

	int np = 0;
	int ni = 0;
	int nj = 0;
	int offp = 0;
	unsigned int messCode = 0;
	if (Sat.sys == +E_Sys::GPS) {	messCode = +RtcmMessageType::GPS_SSR_PHASE_BIAS;	np=6; ni= 8; nj= 0; offp=0;	bitLen = 69+numSat*28+totalNbias*32;}
	if (Sat.sys == +E_Sys::GAL)	{	messCode = +RtcmMessageType::GAL_SSR_PHASE_BIAS;	np=6; ni=10; nj= 0; offp=0;	bitLen = 69+numSat*28+totalNbias*32;}

	int byteLen = ceil(bitLen/8.0);
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	i = setbituInc(buf,i,12,messCode);
	i = setbituInc(buf,i,20,ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,	ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,	ssrMeta.multipleMessage);

	i = setbituInc(buf,i,4,	ssrPhasBias.iod);
	i = setbituInc(buf,i,16,ssrMeta.provider);
	i = setbituInc(buf,i,4,	ssrMeta.solution);

	i = setbituInc(buf,i,1,	ssrPhasBias.ssrPhase.dispBiasConistInd);
	i = setbituInc(buf,i,1,	ssrPhasBias.ssrPhase.MWConistInd);

	i = setbituInc(buf,i,6,numSat);

	for (auto& [sat, ssrPhasBias] : ssrPBMap)
	{
		SSRPhase ssrPhase = ssrPhasBias.ssrPhase;

		int d;
															i = setbituInc(buf,i,np,sat.prn);
		d = ssrPhasBias.obsCodeBiasMap.size();				i = setbituInc(buf,i,5,	d);
		d = (int)round(ssrPhase.yawAngle	*256	/PI);	i = setbituInc(buf,i,9,	d);
		d = (int)round(ssrPhase.yawRate		*8192	/PI);	i = setbitsInc(buf,i,8,	d);

		for (auto& [obsCode, entry] : ssrPhasBias.obsCodeBiasMap)
		{
			SSRPhaseCh ssrPhaseCh = ssrPhasBias.ssrPhaseChs[obsCode];

			//BOOST_LOG_TRIVIAL(debug) << "Phase, obsCode : " << obsCode << std::endl;
			//BOOST_LOG_TRIVIAL(debug) << "E_sys         : " << sys << std::endl;
			//BOOST_LOG_TRIVIAL(debug) << "mCodes_gps.size() : " << mCodes_gps.size() << std::endl;
			//print_map( mCodes_gps.left, " E_ObsCode <--> RTCM ", BOOST_LOG_TRIVIAL(debug) );

			int rtcm_code = 0;
			if		( sat.sys == +E_Sys::GPS )		rtcm_code = mCodes_gps.left.at(obsCode);		//todo aaron, crash heaven, needs else, try
			else if ( sat.sys == +E_Sys::GAL )		rtcm_code = mCodes_gal.left.at(obsCode);

			//BOOST_LOG_TRIVIAL(debug) << "rtcm_code      : " << rtcm_code << std::endl;

													i = setbituInc(buf,i,5,	rtcm_code);
													i = setbituInc(buf,i,1,	ssrPhaseCh.signalIntInd);
													i = setbituInc(buf,i,2,	ssrPhaseCh.signalWidIntInd);
													i = setbituInc(buf,i,4,	ssrPhaseCh.signalDisconCnt);
			d = (int)round(entry.bias / 0.0001);	i = setbitsInc(buf,i,20,d);

			traceSsrPhasB(sat, obsCode, ssrPhasBias);
		}
	}

	int bitl = byteLen*8-i;
	if (bitl > 7 )
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding SSR Phase.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}

	i = setbituInc(buf,i,bitl,0);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrCode(
	SsrCBMap& ssrCBMap)
{
	int numSat = ssrCBMap.size();

	int i = 0;

	int totalNbias = 0;
	for (auto s_it = ssrCBMap.begin(); s_it != ssrCBMap.end(); s_it++)
	{
		SSRCodeBias ssrCodeBias = s_it->second;
		totalNbias += ssrCodeBias.obsCodeBiasMap.size();
	}

	if (totalNbias == 0)
	{
		return vector<uint8_t>();
	}

	// Write the header information.

	auto s_it = ssrCBMap.begin();
	auto& [Sat, ssrCodeBias] = *s_it;
	SSRMeta& ssrMeta = ssrCodeBias.ssrMeta;

	int bitLen	= 67
				+ 11 * numSat
				+ 19 * totalNbias;
	int byteLen = ceil(bitLen / 8.0);

	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	unsigned int messCode = 0;
	if (Sat.sys == +E_Sys::GPS)	{	messCode = +RtcmMessageType::GPS_SSR_CODE_BIAS;}
	if (Sat.sys == +E_Sys::GAL)	{	messCode = +RtcmMessageType::GAL_SSR_CODE_BIAS;}

	i = setbituInc(buf,i,12,messCode);
	i = setbituInc(buf,i,20,ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,	ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,	ssrMeta.multipleMessage);

	i = setbituInc(buf,i,4,	ssrCodeBias.iod);
	i = setbituInc(buf,i,16,ssrMeta.provider);
	i = setbituInc(buf,i,4,	ssrMeta.solution);
	i = setbituInc(buf,i,6,	numSat);

	for (auto& [sat, ssrCodeBias] : ssrCBMap)
	{
		i = setbituInc(buf, i, 6, sat.prn);
		unsigned int nbias = ssrCodeBias.obsCodeBiasMap.size();

		i = setbituInc(buf, i, 5, nbias);

		for (auto& [obsCode, entry] : ssrCodeBias.obsCodeBiasMap)
		{
			int rtcm_code = 0;
			if		( sat.sys == +E_Sys::GPS )	{	rtcm_code = mCodes_gps.left.at(obsCode);		}
			else if ( sat.sys == +E_Sys::GAL )	{	rtcm_code = mCodes_gal.left.at(obsCode);		}

													i = setbituInc(buf, i, 5,	rtcm_code);
			int d = (int)round(entry.bias / 0.01);	i = setbitsInc(buf, i, 14,	d);

			traceSsrCodeB(sat, obsCode, ssrCodeBias);
		}
	}

	int bitl = byteLen*8-i;
	if (bitl > 7)
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding SSR Code.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}

	i = setbituInc(buf,i,bitl,0);

	return buffer;
}


//==============================================================================
//          Copyright 2014    Jean-Thierry 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_SIMD_SSE_AVX_CUMMAX_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_AVX_CUMMAX_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/swar/functions/cummax.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT (cummax_, boost::simd::tag::avx_,
                            (A0),
                            ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                           )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a000 = _mm256_extractf128_si256(a0, 0);
      svtype a011 = _mm256_extractf128_si256(a0, 1);
      svtype a00 =  cummax(a000);
      svtype a01 =  cummax(a011);
      svtype z = splat<svtype>(a00[meta::cardinal_of<svtype>::value-1]);
      __m256i that = _mm256_castsi128_si256(a00);
      return _mm256_insertf128_si256(that, max(a01, z), 1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////

  BOOST_DISPATCH_IMPLEMENT         (cummax_, boost::simd::tag::avx_,
                                    (A0),
                                    ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a000 = _mm256_extractf128_pd(a0, 0);
      svtype a011 = _mm256_extractf128_pd(a0, 1);
      svtype a00 =  cummax(a000);
      svtype a01 =  cummax(a011);
      svtype z = splat<svtype>(a00[meta::cardinal_of<svtype>::value-1]);
      __m256d that = _mm256_castpd128_pd256(a00);
      return _mm256_insertf128_pd(that, max(a01, z), 1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////

  BOOST_DISPATCH_IMPLEMENT         (cummax_, boost::simd::tag::avx_,
                                    (A0),
                                    ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a011;
      a011=  _mm256_extractf128_ps(a0, 1);
      svtype a000;
      a000 =  _mm256_extractf128_ps(a0, 0);
      svtype a00 =  cummax(a000);
      svtype a01 =  cummax(a011);
      svtype z = splat<svtype>(a00[meta::cardinal_of<svtype>::value-1]);
      __m256 that = _mm256_castps128_ps256(a00);
      return _mm256_insertf128_ps(that, max(a01, z), 1);
    }
  };
} } }
#endif
#endif


/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <ThermalOperator.hh>
#include <instantiation.hh>

#include <deal.II/base/index_set.h>
#include <deal.II/base/types.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/mapping_q1.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/hp/fe_values.h>
#include <deal.II/matrix_free/fe_evaluation.h>

namespace adamantine
{

template <int dim, int fe_degree, typename MemorySpaceType>
ThermalOperator<dim, fe_degree, MemorySpaceType>::ThermalOperator(
    MPI_Comm const &communicator, BoundaryType boundary_type,
    std::shared_ptr<MaterialProperty<dim>> material_properties,
    std::vector<std::shared_ptr<HeatSource<dim>>> heat_sources)
    : _communicator(communicator), _boundary_type(boundary_type),
      _material_properties(material_properties), _heat_sources(heat_sources),
      _inverse_mass_matrix(
          new dealii::LA::distributed::Vector<double, MemorySpaceType>())
{
  _matrix_free_data.tasks_parallel_scheme =
      dealii::MatrixFree<dim, double>::AdditionalData::partition_color;
  _matrix_free_data.mapping_update_flags =
      dealii::update_values | dealii::update_gradients |
      dealii::update_JxW_values | dealii::update_quadrature_points;
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::reinit(
    dealii::DoFHandler<dim> const &dof_handler,
    dealii::AffineConstraints<double> const &affine_constraints,
    dealii::hp::QCollection<1> const &q_collection)
{
  _matrix_free.reinit(dealii::StaticMappingQ1<dim>::mapping, dof_handler,
                      affine_constraints, q_collection, _matrix_free_data);
  _affine_constraints = &affine_constraints;
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::
    compute_inverse_mass_matrix(
        dealii::DoFHandler<dim> const &dof_handler,
        dealii::AffineConstraints<double> const &affine_constraints,
        dealii::hp::FECollection<dim> const &fe_collection)
{
  // Compute the inverse of the mass matrix
  dealii::hp::QCollection<dim> mass_matrix_q_collection;
  mass_matrix_q_collection.push_back(dealii::QGaussLobatto<dim>(fe_degree + 1));
  mass_matrix_q_collection.push_back(dealii::QGaussLobatto<dim>(2));
  auto locally_owned_dofs = dof_handler.locally_owned_dofs();
  dealii::IndexSet locally_relevant_dofs;
  dealii::DoFTools::extract_locally_relevant_dofs(dof_handler,
                                                  locally_relevant_dofs);
  _inverse_mass_matrix->reinit(locally_owned_dofs, locally_relevant_dofs,
                               _communicator);
  dealii::hp::FEValues<dim> hp_fe_values(
      fe_collection, mass_matrix_q_collection,
      dealii::update_quadrature_points | dealii::update_values |
          dealii::update_JxW_values);
  unsigned int const dofs_per_cell = fe_collection.max_dofs_per_cell();
  unsigned int const n_q_points =
      mass_matrix_q_collection.max_n_quadrature_points();
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  dealii::Vector<double> cell_mass(dofs_per_cell);
  for (auto const &cell : dealii::filter_iterators(
           dof_handler.active_cell_iterators(),
           dealii::IteratorFilters::LocallyOwnedCell(),
           dealii::IteratorFilters::ActiveFEIndexEqualTo(0)))
  {
    cell_mass = 0.;
    hp_fe_values.reinit(cell);
    dealii::FEValues<dim> const &fe_values =
        hp_fe_values.get_present_fe_values();
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
    {
      for (unsigned int q = 0; q < n_q_points; ++q)
      {
        for (unsigned int j = 0; j < dofs_per_cell; ++j)
        {

          cell_mass[i] += fe_values.shape_value(j, q) *
                          fe_values.shape_value(i, q) * fe_values.JxW(q);
        }
      }
    }
    cell->get_dof_indices(local_dof_indices);
    affine_constraints.distribute_local_to_global(cell_mass, local_dof_indices,
                                                  *_inverse_mass_matrix);
  }
  _inverse_mass_matrix->compress(dealii::VectorOperation::add);

  unsigned int const local_size = _inverse_mass_matrix->locally_owned_size();
  for (unsigned int k = 0; k < local_size; ++k)
  {
    if (_inverse_mass_matrix->local_element(k) > 1e-15)
      _inverse_mass_matrix->local_element(k) =
          1. / _inverse_mass_matrix->local_element(k);
    else
      _inverse_mass_matrix->local_element(k) = 0.;
  }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::clear()
{
  _matrix_free.clear();
  _inverse_mass_matrix->reinit(0);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::vmult(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  dst = 0.;
  vmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::Tvmult(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  dst = 0.;
  Tvmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::vmult_add(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  // Execute the matrix-free matrix-vector multiplication
  _matrix_free.cell_loop(&ThermalOperator::cell_local_apply, this, dst, src);
  // We compute the boundary conditions by hand. We would like to use
  // dealii::MatrixFree::loop() but there are two problems: 1. the function does
  // not work with FE_Nothing 2. even if it did we could only use it for the
  // domain boundary, we would still need to deal with the interface with
  // FE_Nothing ourselves.
  if (!(_boundary_type & BoundaryType::adiabatic))
  {
    unsigned int const dofs_per_cell = _matrix_free.get_dofs_per_cell();
    dealii::Vector<double> cell_src(dofs_per_cell);
    dealii::Vector<double> cell_dst(dofs_per_cell);
    auto &dof_handler = _matrix_free.get_dof_handler();
    std::vector<dealii::types::global_dof_index> local_dof_indices(
        dofs_per_cell);
    dealii::QGauss<dim - 1> face_quadrature(fe_degree + 1);
    dealii::FEFaceValues<dim> fe_face_values(
        dof_handler.get_fe(), face_quadrature,
        dealii::update_values | dealii::update_quadrature_points |
            dealii::update_JxW_values);
    unsigned int const n_face_q_points = face_quadrature.size();
    // Loop over the locally owned cells with an active FE index of zero
    for (auto const &cell : dealii::filter_iterators(
             dof_handler.active_cell_iterators(),
             dealii::IteratorFilters::LocallyOwnedCell(),
             dealii::IteratorFilters::ActiveFEIndexEqualTo(0)))
    {
      cell_dst = 0.;
      cell->get_dof_indices(local_dof_indices);
      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        cell_src[i] = src[local_dof_indices[i]];
      bool is_on_domain_boundary = false;
      for (unsigned int f = 0; f < dealii::GeometryInfo<dim>::faces_per_cell;
           ++f)
      {
        // We need to add the boundary conditions on the faces on the boundary
        // but also on the faces at the interface with FE_Nothing
        auto const &face = cell->face(f);
        if ((face->at_boundary()) ||
            ((!face->at_boundary()) &&
             (cell->neighbor(f)->active_fe_index() != 0)))
        {
          double conv_temperature_infty = 0.;
          double conv_heat_transfer_coef = 0.;
          double rad_temperature_infty = 0.;
          double rad_heat_transfer_coef = 0.;
          if (_boundary_type & BoundaryType::convective)
          {
            conv_temperature_infty = _material_properties->get_cell_value(
                cell, Property::convection_temperature_infty);
            conv_heat_transfer_coef = _material_properties->get_cell_value(
                cell, StateProperty::convection_heat_transfer_coef);
          }
          if (_boundary_type & BoundaryType::radiative)
          {
            rad_temperature_infty = _material_properties->get_cell_value(
                cell, Property::radiation_temperature_infty);
            rad_heat_transfer_coef = _material_properties->get_cell_value(
                cell, StateProperty::radiation_heat_transfer_coef);
          }

          fe_face_values.reinit(cell, face);
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
          {
            for (unsigned int i = 0; i < dofs_per_cell; ++i)
            {
              for (unsigned int q = 0; q < n_face_q_points; ++q)
              {
                // FIXME Need to be aware that we are using face quadrature
                // points not volumetric quadrature point. Right now we accept
                // this slight error.
                double const inv_rho_cp = get_inv_rho_cp(cell, q);

                cell_dst[i] -= inv_rho_cp *
                               (conv_heat_transfer_coef *
                                    (cell_src[j] - conv_temperature_infty) +
                                rad_heat_transfer_coef *
                                    (cell_src[j] - rad_temperature_infty)) *
                               fe_face_values.shape_value(i, q) *
                               fe_face_values.shape_value(j, q) *
                               fe_face_values.JxW(q);
              }
            }
          }
          is_on_domain_boundary = true;
        }
      }

      if (is_on_domain_boundary)
      {
        _affine_constraints->distribute_local_to_global(cell_dst,
                                                        local_dof_indices, dst);
      }
    }
  }

  // Because cell_loop resolves the constraints, the constrained dofs are not
  // called they stay at zero. Thus, we need to force the value on the
  // constrained dofs by hand. The variable scaling is used so that we get the
  // right order of magnitude.
  // TODO: for now the value of scaling is set to 1
  double const scaling = 1.;
  std::vector<unsigned int> const &constrained_dofs =
      _matrix_free.get_constrained_dofs();
  for (auto &dof : constrained_dofs)
    dst.local_element(dof) += scaling * src.local_element(dof);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::Tvmult_add(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  // The system of equation is symmetric so we can use vmult_add
  vmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::update_state_ratios(
    unsigned int cell, unsigned int q,
    dealii::VectorizedArray<double> temperature,
    std::array<dealii::VectorizedArray<double>,
               static_cast<unsigned int>(MaterialState::SIZE)> &state_ratios)
    const
{
  unsigned int constexpr liquid =
      static_cast<unsigned int>(MaterialState::liquid);
  unsigned int constexpr powder =
      static_cast<unsigned int>(MaterialState::powder);
  unsigned int constexpr solid =
      static_cast<unsigned int>(MaterialState::solid);

  // Loop over the vectorized arrays
  for (unsigned int n = 0; n < temperature.size(); ++n)
  {
    // Get the material id at this point
    dealii::types::material_id material_id = _material_id(cell, q)[n];

    // Get the material thermodynamic properties
    double const solidus =
        _material_properties->get(material_id, Property::solidus);
    double const liquidus =
        _material_properties->get(material_id, Property::liquidus);

    // Update the state ratios
    state_ratios[powder] = _powder_ratio(cell, q);

    if (temperature[n] < solidus)
      state_ratios[liquid][n] = 0.;
    else if (temperature[n] > liquidus)
      state_ratios[liquid][n] = 1.;
    else
    {
      state_ratios[liquid][n] =
          (temperature[n] - solidus) / (liquidus - solidus);
    }
    // Because the powder can only become liquid, the solid can only
    // become liquid, and the liquid can only become solid, the ratio of
    // powder can only decrease.
    state_ratios[powder][n] =
        std::min(1. - state_ratios[liquid][n], state_ratios[powder][n]);
    // Use max to make sure that we don't create matter because of
    // round-off.
    state_ratios[solid][n] =
        std::max(1. - state_ratios[liquid][n] - state_ratios[powder][n], 0.);
  }

  _liquid_ratio(cell, q) = state_ratios[liquid];
  _powder_ratio(cell, q) = state_ratios[powder];
}

template <int dim, int fe_degree, typename MemorySpaceType>
dealii::VectorizedArray<double>
ThermalOperator<dim, fe_degree, MemorySpaceType>::get_inv_rho_cp(
    unsigned int cell, unsigned int q,
    std::array<dealii::VectorizedArray<double>,
               static_cast<unsigned int>(MaterialState::SIZE)>
        state_ratios,
    dealii::VectorizedArray<double> temperature) const
{
  // Here we need the specific heat (including the latent heat contribution)
  // and the density

  auto material_id = _material_id(cell, q);
  // First, get the state-independent material properties
  dealii::VectorizedArray<double> solidus, liquidus, latent_heat;
  for (unsigned int n = 0; n < solidus.size(); ++n)
  {
    solidus[n] = _material_properties->get(material_id[n], Property::solidus);
    liquidus[n] = _material_properties->get(material_id[n], Property::liquidus);
    latent_heat[n] =
        _material_properties->get(material_id[n], Property::latent_heat);
  }

  // Now compute the state-dependent properties
  dealii::VectorizedArray<double> density =
      _material_properties->compute_material_property(
          StateProperty::density, material_id.data(), state_ratios.data(),
          temperature);

  dealii::VectorizedArray<double> specific_heat =
      _material_properties->compute_material_property(
          StateProperty::specific_heat, material_id.data(), state_ratios.data(),
          temperature);

  // Add in the latent heat contribution
  unsigned int constexpr liquid =
      static_cast<unsigned int>(MaterialState::liquid);

  for (unsigned int n = 0; n < specific_heat.size(); ++n)
  {
    if (state_ratios[liquid][n] > 0.0 && (state_ratios[liquid][n] < 1.0))
    {
      specific_heat[n] += latent_heat[n] / (liquidus[n] - solidus[n]);
    }
  }

  _inv_rho_cp(cell, q) = 1.0 / (density * specific_heat);

  return _inv_rho_cp(cell, q);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::cell_local_apply(
    dealii::MatrixFree<dim, double> const &data,
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src,
    std::pair<unsigned int, unsigned int> const &cell_range) const
{
  // Get the subrange of cells associated with the fe index 0
  std::pair<unsigned int, unsigned int> cell_subrange =
      data.create_cell_subrange_hp_by_index(cell_range, 0);

  dealii::FEEvaluation<dim, fe_degree, fe_degree + 1, 1, double> fe_eval(data);
  dealii::Tensor<1, dim> unit_tensor;
  for (unsigned int i = 0; i < dim; ++i)
    unit_tensor[i] = 1.;

  std::array<dealii::VectorizedArray<double>,
             static_cast<unsigned int>(MaterialState::SIZE)>
      state_ratios = {{dealii::make_vectorized_array(-1.0),
                       dealii::make_vectorized_array(-1.0),
                       dealii::make_vectorized_array(-1.0)}};

  // Loop over the "cells". Note that we don't really work on a cell but on a
  // set of quadrature point.
  for (unsigned int cell = cell_subrange.first; cell < cell_subrange.second;
       ++cell)
  {
    // Reinit fe_eval on the current cell
    fe_eval.reinit(cell);
    // Store in a local vector the local values of src
    fe_eval.read_dof_values(src);
    // Evaluate the function and its gradient on the reference cell
    fe_eval.evaluate(dealii::EvaluationFlags::values |
                     dealii::EvaluationFlags::gradients);
    // Apply the Jacobian of the transformation, multiply by the variable
    // coefficients and the quadrature points
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
    {
      auto temperature = fe_eval.get_value(q);

      // Calculate the local material properties
      update_state_ratios(cell, q, temperature, state_ratios);
      auto inv_rho_cp = get_inv_rho_cp(cell, q, state_ratios, temperature);
      auto mat_id = _material_id(cell, q);
      auto thermal_conductivity =
          _material_properties->compute_material_property(
              StateProperty::thermal_conductivity, mat_id.data(),
              state_ratios.data(), temperature);

      fe_eval.submit_gradient(
          -inv_rho_cp * thermal_conductivity * fe_eval.get_gradient(q), q);

      // Compute source term
      dealii::Point<dim, dealii::VectorizedArray<double>> const &q_point =
          fe_eval.quadrature_point(q);

      dealii::VectorizedArray<double> quad_pt_source = 0.0;
      for (unsigned int i = 0;
           i < _matrix_free.n_active_entries_per_cell_batch(cell); ++i)
      {
        dealii::Point<dim> q_point_loc;
        for (unsigned int d = 0; d < dim; ++d)
          q_point_loc(d) = q_point(d)[i];

        for (auto &beam : _heat_sources)
          quad_pt_source[i] +=
              beam->value(q_point_loc, _time, _current_source_height);
      }
      quad_pt_source *= inv_rho_cp;

      fe_eval.submit_value(quad_pt_source, q);
    }
    // Sum over the quadrature points.
    fe_eval.integrate(dealii::EvaluationFlags::values |
                      dealii::EvaluationFlags::gradients);
    fe_eval.distribute_local_to_global(dst);
  }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree,
                     MemorySpaceType>::get_state_from_material_properties()
{
  unsigned int const n_cells = _matrix_free.n_cell_batches();
  dealii::FEEvaluation<dim, fe_degree, fe_degree + 1, 1, double> fe_eval(
      _matrix_free);

  _liquid_ratio.reinit(n_cells, fe_eval.n_q_points);
  _powder_ratio.reinit(n_cells, fe_eval.n_q_points);
  _material_id.reinit(n_cells, fe_eval.n_q_points);
  _inv_rho_cp.reinit(n_cells, fe_eval.n_q_points);

  for (unsigned int cell = 0; cell < n_cells; ++cell)
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
      for (unsigned int i = 0;
           i < _matrix_free.n_active_entries_per_cell_batch(cell); ++i)
      {
        typename dealii::DoFHandler<dim>::cell_iterator cell_it =
            _matrix_free.get_cell_iterator(cell, i);
        // Cast to Triangulation<dim>::cell_iterator to access the material_id
        typename dealii::Triangulation<dim>::active_cell_iterator cell_tria(
            cell_it);

        _liquid_ratio(cell, q)[i] = _material_properties->get_state_ratio(
            cell_tria, MaterialState::liquid);
        _powder_ratio(cell, q)[i] = _material_properties->get_state_ratio(
            cell_tria, MaterialState::powder);
        _material_id(cell, q)[i] = cell_tria->material_id();
      }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree,
                     MemorySpaceType>::set_state_to_material_properties()
{
  _material_properties->set_state(_liquid_ratio, _powder_ratio,
                                  _cell_it_to_mf_cell_map,
                                  _matrix_free.get_dof_handler());
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::
    evaluate_material_properties(
        dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> const
            &temperature)
{
  if (!(_boundary_type & BoundaryType::adiabatic))
    _material_properties->update_boundary_material_properties(
        _matrix_free.get_dof_handler(), temperature);

  // Store the volumetric material properties
  unsigned int const n_cells = _matrix_free.n_cell_batches();
  for (unsigned int cell = 0; cell < n_cells; ++cell)
    for (unsigned int i = 0;
         i < _matrix_free.n_active_entries_per_cell_batch(cell); ++i)
    {
      typename dealii::DoFHandler<dim>::cell_iterator cell_it =
          _matrix_free.get_cell_iterator(cell, i);
      _cell_it_to_mf_cell_map[cell_it] = std::make_pair(cell, i);
    }
}
} // namespace adamantine

INSTANTIATE_DIM_FEDEGREE_HOST(TUPLE(ThermalOperator))


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_HASH_BLAKE2B_HPP
#define CRYPTO3_HASH_BLAKE2B_HPP

#include <boost/crypto3/hash/detail/blake2b/blake2b_functions.hpp>
#include <boost/crypto3/hash/detail/blake2b/blake2b_padding.hpp>

#include <boost/crypto3/hash/detail/haifa_construction.hpp>
#include <boost/crypto3/hash/detail/block_stream_processor.hpp>

namespace boost {
    namespace crypto3 {
        namespace hashes {
            template<std::size_t DigestBits>
            class blake2b_compressor {
            protected:
                typedef detail::blake2b_functions<DigestBits> policy_type;

                typedef typename policy_type::state_type::value_type value_type;

            public:
                typedef typename policy_type::iv_generator iv_generator;

                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t state_bits = policy_type::state_bits;
                constexpr static const std::size_t state_words = policy_type::state_words;
                typedef typename policy_type::state_type state_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                constexpr static const std::size_t salt_bits = policy_type::salt_bits;
                typedef typename policy_type::salt_type salt_type;
                constexpr static const salt_type salt_value = policy_type::salt_value;

                static void process_block(state_type &state, const block_type &block, value_type seen = value_type(),
                                          value_type finalizator = value_type()) {
                    std::array<word_type, state_words * 2> v;

                    std::move(state.begin(), state.end(), v.begin());
                    std::move(iv_generator()().begin(), iv_generator()().end(), v.begin() + state_words);

                    std::array<typename state_type::value_type, 2> s = {seen / CHAR_BIT + ((seen % CHAR_BIT) ? 1 : 0),
                                                                        0x00};

                    v[12] ^= s[0];
                    v[13] ^= s[1];

                    s = {finalizator, 0};

                    v[14] ^= s[0];
                    v[15] ^= s[1];

                    s.fill(0);

                    policy_type::template round<0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15>(v, block);
                    policy_type::template round<14, 10, 4, 8, 9, 15, 13, 6, 1, 12, 0, 2, 11, 7, 5, 3>(v, block);
                    policy_type::template round<11, 8, 12, 0, 5, 2, 15, 13, 10, 14, 3, 6, 7, 1, 9, 4>(v, block);
                    policy_type::template round<7, 9, 3, 1, 13, 12, 11, 14, 2, 6, 5, 10, 4, 0, 15, 8>(v, block);
                    policy_type::template round<9, 0, 5, 7, 2, 4, 10, 15, 14, 1, 11, 12, 6, 8, 3, 13>(v, block);
                    policy_type::template round<2, 12, 6, 10, 0, 11, 8, 3, 4, 13, 7, 5, 15, 14, 1, 9>(v, block);
                    policy_type::template round<12, 5, 1, 15, 14, 13, 4, 10, 0, 7, 6, 3, 9, 2, 8, 11>(v, block);
                    policy_type::template round<13, 11, 7, 14, 12, 1, 3, 9, 5, 0, 15, 4, 8, 6, 2, 10>(v, block);
                    policy_type::template round<6, 15, 14, 9, 11, 3, 0, 8, 12, 2, 13, 7, 1, 4, 10, 5>(v, block);
                    policy_type::template round<10, 2, 8, 4, 7, 6, 1, 5, 15, 11, 9, 14, 3, 12, 13, 0>(v, block);
                    policy_type::template round<0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15>(v, block);
                    policy_type::template round<14, 10, 4, 8, 9, 15, 13, 6, 1, 12, 0, 2, 11, 7, 5, 3>(v, block);

                    for (size_t i = 0; i < state_words; i++) {
                        state[i] ^= v[i] ^ v[i + state_words];
                    }
                }
            };

            /*!
             * @brief Blake2b. A recently designed hashes function. Very fast on 64-bit processors.
             * Can output a hashes of any length between 1 and 64 bytes, this is specified by passing
             * a value to the constructor with the desired length.
             *
             * @ingroup hashes
             * @tparam DigestBits
             */
            template<std::size_t DigestBits>
            class blake2b {
                typedef detail::blake2b_policy<DigestBits> policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                constexpr static const std::size_t digest_bits = DigestBits;
                typedef typename policy_type::digest_type digest_type;

                struct construction {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t length_bits = policy_type::length_bits;
                        constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                    };

                    typedef haifa_construction<params_type, typename policy_type::iv_generator,
                                               blake2b_compressor<DigestBits>, detail::blake2b_padding<policy_type>>
                        type;
                };

                template<typename StateAccumulator, std::size_t ValueBits>
                struct stream_processor {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t value_bits = ValueBits;
                    };

                    typedef block_stream_processor<construction, StateAccumulator, params_type> type;
                };
            };

        }    // namespace hashes
    }        // namespace crypto3
}    // namespace boost

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_SQRT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/arithmetic/functions/sqrt.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/sqr.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm_sqrt_pd(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm_sqrt_ps(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<integer_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return sqrt(tofloat(a0));
    }
  };
} } }

#endif
#endif