// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// InnerExplicit.hpp
// This file is part of the Garamon for e3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file InnerExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades inner products of e3ga.

#ifndef E3GA_INNER_PRODUCT_EXPLICIT_HPP__
#define E3GA_INNER_PRODUCT_EXPLICIT_HPP__
#pragma once

#include <Eigen/Core>

#include "e3ga/Mvec.hpp"
#include "e3ga/Inner.hpp"
#include "e3ga/Constants.hpp"

/*!
 * @namespace e3ga
 */
namespace e3ga {
    template<typename T> class Mvec;

    /// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_0_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_0_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_0_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_0_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_1_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_1_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0) + mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(2)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_1_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(1)*mv2.coeff(0) - mv1.coeff(2)*mv2.coeff(1);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(0) - mv1.coeff(2)*mv2.coeff(2);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(1)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_1_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_2_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_2_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(1)*mv2.coeff(2);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(0) + mv1.coeff(2)*mv2.coeff(2);
		mv3.coeffRef(2) += -mv1.coeff(1)*mv2.coeff(0) - mv1.coeff(2)*mv2.coeff(1);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_2_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(0) - mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(2)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_2_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_3_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_3_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_3_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_3_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(0);
	}

    template<typename T>
	std::array<std::array<std::function<void(const Eigen::Matrix<T, Eigen::Dynamic, 1> & , const Eigen::Matrix<T, Eigen::Dynamic, 1> & , Eigen::Matrix<T, Eigen::Dynamic, 1>&)>, 4>, 4> innerFunctionsContainer = {{
		{{inner_0_0<T>,inner_0_1<T>,inner_0_2<T>,inner_0_3<T>}},
		{{inner_1_0<T>,inner_1_1<T>,inner_1_2<T>,inner_1_3<T>}},
		{{inner_2_0<T>,inner_2_1<T>,inner_2_2<T>,inner_2_3<T>}},
		{{inner_3_0<T>,inner_3_1<T>,inner_3_2<T>,inner_3_3<T>}}
	}};

}/// End of Namespace

#endif // E3GA_INNER_PRODUCT_EXPLICIT_HPP__


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

#include "geometry/homogeneous_transformation.h"

namespace gca {

  homogeneous_transform
  apply(const point d, const homogeneous_transform& t) {
    return std::make_pair(t.first, t.second + to_vector(d));
  }

  point apply(const homogeneous_transform& t, const point p) {
    return times_3(t.first, p) + from_vector(t.second);
  }

  triangular_mesh apply(const homogeneous_transform& t, const triangular_mesh& m) {
    triangular_mesh rotated =
      m.apply([t](const point p)
	      { return times_3(t.first, p); });
    triangular_mesh shifted =
      rotated.apply_to_vertices([t](const point p)
				{ return p + from_vector(t.second); });
    return shifted;
  }

  boost::optional<homogeneous_transform>
  mate_planes(const plane a, const plane b, const plane c,
	      const plane ap, const plane bp, const plane cp) {

    const ublas::matrix<double> rotation =
      plane_basis_rotation(a.normal(), b.normal(), c.normal(),
			   -1*ap.normal(), -1*bp.normal(), -1*cp.normal());

    if (!within_eps(determinant(rotation), 1.0, 0.001)) {
      return boost::none;
    }

    const ublas::vector<double> displacement =
      plane_basis_displacement(rotation,
			       ap.normal(), bp.normal(), cp.normal(),
			       ap.pt(), bp.pt(), cp.pt(),
			       a.pt(), b.pt(), c.pt());

    std::pair<const ublas::matrix<double>,
	      const ublas::vector<double> > p =
      std::make_pair(rotation, displacement);
    return p;
  }

  std::vector<point> apply(const homogeneous_transform& t,
			   const std::vector<point>& pts) {
    vector<point> rpts;
    for (auto p : pts) {
      rpts.push_back(apply(t, p));
    }
    return rpts;
  }

  labeled_polygon_3 apply(const homogeneous_transform& t,
			  const labeled_polygon_3& p) {
    vector<point> rotated_verts = apply(t, p.vertices());

    vector<vector<point>> holes;
    for (auto h : p.holes()) {
      holes.push_back(apply(t, h));
    }

    polygon_3 transformed = build_clean_polygon_3(rotated_verts, holes);

    transformed.correct_winding_order(times_3(t.first, p.normal()));

    point rnorm = transformed.normal();
    point pnorm = p.normal();
    point rtnorm = times_3(t.first, p.normal());

    // cout << "Original normal             = " << pnorm << endl;
    // cout << "Transformed normal              = " << rnorm << endl;
    // cout << "Rotation of original normal = " << rtnorm << endl;

    double theta = angle_between(transformed.normal(), rtnorm);

    DBG_ASSERT(within_eps(theta, 0.0, 0.1));

    return transformed;
  }

}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
// Copyright 2018 Tom Westerhout
//
// 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 <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <iostream>
#include "Utils/random_utils.hpp"
#include <vector>
#include "Utils/json_utils.hpp"
#include "abstract_hilbert.hpp"

#ifndef NETKET_SPIN_HPP
#define NETKET_SPIN_HPP

namespace netket {

/**
  Hilbert space for integer or half-integer spins.
  Notice that here integer values are always used to represent the local quantum
  numbers, such that for example if total spin is S=3/2, the allowed quantum
  numbers are -3,-1,1,3, and if S=1 we have -2,0,2.
*/

class Spin : public AbstractHilbert {
  double S_;
  double totalS_;
  bool constraintSz_;

  std::vector<double> local_;

  int nstates_;

  int nspins_;

 public:
  explicit Spin(const json &pars) {
    const int nspins = FieldVal(pars["Hilbert"], "Nspins", "Hilbert");
    const double S = FieldVal(pars["Hilbert"], "S", "Hilbert");

    Init(nspins, S);

    if (FieldExists(pars["Hilbert"], "TotalSz")) {
      SetConstraint(pars["Hilbert"]["TotalSz"]);
    } else {
      constraintSz_ = false;
    }
  }

  void Init(int nspins, double S) {
    S_ = S;
    nspins_ = nspins;

    if (S <= 0) {
      throw InvalidInputError("Invalid spin value");
    }

    if (std::floor(2. * S) != 2. * S) {
      throw InvalidInputError("Spin value is neither integer nor half integer");
    }

    nstates_ = std::floor(2. * S) + 1;

    local_.resize(nstates_);

    int sp = -std::floor(2. * S);
    for (int i = 0; i < nstates_; i++) {
      local_[i] = sp;
      sp += 2;
    }
  }

  void SetConstraint(double totalS) {
    constraintSz_ = true;
    totalS_ = totalS;
  }

  bool IsDiscrete() const override { return true; }

  int LocalSize() const override { return nstates_; }

  int Size() const override { return nspins_; }

  std::vector<double> LocalStates() const override { return local_; }

  void RandomVals(Eigen::VectorXd &state,
                  netket::default_random_engine &rgen) const override {
    std::uniform_int_distribution<int> distribution(0, nstates_ - 1);

    assert(state.size() == nspins_);

    if (!constraintSz_) {
      // unconstrained random
      for (int i = 0; i < state.size(); i++) {
        state(i) = 2. * (distribution(rgen) - S_);
      }
    } else if (S_ == 0.5) {
      using std::begin;
      using std::end;
      // Magnetisation as a count
      auto const m = static_cast<int>(2 * totalS_);
      if (std::abs(m) > nspins_) {
        throw InvalidInputError(
            "Cannot fix the total magnetization: 2|M| cannot "
            "exceed Nspins.");
      }
      if ((nspins_ + m) % 2 != 0) {
        throw InvalidInputError(
            "Cannot fix the total magnetization: Nspins + "
            "totalSz must be even.");
      }
      auto const nup = (nspins_ + m) / 2;
      auto const ndown = (nspins_ - m) / 2;
      std::fill_n(state.data(), nup, 1.0);
      std::fill_n(state.data() + nup, ndown, -1.0);
      std::shuffle(state.data(), state.data() + nspins_, rgen);
      return;
    } else {
      std::vector<int> sites;
      for (int i = 0; i < nspins_; ++i) sites.push_back(i);

      state.setConstant(-2 * S_);
      int ss = nspins_;

      for (int i = 0; i < S_ * nspins_ + totalS_; ++i) {
        std::uniform_int_distribution<int> distribution_ss(0, ss - 1);
        int s = distribution_ss(rgen);
        state(sites[s]) += 2;
        if (state(sites[s]) > 2 * S_ - 1) {
          sites.erase(sites.begin() + s);
          ss -= 1;
        }
      }
    }
  }

  void UpdateConf(Eigen::VectorXd &v, const std::vector<int> &tochange,
                  const std::vector<double> &newconf) const override {
    assert(v.size() == nspins_);

    int i = 0;
    for (auto sf : tochange) {
      v(sf) = newconf[i];
      i++;
    }
  }
};

}  // namespace netket
#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2012 Ferdinando Ametrano
 Copyright (C) 2006 Mark Joshi
 Copyright (C) 2006 StatPro Italia srl
 Copyright (C) 2007 Cristina Duminuco
 Copyright (C) 2007 Chiara Fornarola
 Copyright (C) 2013 Gary Kennedy
 Copyright (C) 2015 Peter Caspers
 Copyright (C) 2017 Klaus Spanderen
 Copyright (C) 2019 Wojciech Ślusarski
 Copyright (C) 2020 Marcin Rybacki

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/pricingengines/blackformula.hpp>
#include <ql/math/functional.hpp>
#include <ql/math/solvers1d/newtonsafe.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/math/special_functions/atanh.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

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

namespace {
    void checkParameters(QuantLib::Real strike,
                         QuantLib::Real forward,
                         QuantLib::Real displacement)
    {
        QL_REQUIRE(displacement >= 0.0, "displacement ("
                                            << displacement
                                            << ") must be non-negative");
        QL_REQUIRE(strike + displacement >= 0.0,
                   "strike + displacement (" << strike << " + " << displacement
                                             << ") must be non-negative");
        QL_REQUIRE(forward + displacement > 0.0, "forward + displacement ("
                                                     << forward << " + "
                                                     << displacement
                                                     << ") must be positive");
    }
}

namespace QuantLib {

    Real blackFormula(Option::Type optionType,
                      Real strike,
                      Real forward,
                      Real stdDev,
                      Real discount,
                      Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        auto sign = Integer(optionType);

        if (stdDev == 0.0)
            return std::max((forward-strike) * sign, Real(0.0)) * discount;

        forward = forward + displacement;
        strike = strike + displacement;

        // since displacement is non-negative strike==0 iff displacement==0
        // so returning forward*discount is OK
        if (strike==0.0)
            return (optionType==Option::Call ? forward*discount : 0.0);

        Real d1 = std::log(forward/strike)/stdDev + 0.5*stdDev;
        Real d2 = d1 - stdDev;
        CumulativeNormalDistribution phi;
        Real nd1 = phi(sign * d1);
        Real nd2 = phi(sign * d2);
        Real result = discount * sign * (forward*nd1 - strike*nd2);
        QL_ENSURE(result>=0.0,
                  "negative value (" << result << ") for " <<
                  stdDev << " stdDev, " <<
                  optionType << " option, " <<
                  strike << " strike , " <<
                  forward << " forward");
        return result;
    }

    Real blackFormula(const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                      Real forward,
                      Real stdDev,
                      Real discount,
                      Real displacement) {
        return blackFormula(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount, displacement);
    }

    Real blackFormulaForwardDerivative(Option::Type optionType,
                                       Real strike,
                                       Real forward,
                                       Real stdDev,
                                       Real discount,
                                       Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        auto sign = Integer(optionType);

        if (stdDev == 0.0)
            return sign * std::max(1.0 * boost::math::sign((forward - strike) * sign), 0.0) * discount;

        forward = forward + displacement;
        strike = strike + displacement;

        if (strike == 0.0)
            return (optionType == Option::Call ? discount : 0.0);

        Real d1 = std::log(forward/strike)/stdDev + 0.5*stdDev;
        CumulativeNormalDistribution phi;
        return sign * phi(sign * d1) * discount;
    }

    Real blackFormulaForwardDerivative(const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                                       Real forward,
                                       Real stdDev,
                                       Real discount,
                                       Real displacement)
    {
        return blackFormulaForwardDerivative(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount, displacement);
    }

    Real blackFormulaImpliedStdDevApproximation(Option::Type optionType,
                                                Real strike,
                                                Real forward,
                                                Real blackPrice,
                                                Real discount,
                                                Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(blackPrice>=0.0,
                   "blackPrice (" << blackPrice << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        Real stdDev;
        forward = forward + displacement;
        strike = strike + displacement;
        if (strike==forward)
            // Brenner-Subrahmanyan (1988) and Feinstein (1988) ATM approx.
            stdDev = blackPrice/discount*std::sqrt(2.0 * M_PI)/forward;
        else {
            // Corrado and Miller extended moneyness approximation
            Real moneynessDelta = Integer(optionType) * (forward-strike);
            Real moneynessDelta_2 = moneynessDelta/2.0;
            Real temp = blackPrice/discount - moneynessDelta_2;
            Real moneynessDelta_PI = moneynessDelta*moneynessDelta/M_PI;
            Real temp2 = temp*temp-moneynessDelta_PI;
            if (temp2<0.0) // approximation breaks down, 2 alternatives:
                // 1. zero it
                temp2=0.0;
                // 2. Manaster-Koehler (1982) efficient Newton-Raphson seed
                //return std::fabs(std::log(forward/strike))*std::sqrt(2.0);
            temp2 = std::sqrt(temp2);
            temp += temp2;
            temp *= std::sqrt(2.0 * M_PI);
            stdDev = temp/(forward+strike);
        }
        QL_ENSURE(stdDev>=0.0,
                  "stdDev (" << stdDev << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDevApproximation(
                      const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                      Real forward,
                      Real blackPrice,
                      Real discount,
                      Real displacement) {
        return blackFormulaImpliedStdDevApproximation(payoff->optionType(),
            payoff->strike(), forward, blackPrice, discount, displacement);
    }

    Real blackFormulaImpliedStdDevChambers(Option::Type optionType,
                                                Real strike,
                                                Real forward,
                                                Real blackPrice,
                                                Real blackAtmPrice,
                                                Real discount,
                                                Real displacement) {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(blackPrice >= 0.0,
                   "blackPrice (" << blackPrice << ") must be non-negative");
        QL_REQUIRE(blackAtmPrice >= 0.0, "blackAtmPrice ("
                                             << blackAtmPrice
                                             << ") must be non-negative");
        QL_REQUIRE(discount > 0.0, "discount (" << discount
                                                << ") must be positive");

        Real stdDev;

        forward = forward + displacement;
        strike = strike + displacement;
        blackPrice /= discount;
        blackAtmPrice /= discount;

        Real s0 = M_SQRT2 * M_SQRTPI * blackAtmPrice /
                  forward; // Brenner-Subrahmanyam formula
        Real priceAtmVol =
            blackFormula(optionType, strike, forward, s0, 1.0, 0.0);
        Real dc = blackPrice - priceAtmVol;

        if (close(dc, 0.0)) {
            stdDev = s0;
        } else {
            Real d1 =
                blackFormulaStdDevDerivative(strike, forward, s0, 1.0, 0.0);
            Real d2 = blackFormulaStdDevSecondDerivative(strike, forward, s0,
                                                         1.0, 0.0);
            Real ds = 0.0;
            Real tmp = d1 * d1 + 2.0 * d2 * dc;
            if (std::fabs(d2) > 1E-10 && tmp >= 0.0)
                ds = (-d1 + std::sqrt(tmp)) / d2; // second order approximation
            else
                if(std::fabs(d1) > 1E-10)
                    ds = dc / d1; // first order approximation
            stdDev = s0 + ds;
        }

        QL_ENSURE(stdDev >= 0.0, "stdDev (" << stdDev
                                            << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDevChambers(
        const ext::shared_ptr<PlainVanillaPayoff> &payoff,
        Real forward,
        Real blackPrice,
        Real blackAtmPrice,
        Real discount,
        Real displacement) {
        return blackFormulaImpliedStdDevChambers(
            payoff->optionType(), payoff->strike(), forward, blackPrice,
            blackAtmPrice, discount, displacement);
    }

    namespace {
        Real Af(Real x) {
            return 0.5*(1.0+boost::math::sign(x)
                *std::sqrt(1.0-std::exp(-M_2_PI*x*x)));
        }
    }

    Real blackFormulaImpliedStdDevApproximationRS(
        Option::Type type, Real K, Real F,
        Real marketValue, Real df, Real displacement) {

        checkParameters(K, F, displacement);
        QL_REQUIRE(marketValue >= 0.0,
                   "blackPrice (" << marketValue << ") must be non-negative");
        QL_REQUIRE(df > 0.0, "discount (" << df << ") must be positive");

        F = F + displacement;
        K = K + displacement;

        const Real ey = F/K;
        const Real ey2 = ey*ey;
        const Real y = std::log(ey);
        const Real alpha = marketValue/(K*df);
        const Real R = 2*alpha + ((type == Option::Call) ? -ey+1.0 : ey-1.0);
        const Real R2 = R*R;

        const Real a = std::exp((1.0-M_2_PI)*y);
        const Real A = square<Real>()(a - 1.0/a);
        const Real b = std::exp(M_2_PI*y);
        const Real B = 4.0*(b + 1/b)
            - 2*K/F*(a + 1.0/a)*(ey2 + 1 - R2);
        const Real C = (R2-square<Real>()(ey-1))*(square<Real>()(ey+1)-R2)/ey2;

        const Real beta = 2*C/(B+std::sqrt(B*B+4*A*C));
        const Real gamma = -M_PI_2*std::log(beta);

        if (y >= 0.0) {
            const Real M0 = K*df*(
                (type == Option::Call) ? ey*Af(std::sqrt(2*y)) - 0.5
                                       : 0.5-ey*Af(-std::sqrt(2*y)));

            if (marketValue <= M0)
                return std::sqrt(gamma+y)-std::sqrt(gamma-y);
            else
                return std::sqrt(gamma+y)+std::sqrt(gamma-y);
        }
        else {
            const Real M0 = K*df*(
                (type == Option::Call) ? 0.5*ey - Af(-std::sqrt(-2*y))
                                       : Af(std::sqrt(-2*y)) - 0.5*ey);

            if (marketValue <= M0)
                return std::sqrt(gamma-y)-std::sqrt(gamma+y);
            else
                return std::sqrt(gamma+y)+std::sqrt(gamma-y);
        }
    }

    Real blackFormulaImpliedStdDevApproximationRS(
        const ext::shared_ptr<PlainVanillaPayoff> &payoff,
        Real F, Real marketValue,
        Real df, Real displacement) {

        return blackFormulaImpliedStdDevApproximationRS(
            payoff->optionType(), payoff->strike(),
            F, marketValue, df, displacement);
    }

    class BlackImpliedStdDevHelper {
      public:
        BlackImpliedStdDevHelper(Option::Type optionType,
                                 Real strike,
                                 Real forward,
                                 Real undiscountedBlackPrice,
                                 Real displacement = 0.0)
        : halfOptionType_(0.5 * Integer(optionType)),
          signedStrike_(Integer(optionType) * (strike+displacement)),
          signedForward_(Integer(optionType) * (forward+displacement)),
          undiscountedBlackPrice_(undiscountedBlackPrice)
        {
            checkParameters(strike, forward, displacement);
            QL_REQUIRE(undiscountedBlackPrice>=0.0,
                       "undiscounted Black price (" <<
                       undiscountedBlackPrice << ") must be non-negative");
            signedMoneyness_ = Integer(optionType) * std::log((forward+displacement)/(strike+displacement));
        }

        Real operator()(Real stdDev) const {
            #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(stdDev>=0.0,
                       "stdDev (" << stdDev << ") must be non-negative");
            #endif
            if (stdDev==0.0)
                return std::max(signedForward_-signedStrike_, Real(0.0))
                                                   - undiscountedBlackPrice_;
            Real temp = halfOptionType_*stdDev;
            Real d = signedMoneyness_/stdDev;
            Real signedD1 = d + temp;
            Real signedD2 = d - temp;
            Real result = signedForward_ * N_(signedD1)
                - signedStrike_ * N_(signedD2);
            // numerical inaccuracies can yield a negative answer
            return std::max(Real(0.0), result) - undiscountedBlackPrice_;
        }
        Real derivative(Real stdDev) const {
            #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(stdDev>=0.0,
                       "stdDev (" << stdDev << ") must be non-negative");
            #endif
            Real signedD1 = signedMoneyness_/stdDev + halfOptionType_*stdDev;
            return signedForward_*N_.derivative(signedD1);
        }
      private:
        Real halfOptionType_;
        Real signedStrike_, signedForward_;
        Real undiscountedBlackPrice_, signedMoneyness_;
        CumulativeNormalDistribution N_;
    };

    Real blackFormulaImpliedStdDev(Option::Type optionType,
                                   Real strike,
                                   Real forward,
                                   Real blackPrice,
                                   Real discount,
                                   Real displacement,
                                   Real guess,
                                   Real accuracy,
                                   Natural maxIterations)
    {
        checkParameters(strike, forward, displacement);

        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        QL_REQUIRE(blackPrice>=0.0,
                   "option price (" << blackPrice << ") must be non-negative");
        // check the price of the "other" option implied by put-call paity
        Real otherOptionPrice = blackPrice - Integer(optionType) * (forward-strike)*discount;
        QL_REQUIRE(otherOptionPrice>=0.0,
                   "negative " << Option::Type(-1*optionType) <<
                   " price (" << otherOptionPrice <<
                   ") implied by put-call parity. No solution exists for " <<
                   optionType << " strike " << strike <<
                   ", forward " << forward <<
                   ", price " << blackPrice <<
                   ", deflator " << discount);

        // solve for the out-of-the-money option which has
        // greater vega/price ratio, i.e.
        // it is numerically more robust for implied vol calculations
        if (optionType==Option::Put && strike>forward) {
            optionType = Option::Call;
            blackPrice = otherOptionPrice;
        }
        if (optionType==Option::Call && strike<forward) {
            optionType = Option::Put;
            blackPrice = otherOptionPrice;
        }

        strike = strike + displacement;
        forward = forward + displacement;

        if (guess==Null<Real>())
            guess = blackFormulaImpliedStdDevApproximation(
                optionType, strike, forward, blackPrice, discount, displacement);
        else
            QL_REQUIRE(guess>=0.0,
                       "stdDev guess (" << guess << ") must be non-negative");
        BlackImpliedStdDevHelper f(optionType, strike, forward,
                                   blackPrice/discount);
        NewtonSafe solver;
        solver.setMaxEvaluations(maxIterations);
        Real minSdtDev = 0.0, maxStdDev = 24.0; // 24 = 300% * sqrt(60)
        Real stdDev = solver.solve(f, accuracy, guess, minSdtDev, maxStdDev);
        QL_ENSURE(stdDev>=0.0,
                  "stdDev (" << stdDev << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDev(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real blackPrice,
                        Real discount,
                        Real displacement,
                        Real guess,
                        Real accuracy,
                        Natural maxIterations) {
        return blackFormulaImpliedStdDev(payoff->optionType(), payoff->strike(),
            forward, blackPrice, discount, displacement, guess, accuracy, maxIterations);
    }

    namespace {
        Real Np(Real x, Real v) {
            return CumulativeNormalDistribution()(x/v + 0.5*v);
        }
        Real Nm(Real x, Real v) {
            return std::exp(-x)*CumulativeNormalDistribution()(x/v - 0.5*v);
        }
        Real phi(Real x, Real v) {
            const Real ax = 2*std::fabs(x);
            const Real v2 = v*v;
            return (v2-ax)/(v2+ax);
        }
        Real F(Real v, Real x, Real cs, Real w) {
            return cs+Nm(x,v)+w*Np(x,v);
        }
        Real G(Real v, Real x, Real cs, Real w) {
            const Real q = F(v,x,cs,w)/(1+w);

            // Acklam's inverse w/o Halley's refinement step
            // does not provide enough accuracy. But both together are
            // slower than the boost replacement.
            const Real k = MaddockInverseCumulativeNormal()(q);

            return k + std::sqrt(k*k + 2*std::fabs(x));
        }
    }

    Real blackFormulaImpliedStdDevLiRS(
        Option::Type optionType,
        Real strike,
        Real forward,
        Real blackPrice,
        Real discount,
        Real displacement,
        Real guess,
        Real w,
        Real accuracy,
        Natural maxIterations) {

        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        QL_REQUIRE(blackPrice>=0.0,
                   "option price (" << blackPrice << ") must be non-negative");

        strike = strike + displacement;
        forward = forward + displacement;

        if (guess == Null<Real>()) {
            guess = blackFormulaImpliedStdDevApproximationRS(
                optionType, strike, forward,
                blackPrice, discount, displacement);
        }
        else {
            QL_REQUIRE(guess>=0.0,
                "stdDev guess (" << guess << ") must be non-negative");
        }

        Real x = std::log(forward/strike);
        Real cs = (optionType == Option::Call)
            ? blackPrice / (forward*discount)
            : (blackPrice/ (forward*discount) + 1.0 - strike/forward);

        QL_REQUIRE(cs >= 0.0, "normalized call price (" << cs
                   << ") must be positive");

        if (x > 0) {
            // use in-out duality
            cs = forward/strike*cs + 1.0 - forward/strike;
            QL_REQUIRE(cs >= 0.0, "negative option price from in-out duality");
            x = -x;
        }

        Size nIter = 0;
        Real dv, vk, vkp1 = guess;

        do {
            vk = vkp1;
            const Real alphaK = (1+w)/(1+phi(x,vk));
            vkp1 = alphaK*G(vk,x,cs,w) + (1-alphaK)*vk;
            dv = std::fabs(vkp1 - vk);
        } while (dv > accuracy && ++nIter < maxIterations);

        QL_REQUIRE(dv <= accuracy, "max iterations exceeded");
        QL_REQUIRE(vk >= 0.0, "stdDev (" << vk << ") must be non-negative");

        return vk;
    }

    Real blackFormulaImpliedStdDevLiRS(
        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
        Real forward,
        Real blackPrice,
        Real discount,
        Real displacement,
        Real guess,
        Real omega,
        Real accuracy,
        Natural maxIterations) {

        return blackFormulaImpliedStdDevLiRS(
            payoff->optionType(), payoff->strike(),
            forward, blackPrice, discount, displacement,
            guess, omega, accuracy, maxIterations);
    }

    Real blackFormulaCashItmProbability(Option::Type optionType,
                                        Real strike,
                                        Real forward,
                                        Real stdDev,
                                        Real displacement) {
        checkParameters(strike, forward, displacement);

        auto sign = Integer(optionType);

        if (stdDev==0.0)
            return (forward * sign > strike * sign ? 1.0 : 0.0);

        forward = forward + displacement;
        strike = strike + displacement;
        if (strike==0.0)
            return (optionType == Option::Call ? 1.0 : 0.0);
        Real d2 = std::log(forward/strike)/stdDev - 0.5*stdDev;
        CumulativeNormalDistribution phi;
        return phi(sign * d2);
    }

    Real blackFormulaCashItmProbability(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real displacement) {
        return blackFormulaCashItmProbability(payoff->optionType(),
            payoff->strike(), forward, stdDev , displacement);
    }

    Real blackFormulaAssetItmProbability(
                        Option::Type optionType,
                        Real strike,
                        Real forward,
                        Real stdDev,
                        Real displacement) {
        checkParameters(strike, forward, displacement);

        auto sign = Integer(optionType);

        if (stdDev==0.0)
            return (forward * sign < strike * sign ? 1.0 : 0.0);

        forward = forward + displacement;
        strike = strike + displacement;
        if (strike == 0.0)
            return (optionType == Option::Call ? 1.0 : 0.0);
        Real d1 = std::log(forward/strike)/stdDev + 0.5*stdDev;
        CumulativeNormalDistribution phi;
        return phi(sign * d1);
    }

    Real blackFormulaAssetItmProbability(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real displacement) {
        return blackFormulaAssetItmProbability(payoff->optionType(),
            payoff->strike(), forward, stdDev , displacement);
    }

    Real blackFormulaVolDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real expiry,
                                      Real discount,
                                      Real displacement)
    {
        return  blackFormulaStdDevDerivative(strike,
                                     forward,
                                     stdDev,
                                     discount,
                                     displacement)*std::sqrt(expiry);
    }

    Real blackFormulaStdDevDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real discount,
                                      Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        forward = forward + displacement;
        strike = strike + displacement;

        if (stdDev==0.0 || strike==0.0)
            return 0.0;

        Real d1 = std::log(forward/strike)/stdDev + .5*stdDev;
        return discount * forward *
            CumulativeNormalDistribution().derivative(d1);
    }

    Real blackFormulaStdDevDerivative(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount,
                        Real displacement) {
        return blackFormulaStdDevDerivative(payoff->strike(), forward,
                                     stdDev, discount, displacement);
    }

    Real blackFormulaStdDevSecondDerivative(Rate strike,
                                            Rate forward,
                                            Real stdDev,
                                            Real discount,
                                            Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        forward = forward + displacement;
        strike = strike + displacement;

        if (stdDev==0.0 || strike==0.0)
            return 0.0;

        Real d1 = std::log(forward/strike)/stdDev + .5*stdDev;
        Real d1p = -std::log(forward/strike)/(stdDev*stdDev) + .5;
        return discount * forward *
            NormalDistribution().derivative(d1) * d1p;
    }

    Real blackFormulaStdDevSecondDerivative(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount,
                        Real displacement) {
        return blackFormulaStdDevSecondDerivative(payoff->strike(), forward,
                                     stdDev, discount, displacement);
    }

    Real bachelierBlackFormula(Option::Type optionType,
                               Real strike,
                               Real forward,
                               Real stdDev,
                               Real discount)
    {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");
        Real d = (forward-strike) * Integer(optionType), h = d / stdDev;
        if (stdDev==0.0)
            return discount*std::max(d, 0.0);
        CumulativeNormalDistribution phi;
        Real result = discount*(stdDev*phi.derivative(h) + d*phi(h));
        QL_ENSURE(result>=0.0,
                  "negative value (" << result << ") for " <<
                  stdDev << " stdDev, " <<
                  optionType << " option, " <<
                  strike << " strike , " <<
                  forward << " forward");
        return result;
    }

    Real bachelierBlackFormula(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount) {
        return bachelierBlackFormula(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount);
    }

    Real bachelierBlackFormulaForwardDerivative(
        Option::Type optionType, Real strike, Real forward, Real stdDev, Real discount)
    {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");
        auto sign = Integer(optionType);
        if (stdDev == 0.0)
            return sign * std::max(1.0 * boost::math::sign((forward - strike) * sign), 0.0) * discount;
        Real d = (forward - strike) * sign, h = d / stdDev;
        CumulativeNormalDistribution phi;
        return sign * phi(h) * discount;
    }

    Real bachelierBlackFormulaForwardDerivative(
        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
        Real forward,
        Real stdDev,
        Real discount)
    {
        return bachelierBlackFormulaForwardDerivative(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount);
    }

    static Real h(Real eta) {

        const static Real  A0          = 3.994961687345134e-1;
        const static Real  A1          = 2.100960795068497e+1;
        const static Real  A2          = 4.980340217855084e+1;
        const static Real  A3          = 5.988761102690991e+2;
        const static Real  A4          = 1.848489695437094e+3;
        const static Real  A5          = 6.106322407867059e+3;
        const static Real  A6          = 2.493415285349361e+4;
        const static Real  A7          = 1.266458051348246e+4;

        const static Real  B0          = 1.000000000000000e+0;
        const static Real  B1          = 4.990534153589422e+1;
        const static Real  B2          = 3.093573936743112e+1;
        const static Real  B3          = 1.495105008310999e+3;
        const static Real  B4          = 1.323614537899738e+3;
        const static Real  B5          = 1.598919697679745e+4;
        const static Real  B6          = 2.392008891720782e+4;
        const static Real  B7          = 3.608817108375034e+3;
        const static Real  B8          = -2.067719486400926e+2;
        const static Real  B9          = 1.174240599306013e+1;

        QL_REQUIRE(eta>=0.0,
                       "eta (" << eta << ") must be non-negative");

        const Real num = A0 + eta * (A1 + eta * (A2 + eta * (A3 + eta * (A4 + eta
                    * (A5 + eta * (A6 + eta * A7))))));

        const Real den = B0 + eta * (B1 + eta * (B2 + eta * (B3 + eta * (B4 + eta
                    * (B5 + eta * (B6 + eta * (B7 + eta * (B8 + eta * B9))))))));

        return std::sqrt(eta) * (num / den);

    }

    Real bachelierBlackFormulaImpliedVol(Option::Type optionType,
                                   Real strike,
                                   Real forward,
                                   Real tte,
                                   Real bachelierPrice,
                                   Real discount) {

        const static Real SQRT_QL_EPSILON = std::sqrt(QL_EPSILON);

        QL_REQUIRE(tte>0.0,
                   "tte (" << tte << ") must be positive");

        Real forwardPremium = bachelierPrice/discount;

        Real straddlePremium;
        if (optionType==Option::Call){
            straddlePremium = 2.0 * forwardPremium - (forward - strike);
        } else {
            straddlePremium = 2.0 * forwardPremium + (forward - strike);
        }

        Real nu = (forward - strike) / straddlePremium;
        QL_REQUIRE(nu<1.0 || close_enough(nu,1.0),
                   "nu (" << nu << ") must be <= 1.0");
        QL_REQUIRE(nu>-1.0 || close_enough(nu,-1.0),
                     "nu (" << nu << ") must be >= -1.0");

        nu = std::max(-1.0 + QL_EPSILON, std::min(nu,1.0 - QL_EPSILON));

        // nu / arctanh(nu) -> 1 as nu -> 0
        Real eta = (std::fabs(nu) < SQRT_QL_EPSILON) ? 1.0 : nu / boost::math::atanh(nu);

        Real heta = h(eta);

        Real impliedBpvol = std::sqrt(M_PI / (2 * tte)) * straddlePremium * heta;

        return impliedBpvol;
    }

        Real bachelierBlackFormulaStdDevDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real discount)
    {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        if (stdDev==0.0)
            return 0.0;

        Real d1 = (forward - strike)/stdDev;
        return discount *
            CumulativeNormalDistribution().derivative(d1);
    }

    Real bachelierBlackFormulaStdDevDerivative(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount) {
        return bachelierBlackFormulaStdDevDerivative(payoff->strike(), forward,
                                     stdDev, discount);
    }

    Real bachelierBlackFormulaAssetItmProbability(
                        Option::Type optionType,
                        Real strike,
                        Real forward,
                        Real stdDev) {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        Real d = (forward - strike) * Integer(optionType), h = d / stdDev;
        if (stdDev==0.0)
            return std::max(d, 0.0);
        CumulativeNormalDistribution phi;
        Real result = phi(h);
        return result;
    }

    Real bachelierBlackFormulaAssetItmProbability(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev) {
        return bachelierBlackFormulaAssetItmProbability(payoff->optionType(),
            payoff->strike(), forward, stdDev);
    }

    Real bachelierBlackFormulaDelta(Option::Type optionType,
		                            Real         strike,
                                    Real         forward,
                                    Real         stdDev,
                                    Real         discount) {
        QL_REQUIRE(stdDev>=0.0,   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,  "discount (" << discount << ") must be positive");
        Real d = (forward-strike)*Integer(optionType), h = d/stdDev;
        CumulativeNormalDistribution phi;
		return Integer(optionType)*discount*phi(h);
	}

    Real bachelierBlackFormulaGamma(Option::Type optionType,
		                            Real         strike,
                                    Real         forward,
                                    Real         stdDev,
                                    Real         discount) {
        QL_REQUIRE(stdDev>=0.0,   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,  "discount (" << discount << ") must be positive");
        Real d = (forward-strike)*Integer(optionType), h = d/stdDev;
        CumulativeNormalDistribution phi;
		return discount/stdDev*phi.derivative(h);
	}

}


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

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

// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_AREA_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_AREA_HPP

#include <type_traits>

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/extensions/nsphere/core/radius.hpp>
#include <boost/geometry/extensions/nsphere/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace area
{

template<typename C>
struct circle_area
{
    typedef typename coordinate_type<C>::type coordinate_type;

    // Returning the coordinate precision, but if integer, returning a double
    typedef std::conditional_t
        <
            std::is_integral<coordinate_type>::value,
            double,
            coordinate_type
        > return_type;

    template <typename S>
    static inline return_type apply(C const& c, S const&)
    {
        // Currently only works for Cartesian circles
        // Todo: use strategy
        // Todo: use concept
        assert_dimension<C, 2>();

        return_type r = get_radius<0>(c);
        r *= r * boost::math::constants::pi<return_type>();
        return r;
    }
};

}} // namespace detail::area

#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Geometry>
struct area<Geometry, nsphere_tag>
    : detail::area::circle_area<Geometry>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_AREA_HPP


#ifndef INCLUDE_SWIFT_VIO_SIMPLE_IMU_ODOMETRY_HPP_
#define INCLUDE_SWIFT_VIO_SIMPLE_IMU_ODOMETRY_HPP_

#include <Eigen/Core>
#include <swift_vio/Measurements.hpp>

#include <okvis/kinematics/Transformation.hpp>
#include <okvis/kinematics/operators.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/ImuMeasurements.hpp>
#include <okvis/Variables.hpp>
#include <okvis/assert_macros.hpp>

namespace swift_vio {
namespace ode {
template<typename Scalar>
int predictStates(const GenericImuMeasurementDeque<Scalar> & imuMeasurements,
            const Scalar gravityMag,
            std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>> &T_WS,
            Eigen::Matrix<Scalar, 9,1>& speedBgBa,
            const Scalar t_start, const Scalar t_end)
{
    Eigen::Matrix<Scalar, 3,1> r_old(T_WS.first), r_new(r_old), v_old(speedBgBa.template head<3>()), v_new(v_old);
    Eigen::Quaternion<Scalar> q_old(T_WS.second.conjugate()), q_new(q_old); //rotation from world to sensor
    assert(imuMeasurements.front().timeStamp <= t_start && imuMeasurements.back().timeStamp >= t_end);

    Scalar time = t_start;
    Scalar end = t_end;
    Scalar Delta_t = Scalar(0);
    bool hasStarted = false;
    int i = 0;
    Scalar nexttime;
    for(typename GenericImuMeasurementDeque<Scalar>::const_iterator it = imuMeasurements.begin();
          it != imuMeasurements.end(); ++it) {

      Eigen::Matrix<Scalar,3,1> omega_S_0 = it->template gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_0 = it->template accelerometers;
      Eigen::Matrix<Scalar,3,1> omega_S_1 = (it + 1)->template gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_1 = (it + 1)->template accelerometers;

      // time delta
      if ((it + 1) == imuMeasurements.end()) {
        nexttime = t_end;
      } else
        nexttime = (it + 1)->timeStamp;
      Scalar dt = nexttime - time;

      if (end < nexttime) {
        Scalar interval = nexttime - it->timeStamp;
        nexttime = t_end;
        dt = nexttime - time;
        if (dt == 0.0)
            break;
        const Scalar r = dt / interval;
        omega_S_1 = ((Scalar(1.0) - r) * omega_S_0 + r * omega_S_1).eval();
        acc_S_1 = ((Scalar(1.0) - r) * acc_S_0 + r * acc_S_1).eval();
      }

      if (dt <= Scalar(0.0)) {
        continue;
      }
      Delta_t += dt;

      if (!hasStarted) {
        hasStarted = true;
        const Scalar r = dt / (nexttime - it->timeStamp);
        omega_S_0 = (r * omega_S_0 + (Scalar(1.0) - r) * omega_S_1).eval();
        acc_S_0 = (r * acc_S_0 + (Scalar(1.0) - r) * acc_S_1).eval();
      }

      // actual propagation
      Eigen::Matrix<Scalar,3,1> a_est = Scalar(0.5)*(acc_S_0+acc_S_1) - speedBgBa.template tail<3>();
      Eigen::Matrix<Scalar,3,1> w_est = Scalar(0.5)*(omega_S_0+omega_S_1) - speedBgBa.template segment<3>(3);
      Eigen::Matrix<Scalar,3,1> gW(Scalar(0), Scalar(0), -gravityMag);

      Eigen::Quaternion<Scalar> qb = okvis::kinematics::rvec2quat(-w_est*dt);
      q_new=qb*q_old;

      Eigen::Matrix<Scalar,3,1> vel_inc1=(q_old.conjugate()._transformVector(a_est*dt)+q_new.conjugate()._transformVector(a_est*dt))*Scalar(0.5);
      Eigen::Matrix<Scalar,3,1> vel_inc2=gW*dt;

      v_new= v_old + vel_inc1+vel_inc2;
      r_new= r_old + (v_new+v_old)*dt*Scalar(0.5);

      time = nexttime;
      ++i;

      if (nexttime == t_end)
        break;

      r_old=r_new;
      v_old=v_new;
      q_old=q_new;
    }
    assert(nexttime == t_end);

    T_WS.second = q_new.conjugate();
    T_WS.first = r_new;
    speedBgBa. template head<3>()=v_new;
    return i;
}

/**
 * @brief predictStates accumulate the rotation vector given gyro measurements.
 * i.e., \f$ \int_{t_1}^{t_2} \hat{\omega} - \mathbf{b}_g \f$
 */
template <typename Scalar>
int predictStates(
    const std::deque<
        okvis::Measurement<Eigen::Vector3d>,
        Eigen::aligned_allocator<okvis::Measurement<Eigen::Vector3d>>>
        &gyroMeasurements,
    Eigen::Vector3d *rotVector, const Eigen::Matrix<Scalar, 3, 1> &gyroBias,
    const okvis::Time t_start, const okvis::Time t_end) {
  rotVector->setZero();
  assert(gyroMeasurements.front().timeStamp <= t_start &&
         gyroMeasurements.back().timeStamp >= t_end);

  okvis::Time time = t_start;
  okvis::Time end = t_end;
  Scalar Delta_t = Scalar(0);
  bool hasStarted = false;
  int i = 0;
  okvis::Time nexttime;
  for (typename std::deque<okvis::Measurement<Eigen::Vector3d>,
                           Eigen::aligned_allocator<okvis::Measurement<
                               Eigen::Vector3d>>>::const_iterator it =
           gyroMeasurements.begin();
       it != gyroMeasurements.end(); ++it) {
    Eigen::Matrix<Scalar, 3, 1> omega_S_0 = it->measurement;
    Eigen::Matrix<Scalar, 3, 1> omega_S_1 = (it + 1)->measurement;
    // time delta
    if ((it + 1) == gyroMeasurements.end()) {
      nexttime = t_end;
    } else
      nexttime = (it + 1)->timeStamp;

    Scalar dt = (Scalar)(nexttime - time).toSec();
    if (end < nexttime) {
      Scalar interval = (Scalar)(nexttime - it->timeStamp).toSec();
      nexttime = t_end;
      dt = (Scalar)(nexttime - time).toSec();
      if (dt == 0.0)
        break;
      const Scalar r = dt / interval;
      omega_S_1 = ((Scalar(1.0) - r) * omega_S_0 + r * omega_S_1).eval();
    }

    if (dt <= Scalar(0.0)) {
      continue;
    }
    Delta_t += dt;

    if (!hasStarted) {
      hasStarted = true;
      const Scalar r = dt / ((Scalar)(nexttime - it->timeStamp).toSec());
      omega_S_0 = (r * omega_S_0 + (Scalar(1.0) - r) * omega_S_1).eval();
    }

    // actual propagation
    Eigen::Matrix<Scalar, 3, 1> w_est =
        Scalar(0.5) * (omega_S_0 + omega_S_1) - gyroBias;
    (*rotVector) += w_est * dt;

    time = nexttime;
    ++i;

    if (nexttime == t_end)
      break;
  }
  assert(nexttime == t_end);
  return i;
}

// time_pair[0] timestamp of the provided state values. time_pair[0] >= time_pair[1],
template<typename Scalar>
int predictStatesBackward(const GenericImuMeasurementDeque<Scalar> & imuMeasurements,
                    const Scalar gravityMag,
                    std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>> &T_WS,
                    Eigen::Matrix<Scalar, 9,1>& speedBgBa,
                    const Scalar t_start, const Scalar t_end)
{
    Eigen::Matrix<Scalar, 3,1> r_old(T_WS.first), r_new(r_old), v_old(speedBgBa.template head<3>()), v_new(v_old);
    Eigen::Quaternion<Scalar> q_old(T_WS.second.conjugate()), q_new(q_old); //rotation from world to sensor
    assert(imuMeasurements.front().timeStamp <= t_end && imuMeasurements.back().timeStamp >= t_start);

    Scalar time = t_start;
    Scalar end = t_end;
    Scalar Delta_t = Scalar(0);
    bool hasStarted = false;
    int i = 0;
    Scalar nexttime;
    for(typename GenericImuMeasurementDeque<Scalar>::const_reverse_iterator it = imuMeasurements.rbegin();
          it != imuMeasurements.rend(); ++it) {

      Eigen::Matrix<Scalar,3,1> omega_S_0 = it->template gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_0 = it->template accelerometers;
      Eigen::Matrix<Scalar,3,1> omega_S_1 = (it + 1)->template gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_1 = (it + 1)->template accelerometers;

      // time delta
      if ((it + 1) == imuMeasurements.rend()) {
        nexttime = t_end;
      } else
        nexttime = (it + 1)->timeStamp;
      Scalar dt = nexttime - time;

      if (end > nexttime) {
        Scalar interval = nexttime - it->timeStamp;
        nexttime = t_end;
        dt = nexttime - time;
        if (dt == 0.0)
            break;
        const Scalar r = dt / interval;
        omega_S_1 = ((Scalar(1.0) - r) * omega_S_0 + r * omega_S_1).eval();
        acc_S_1 = ((Scalar(1.0) - r) * acc_S_0 + r * acc_S_1).eval();
      }

      if (dt >= Scalar(0.0)) {
        continue;
      }
      Delta_t += dt;

      if (!hasStarted) {
        hasStarted = true;
        const Scalar r = dt / (nexttime - it->timeStamp);
        omega_S_0 = (r * omega_S_0 + (Scalar(1.0) - r) * omega_S_1).eval();
        acc_S_0 = (r * acc_S_0 + (Scalar(1.0) - r) * acc_S_1).eval();
      }

      // actual propagation

      Eigen::Matrix<Scalar,3,1> a_est = Scalar(0.5)*(acc_S_0+acc_S_1) - speedBgBa.template tail<3>();
      Eigen::Matrix<Scalar,3,1> w_est = Scalar(0.5)*(omega_S_0+omega_S_1) - speedBgBa.template segment<3>(3);
      Eigen::Matrix<Scalar,3,1> gW(Scalar(0), Scalar(0), -gravityMag);

      Eigen::Quaternion<Scalar> qb = okvis::kinematics::rvec2quat(-w_est*dt);
      q_new=qb*q_old;

      Eigen::Matrix<Scalar,3,1> vel_inc1=(q_old.conjugate()._transformVector(a_est*dt)+q_new.conjugate()._transformVector(a_est*dt))*Scalar(0.5);
      Eigen::Matrix<Scalar,3,1> vel_inc2=gW*dt;

      v_new= v_old + vel_inc1+vel_inc2;
      r_new= r_old + (v_new+v_old)*dt*Scalar(0.5);

      time = nexttime;
      ++i;

      if (nexttime == t_end)
        break;

      r_old=r_new;
      v_old=v_new;
      q_old=q_new;
    }
    assert(nexttime == t_end);

    T_WS.second = q_new.conjugate();
    T_WS.first = r_new;
    speedBgBa.template head<3>()=v_new;
    return i;
}

template<typename Scalar>
int predictStates(const okvis::ImuMeasurementDeque & imuMeasurements,
            const Scalar gravityMag,
            std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>> &T_WS,
            Eigen::Matrix<Scalar, 9,1>& speedBgBa,
            const okvis::Time t_start, const okvis::Time t_end)
{
    Eigen::Matrix<Scalar, 3,1> r_old(T_WS.first), r_new(r_old), v_old(speedBgBa.template head<3>()), v_new(v_old);
    Eigen::Quaternion<Scalar> q_old(T_WS.second.conjugate()), q_new(q_old); //rotation from world to sensor
    assert(imuMeasurements.front().timeStamp <= t_start && imuMeasurements.back().timeStamp >= t_end);

    okvis::Time time = t_start;
    okvis::Time end = t_end;
    Scalar Delta_t = Scalar(0);
    bool hasStarted = false;
    int i = 0;
    okvis::Time nexttime;
    for(typename okvis::ImuMeasurementDeque::const_iterator it = imuMeasurements.begin();
          it != imuMeasurements.end(); ++it) {

      Eigen::Matrix<Scalar,3,1> omega_S_0 = it->measurement.gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_0 = it->measurement.accelerometers;
      Eigen::Matrix<Scalar,3,1> omega_S_1 = (it + 1)->measurement.gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_1 = (it + 1)->measurement.accelerometers;

      // time delta
      if ((it + 1) == imuMeasurements.end()) {
        nexttime = t_end;
      } else
        nexttime = (it + 1)->timeStamp;

      Scalar dt = (Scalar)(nexttime - time).toSec();

      if (end < nexttime) {
        Scalar interval = (Scalar)(nexttime - it->timeStamp).toSec();
        nexttime = t_end;
        dt =(Scalar)(nexttime - time).toSec();
        if (dt == 0.0)
            break;
        const Scalar r = dt / interval;
        omega_S_1 = ((Scalar(1.0) - r) * omega_S_0 + r * omega_S_1).eval();
        acc_S_1 = ((Scalar(1.0) - r) * acc_S_0 + r * acc_S_1).eval();
      }

      if (dt <= Scalar(0.0)) {
        continue;
      }
      Delta_t += dt;

      if (!hasStarted) {
        hasStarted = true;
        const Scalar r = dt / ((Scalar)(nexttime - it->timeStamp).toSec());
        omega_S_0 = (r * omega_S_0 + (Scalar(1.0) - r) * omega_S_1).eval();
        acc_S_0 = (r * acc_S_0 + (Scalar(1.0) - r) * acc_S_1).eval();
      }

      // actual propagation
      Eigen::Matrix<Scalar,3,1> a_est = Scalar(0.5)*(acc_S_0+acc_S_1) - speedBgBa.template tail<3>();
      Eigen::Matrix<Scalar,3,1> w_est = Scalar(0.5)*(omega_S_0+omega_S_1) - speedBgBa.template segment<3>(3);
      Eigen::Matrix<Scalar,3,1> gW(Scalar(0), Scalar(0), -gravityMag);

      Eigen::Quaternion<Scalar> qb = okvis::kinematics::rvec2quat(-w_est*dt);
      q_new=qb*q_old;

      Eigen::Matrix<Scalar,3,1> vel_inc1=(q_old.conjugate()._transformVector(a_est*dt)+q_new.conjugate()._transformVector(a_est*dt))*Scalar(0.5);
      Eigen::Matrix<Scalar,3,1> vel_inc2=gW*dt;

      v_new= v_old + vel_inc1+vel_inc2;
      r_new= r_old + (v_new+v_old)*dt*Scalar(0.5);

      time = nexttime;
      ++i;

      if (nexttime == t_end)
        break;

      r_old=r_new;
      v_old=v_new;
      q_old=q_new;
    }
    assert(nexttime == t_end);

    T_WS.second = q_new.conjugate();
    T_WS.first = r_new;
    speedBgBa. template head<3>()=v_new;
    return i;
}

// time_pair[0] timestamp of the provided state values. time_pair[0] >= time_pair[1],
template<typename Scalar>
int predictStatesBackward(const okvis::ImuMeasurementDeque & imuMeasurements,
                    const Scalar gravityMag,
                    std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>> &T_WS,
                    Eigen::Matrix<Scalar, 9,1>& speedBgBa,
                    const okvis::Time t_start, const okvis::Time t_end)
{
    Eigen::Matrix<Scalar, 3,1> r_old(T_WS.first), r_new(r_old), v_old(speedBgBa.template head<3>()), v_new(v_old);
    Eigen::Quaternion<Scalar> q_old(T_WS.second.conjugate()), q_new(q_old); //rotation from world to sensor
    assert(imuMeasurements.front().timeStamp <= t_end && imuMeasurements.back().timeStamp >= t_start);
    // if this assertion fails during optimization, it often means the time offset variables diverge. Solution:
    // either try to enlarge the range of imu reading segment, or decrease the time variables' std to effectively lock them

    okvis::Time time = t_start;
    okvis::Time end = t_end;
    Scalar Delta_t = Scalar(0);
    bool hasStarted = false;
    int i = 0;
    okvis::Time nexttime;
    for(typename okvis::ImuMeasurementDeque::const_reverse_iterator it = imuMeasurements.rbegin();
          it != imuMeasurements.rend(); ++it) {

      Eigen::Matrix<Scalar,3,1> omega_S_0 = it->measurement.gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_0 = it->measurement.accelerometers;
      Eigen::Matrix<Scalar,3,1> omega_S_1 = (it + 1)->measurement.gyroscopes;
      Eigen::Matrix<Scalar,3,1> acc_S_1 = (it + 1)->measurement.accelerometers;

      // time delta
      if ((it + 1) == imuMeasurements.rend()) {
        nexttime = t_end;
      } else
        nexttime = (it + 1)->timeStamp;
      Scalar dt = (Scalar)(nexttime - time).toSec();

      if (end > nexttime) {
        Scalar interval = (Scalar)(nexttime - it->timeStamp).toSec();
        nexttime = t_end;
        dt = (Scalar)(nexttime - time).toSec();
        if (dt == 0.0)
            break;
        const Scalar r = dt / interval;
        omega_S_1 = ((Scalar(1.0) - r) * omega_S_0 + r * omega_S_1).eval();
        acc_S_1 = ((Scalar(1.0) - r) * acc_S_0 + r * acc_S_1).eval();
      }

      if (dt >= Scalar(0.0)) {
        continue;
      }
      Delta_t += dt;

      if (!hasStarted) {
        hasStarted = true;
        const Scalar r = dt / ((Scalar)(nexttime - it->timeStamp).toSec());
        omega_S_0 = (r * omega_S_0 + (Scalar(1.0) - r) * omega_S_1).eval();
        acc_S_0 = (r * acc_S_0 + (Scalar(1.0) - r) * acc_S_1).eval();
      }

      // actual propagation

      Eigen::Matrix<Scalar,3,1> a_est = Scalar(0.5)*(acc_S_0+acc_S_1) - speedBgBa.template tail<3>();
      Eigen::Matrix<Scalar,3,1> w_est = Scalar(0.5)*(omega_S_0+omega_S_1) - speedBgBa.template segment<3>(3);
      Eigen::Matrix<Scalar,3,1> gW(Scalar(0), Scalar(0), -gravityMag);

      Eigen::Quaternion<Scalar> qb = okvis::kinematics::rvec2quat(-w_est*dt);
      q_new=qb*q_old;

      Eigen::Matrix<Scalar,3,1> vel_inc1=(q_old.conjugate()._transformVector(a_est*dt)+q_new.conjugate()._transformVector(a_est*dt))*Scalar(0.5);
      Eigen::Matrix<Scalar,3,1> vel_inc2=gW*dt;

      v_new= v_old + vel_inc1+vel_inc2;
      r_new= r_old + (v_new+v_old)*dt*Scalar(0.5);

      time = nexttime;
      ++i;

      if (nexttime == t_end)
        break;

      r_old=r_new;
      v_old=v_new;
      q_old=q_new;
    }
    assert(nexttime == t_end);

    T_WS.second = q_new.conjugate();
    T_WS.first = r_new;
    speedBgBa.template head<3>()=v_new;
    return i;
}

// linear interpolation
inline void interpolateInertialData(const okvis::ImuMeasurementDeque &imuMeas,
                                    const okvis::Time &queryTime,
                                    okvis::ImuMeasurement &queryValue) {
  auto iterLeft = imuMeas.begin(), iterRight = imuMeas.end();
  if (iterLeft->timeStamp > queryTime)
    throw std::runtime_error("iterLeft->timeStamp > queryTime: Imu "
                             "measurements has wrong timestamps");
  for (auto iter = imuMeas.begin(); iter != imuMeas.end(); ++iter) {
    if (iter->timeStamp < queryTime) {
      iterLeft = iter;
    } else if (iter->timeStamp == queryTime) {
      queryValue = *iter;
      return;
    } else {
      iterRight = iter;
      break;
    }
  }
  double ratio = (queryTime - iterLeft->timeStamp).toSec() /
                 (iterRight->timeStamp - iterLeft->timeStamp).toSec();
  queryValue.timeStamp = queryTime;
  Eigen::Vector3d omega_S0 =
      (iterRight->measurement.gyroscopes - iterLeft->measurement.gyroscopes) *
          ratio +
      iterLeft->measurement.gyroscopes;
  Eigen::Vector3d acc_S0 = (iterRight->measurement.accelerometers -
                            iterLeft->measurement.accelerometers) *
                               ratio +
                           iterLeft->measurement.accelerometers;
  queryValue.measurement.gyroscopes = omega_S0;
  queryValue.measurement.accelerometers = acc_S0;
}
}  // namespace ode
}  // namespace swift_vio
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Shikhar Vashistha
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_LQ_INCLUDE
#define MTL_MATRIX_LQ_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {
    namespace mat {

        /// LQ-Factorization of matrix A(m x n)
        /** Return pair L lower triangle matrix and Q= orthogonal matrix. L and Q are always dense2D **/
        template <typename Matrix, typename MatrixQ, typename MatrixR>
        void lq(const Matrix& A, MatrixQ& Q, MatrixR& R)
        {
            vampir_trace<4013> tracer;
            typedef typename Collection<Matrix>::value_type   		    value_type;
            typedef typename Collection<Matrix>::size_type    		    size_type;
            typedef typename Magnitude<value_type>::type      		    magnitude_type;
            typedef mtl::dense_vector<value_type, vec::parameters<> >       vector_type;

            size_type        ncols = num_cols(trans(A)), nrows = num_rows(trans(A)),
                mini = ncols == nrows ? ncols - 1 : (nrows >= ncols ? ncols : nrows);
            magnitude_type   factor = magnitude_type(2);

            Q = 1;
            for (size_type i = 0; i < mini; i++) {
                irange r(i, imax); // Intervals [i, n-1]
                vector_type   w(R[r][i]), v(householder_s(w));

                // R-= 2*v*(v'*R)
                // L will be same if we find R of transpose of A
                MatrixR Rsub(R[r][r]);
                vector_type tmp(-factor * trans(Rsub) * v);
                rank_one_update(Rsub, v, tmp);

                //update Q: Q-= 2*(v*Q)*v'
                MatrixQ Qsub(Q[iall][r]);
                vector_type qtmp(-factor * Qsub * v);
                rank_one_update(Qsub, qtmp, v);
            } //end for
        }

        /// QR-Factorization of matrix A(m x n)
        template <typename Matrix>
        std::pair<mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> >,
            mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> > >
            inline lq(const Matrix& A)
        {
            mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> >  R(A), Q(num_rows(trans(A)), num_rows(trans(A)));
            lq(trans(A), Q, R);
            return std::make_pair(Q, R);
        }

        // LQ-Factorization of matrix A
        // Return Q and L with A = L*Q   L lower triangle and Q othogonal
        template <typename Matrix>
        std::pair<typename mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> >,
            typename mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> > >
            inline lq_factors(const Matrix& A)
        {
            vampir_trace<4014> tracer;
            using std::abs;
            typedef typename Collection<Matrix>::value_type   value_type;
            // typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
            typedef typename Collection<Matrix>::size_type    size_type;
            size_type        ncols = num_cols(trans(A)), nrows = num_rows(trans(A));
            value_type       zero = math::zero(A[0][0]), one = math::one(A[0][0]);

            //evaluation of Q
            Matrix  Q(nrows, nrows), Qk(nrows, nrows), HEL(nrows, ncols), R(nrows, ncols), R_tmp(nrows, ncols);
            Q = one; R = zero; HEL = zero;

            boost::tie(Q, R_tmp) = lq(trans(A));
            R = lower(R_tmp);
			//R will be a lower triangular matrix in lq decomposition
            return std::make_pair(Q, R);
        }

    }
} // namespace mtl::matrix

#endif // MTL_MATRIX_QR_INCLUDE


#include <math.h>
#include <EigenUnsupported/Eigen/KroneckerProduct>
#include "Core/Utilities/QProgInfo/QCircuitInfo.h"
#include "Core/Utilities/Tools/MatrixDecomposition.h"
USING_QPANDA
using namespace std;

using MatrixSequence = std::vector<MatrixUnit>;
using DecomposeEntry = std::pair<int, MatrixSequence>;

using ColumnOperator = std::vector<DecomposeEntry>;
using MatrixOperator = std::vector<ColumnOperator>;

using SingleGateUnit = std::pair<MatrixSequence, QStat>;

static void upper_partition(int order, MatrixOperator &entries)
{
	auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

	for (auto cdx = 0; cdx < order - 1; ++cdx)
	{
		for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
		{
			auto entry = entries[cdx][rdx];

			entry.first += order;
			entry.second[index] = MatrixUnit::SINGLE_P1;

			entries[cdx + order].emplace_back(entry);
		}
	}

    return;
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int lj = ((cdx - 1) >> (udx - 1)) & 1;

	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if 1 ≤ j ≤ m and cj = lj' = 1 , return true
	auto mat = units[units.size() - udx];
	return udx >= 1
		&& udx <= M
		&& lj
		&& mat == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if j = n and none of cn...cm+1 is 1 , return true
	if (units.size() != udx)
	{
		return false;
	}
	else
	{
		auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
		return (units.end() - M) == iter;
	}
}

static void under_partition(int order, MatrixOperator& entries)
{
	auto qubits = (int)std::log2(entries.size() + 1);

	for (auto cdx = 1; cdx < order; ++cdx)
	{
		if (cdx & 1)
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto value = entries[0][rdx + order - 1].first ^ cdx;
				auto entry = make_pair(value, entries[cdx - 1][rdx + order - cdx].second);

				entries[cdx].emplace_back(entry);
			}

			auto &units = entries[cdx].back().second;
			for (auto idx = 0; idx < (int)std::log2(order); ++idx)
			{
				units[qubits - idx - 1] = ((cdx >> idx) & 1) ?
					MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;
			}
		}
		else
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto range = (int)std::log2(order) + 1;
				auto refer = entries[0][rdx + order - 1].second;
				auto entry = entries[0][rdx + order - 1].first ^ cdx;

				MatrixSequence units(refer.begin() + qubits - range, refer.end());

				for (auto udx = 1; udx <= range; ++udx)  /*udx = j , cdx = L*/
				{
					bool steps_accord = steps_requirement(units, udx, cdx + 1);
					bool entry_accord = entry_requirement(units, udx, cdx + 1);

					units[range - udx] = steps_accord ? MatrixUnit::SINGLE_P1 :
						entry_accord ? MatrixUnit::SINGLE_P0 : units[range - udx];
				}

				for (auto idx = 0; idx < qubits - range; ++idx)
				{
					units.insert(units.begin(), MatrixUnit::SINGLE_I2);
				}

				entries[cdx].emplace_back(make_pair(entry, units));
			}

			auto refer_opt = entries[0][2 * order - 2].second;
			for (auto idx = 0; idx < qubits; ++idx)
			{
				if ((cdx >> idx) & 1)
				{
					refer_opt[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
				}
			}

			entries[cdx].back().second = refer_opt;
		}
	}

    return;
}

static void controller(MatrixSequence &sequence, const EigenMatrix2c U2, EigenMatrixXc &matrix)
{
	EigenMatrix2c P0;
	EigenMatrix2c P1;
	EigenMatrix2c I2;

	P0 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0);
	P1 << Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);
	I2 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);

	std::map<MatrixUnit, std::function<EigenMatrix2c()>> mapping =
	{
		{ MatrixUnit::SINGLE_P0, [&]() {return P0; } },
		{ MatrixUnit::SINGLE_P1, [&]() {return P1; } },
		{ MatrixUnit::SINGLE_I2, [&]() {return I2; } },
		{ MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
	};

	auto order = sequence.size();
	EigenMatrixXc Un = EigenMatrixXc::Identity(1, 1);
	EigenMatrixXc In = EigenMatrixXc::Identity(1ull << order, 1ull << order);

	for (const auto &val : sequence)
	{
		EigenMatrix2c M2 = mapping.find(val)->second();
		Un = Eigen::kroneckerProduct(Un, M2).eval();
	}

	matrix = In + Un;
    return;
}

static void recursive_partition(const EigenMatrixXc& sub_matrix, MatrixOperator &entries)
{
    Eigen::Index order = sub_matrix.rows();
    if (1 == order)
    {
        return;
    }
    else
    {
        EigenMatrixXc corner = sub_matrix.topLeftCorner(order / 2, order / 2);

        recursive_partition(corner, entries);

        upper_partition(order / 2, entries);
        under_partition(order / 2, entries);
    }

    return;
}

static void decomposition(EigenMatrixXc& matrix, MatrixOperator& entries, std::vector<SingleGateUnit>& cir_units)
{
	for (auto cdx = 0; cdx < entries.size(); ++cdx)
	{
		auto opts = entries[cdx].size();
		for (auto idx = 0; idx < opts; ++idx)
		{
			auto rdx = entries[cdx][idx].first;
			auto opt = entries[cdx][idx].second;

			if ((EigenComplexT(0, 0) == matrix(rdx, cdx) && (idx != opts - 1)) ||
				(EigenComplexT(1, 0) == matrix(cdx + 1, cdx) && (idx == opts - 1)))
			{
				continue;
			}
			else
			{
				EigenMatrix2c C2; /*placeholder*/
				C2 << EigenComplexT(0, 1), EigenComplexT(0, 1),
					EigenComplexT(0, 1), EigenComplexT(0, 1);

				EigenMatrixXc Cn;
				controller(opt, C2, Cn);

				Qnum indices(2);
				for (Eigen::Index index = 0; index < (1ull << opt.size()); ++index)
				{
					if (Cn(rdx, index) != EigenComplexT(0, 0))
					{
						indices[index == rdx] = index;
					}
				}

				EigenComplexT C0 = matrix(indices[0], cdx);  /*The entry to be eliminated */
				EigenComplexT C1 = matrix(indices[1], cdx);  /*The corresponding entry */

				EigenComplexT V11, V12, V21, V22;

				if (indices[0] < indices[1])
				{
					V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
				}
				else
				{
					V11 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
				}

				EigenMatrix2c V2;
				V2 << V11, V12, V21, V22;

				EigenMatrixXc Un;
				controller(opt, V2, Un);

				matrix = Un * matrix;

				QStat M2 = { (qcomplex_t)V11 ,(qcomplex_t)V12 ,(qcomplex_t)V21 ,(qcomplex_t)V22 };
				cir_units.insert(cir_units.begin(), std::make_pair(opt, M2));
			}
		}
	}

	EigenMatrix2c V2 = matrix.bottomRightCorner(2, 2);
	if (EigenMatrixXc::Identity(2, 2) != V2)
	{
		QStat M2 = { (qcomplex_t)((EigenComplexT)1.0 / V2(0,0)), (qcomplex_t)(V2(0,1)),
					 (qcomplex_t)(V2(1,0)) , (qcomplex_t)((EigenComplexT)1.0 / V2(1,1))};

		auto entry = entries.back().back().second;
		cir_units.insert(cir_units.begin(), std::make_pair(entry, M2));
	}
}

static void initialize(EigenMatrixXc& matrix, MatrixOperator& entries)
{
    auto qubits = (int)std::log2(matrix.rows());

    MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
    Cns.back() = MatrixUnit::SINGLE_V2;
    entries.front().emplace_back(make_pair(1, Cns));

    ColumnOperator& column = entries.front();
    for (auto idx = 1; idx < qubits; ++idx)
    {
        size_t path = 1ull << idx;
        for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
        {
            auto entry = column[opt].first;
            auto units = column[opt].second;

            // 1 : none of cn−1, . . . , c1 equals 1
            // * : otherwise
            auto iter = std::find(units.end() - idx, units.end(), MatrixUnit::SINGLE_P1);

            units[units.size() - 1 - idx] = (units.end() == iter) ?
                MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;

            column.emplace_back(make_pair(entry + path, units));
        }

        MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
        Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

        column.emplace_back(make_pair((1ull << idx), Lns));
    }

    return;
}

static void general_scheme(EigenMatrixXc& matrix, std::vector<SingleGateUnit>& cir_units)
{
	MatrixOperator entries;
	for (auto idx = 1; idx < matrix.cols(); ++idx)
	{
		ColumnOperator Co;
		entries.emplace_back(Co);
	}

	initialize(matrix, entries);
 	recursive_partition(matrix, entries);
	decomposition(matrix, entries, cir_units);

    return;
}

static void circuit_insert(QVec& qubits, std::vector<SingleGateUnit>& cir_units, QCircuit &circuit)
{
	std::sort(qubits.begin(), qubits.end(), [&](Qubit *a, Qubit *b)
	{
		return a->getPhysicalQubitPtr()->getQubitAddr()
			 < b->getPhysicalQubitPtr()->getQubitAddr();
	});

	auto rank = qubits.size();
	for (auto &val : cir_units)
	{
		QVec control;
		QCircuit cir;

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_P0 == val.first[qdx])
			{
				cir << X(qubits[qdx]);
				control.emplace_back(qubits[qdx]);
			}
			else if (MatrixUnit::SINGLE_P1 == val.first[qdx])
			{
				control.emplace_back(qubits[qdx]);
			}
			else
			{}
		}

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_V2 == val.first[qdx])
			{
				circuit << cir
					    << U4(val.second, qubits[qdx]).control(control).dagger()
						<< cir;

				break;
			}
		}
	}
}

QCircuit QPanda::matrix_decompose(QVec qubits, const QStat& src_mat)
{
	auto order = std::sqrt(src_mat.size());

	EigenMatrixXc matrix = EigenMatrixXc::Zero(order, order);
	for (auto rdx = 0; rdx < order; ++rdx)
	{
		for (auto cdx = 0; cdx < order; ++cdx)
		{
			matrix(rdx, cdx) = src_mat[rdx*order + cdx];
		}
	}

	return matrix_decompose(qubits, matrix);
}

QCircuit QPanda::matrix_decompose(QVec qubits, EigenMatrixXc& src_mat)
{
	if (!src_mat.isUnitary(1e-6))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "Non-unitary matrix.");
	}

	if (qubits.size() != log2(src_mat.cols()))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "The qubits number is error.");
	}

	std::vector<SingleGateUnit> cir_units;
	general_scheme(src_mat, cir_units);

	QCircuit output_circuit;
	circuit_insert(qubits, cir_units, output_circuit);

	return output_circuit;
}


//
// author: Ed Valeev (eduard@valeyev.net)
// date  : July 8, 2014
// the use of this software is permitted under the conditions GNU General Public License (GPL) version 2
//

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

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
        Matrix;  // import dense, dynamically sized Matrix type from Eigen;
                 // this is row-major to conform with C/C++
                 // Eigen also supports statically-sized matrices, sparse matrices, etc.

#define INDEX(i,j) ((i>j) ? (((i)*((i)+1)/2)+(j)) : (((j)*((j)+1)/2)+(i)))

struct Atom {
    double zval;
    double x, y, z;
};

void read_geometry(const std::string& filename, std::vector<Atom>& atoms);
void read_1e_ints(Matrix& A, const std::string& filename);
double* read_2e_ints(const std::string& filename, size_t nao);

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

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

  try {

    /*** =========================== ***/
    /*** initialize integrals, etc.  ***/
    /*** =========================== ***/

    // read geometry from xyz file
    std::vector<Atom> atoms;
    read_geometry("geom.dat", atoms);

    // count the number of electrons
    auto nelectron = 0;
    for (auto i = 0; i < atoms.size(); ++i)
      nelectron += atoms[i].zval;
    const auto ndocc = nelectron / 2;

    // compute the nuclear repulsion energy
    auto enuc = 0.0;
    for (auto i = 0; i < atoms.size(); i++)
      for (auto j = i + 1; j < atoms.size(); j++) {
        auto xij = atoms[i].x - atoms[j].x;
        auto yij = atoms[i].y - atoms[j].y;
        auto zij = atoms[i].z - atoms[j].z;
        auto r2 = xij*xij + yij*yij + zij*zij;
        auto r = sqrt(r2);
        enuc += atoms[i].zval * atoms[j].zval / r;
      }
    cout << "\tNuclear repulsion energy = " << enuc << endl;

    // ask the user for # of AOs
    cout << "\nEnter the number of AOs (7 unless U have updated the links): ";
    size_t nao;
    std::cin >> nao;

    // compute overlap integrals
    Matrix S(nao, nao);      // this creates an nao by nao matrix (contents are not initialized!)
                             // to make a matrix of zeroes do this:
                             // auto S = Matrix::Zero(nao, nao);
    read_1e_ints(S, "s.dat");
    cout << "\n\tOverlap Integrals:\n";
    cout << S << endl;

    // compute kinetic-energy integrals
    Matrix T(nao, nao);
    read_1e_ints(T, "t.dat");
    cout << "\n\tKinetic-Energy Integrals:\n";
    cout << T << endl;

    // compute nuclear-attraction integrals
    Matrix V(nao, nao);
    read_1e_ints(V, "v.dat");
    cout << "\n\tNuclear Attraction Integrals:\n";
    cout << V << endl;

    // Core Hamiltonian = T + V
    Matrix H = T + V;
    cout << "\n\tCore Hamiltonian:\n";
    cout << H << endl;

    // T and V no longer needed, free up the memory
    T.resize(0,0);
    V.resize(0,0);

    /* read two-electron integrals */
    auto TEI = read_2e_ints("eri.dat", nao);

    /*** =========================== ***/
    /*** build initial-guess density ***/
    /*** =========================== ***/

    // solve H C = e S C
    Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(H, S);
    auto eps = gen_eig_solver.eigenvalues();
    auto C = gen_eig_solver.eigenvectors();
    cout << "\n\tInitial C Matrix:\n";
    cout << C << endl;

    // compute density, D = C(occ) . C(occ)T
    auto C_occ = C.leftCols(ndocc);
    Matrix D = C_occ * C_occ.transpose();
    cout << "\n\tInitial Density Matrix:\n";
    cout << D << endl;

    // compute HF energy
    auto ehf = 0.0;
    for (auto i = 0; i < nao; i++)
      for (auto j = 0; j < nao; j++)
        ehf += 2.0 * D(i,j) * H(i,j);

    std::cout <<
        "\n\n Iter        E(elec)              E(tot)               Delta(E)             RMS(D)\n";
    printf(" %02d %20.12f %20.12f\n", 0, ehf, ehf + enuc);

    /*** =========================== ***/
    /*** main iterative loop ***/
    /*** =========================== ***/

    const auto maxiter = 100;
    const auto conv = 1e-12;
    auto iter = 0;
    auto rmsd = 0.0;
    auto ediff = 0.0;
    do {
      ++iter;

      // Save a copy of the energy and the density
      auto ehf_last = ehf;
      auto D_last = D;

      // build a new Fock matrix
      auto F = H;
      for (auto i = 0; i < nao; i++)
        for (auto j = 0; j < nao; j++) {
          for (auto k = 0; k < nao; k++)
            for (auto l = 0; l < nao; l++) {
              auto ij = INDEX(i, j);
              auto kl = INDEX(k, l);
              auto ijkl = INDEX(ij, kl);
              auto ik = INDEX(i, k);
              auto jl = INDEX(j, l);
              auto ikjl = INDEX(ik, jl);

              F(i,j) += D(k,l) * (2.0 * TEI[ijkl] - TEI[ikjl]);
            }
        }

      if (iter == 1) {
        cout << "\n\tFock Matrix:\n";
        cout << F << endl;
      }

      // solve F C = e S C
      Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(F, S);
      auto eps = gen_eig_solver.eigenvalues();
      auto C = gen_eig_solver.eigenvectors();

      // compute density, D = C(occ) . C(occ)T
      auto C_occ = C.leftCols(ndocc);
      D = C_occ * C_occ.transpose();

      // compute HF energy
      ehf = 0.0;
      for (auto i = 0; i < nao; i++)
        for (auto j = 0; j < nao; j++)
          ehf += D(i,j) * (H(i,j) + F(i,j));

      // compute difference with last iteration
      ediff = ehf - ehf_last;
      rmsd = (D - D_last).norm();

      printf(" %02d %20.12f %20.12f %20.12f %20.12f\n", iter, ehf, ehf + enuc,
             ediff, rmsd);

    } while (((fabs(ediff) > conv) || (fabs(rmsd) > conv)) && (iter < maxiter));

    delete[] TEI;
  } // end of try block

  catch (const char* ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::string& ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::exception& ex) {
    cerr << ex.what() << endl;
    return 1;
  }
  catch (...) {
    cerr << "caught unknown exception\n";
    return 1;
  }

  return 0;
}

void read_geometry(const std::string& filename, std::vector<Atom>& atoms) {

  std::ifstream is(filename);
  assert(is.good());
  size_t natom;
  is >> natom;

  atoms.resize(natom);
  for (int i = 0; i < natom; i++)
    is >> atoms[i].zval >> atoms[i].x >> atoms[i].y >> atoms[i].z;
}

void read_1e_ints(Matrix& A, const std::string& filename) {
  std::ifstream is(filename);
  assert(is.good());

  while (is) {
    int i, j;
    double val;
    is >> i >> j >> val;
    --i; --j;
    A(i,j) = A(j,i) = val;
  }
}

double* read_2e_ints(const std::string& filename, size_t nao) {
  auto nints = ((nao * (nao + 1) / 2) * ((nao * (nao + 1) / 2) + 1) / 2);
  auto result = new double[nints];
  std::fill(result, result+nints, 0);

  std::ifstream is(filename);
  assert(is.good());

  while (is) {
    size_t i, j, k, l;
    double val;
    is >> i >> j >> k >> l >> val;
    auto ij = INDEX(i - 1, j - 1);
    auto kl = INDEX(k - 1, l - 1);
    auto ijkl = INDEX(ij, kl);

    result[ijkl] = val;
  }

  return result;
}