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

#ifndef CRYPTO3_R1CS_GG_PPZKSNARK_IPP2_VERIFY_HPP
#define CRYPTO3_R1CS_GG_PPZKSNARK_IPP2_VERIFY_HPP

#include <memory>
#include <vector>
#include <tuple>

#include <boost/iterator/zip_iterator.hpp>

#include <nil/crypto3/detail/pack_numeric.hpp>

#include <nil/crypto3/hash/algorithm/hash.hpp>
#include <nil/crypto3/hash/sha2.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/verification_key.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/ipp2/proof.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/ipp2/srs.hpp>

namespace nil {
    namespace crypto3 {
        namespace zk {
            namespace snark {
                template<typename CurveType>
                struct Op {
                    typedef CurveType curve_type;

                    typedef typename curve_type::pairing::fqk_type::value_type TAB;
                    typedef typename curve_type::pairing::fqk_type::value_type UAB;
                    typedef typename curve_type::pairing::fqk_type::value_type ZAB;
                    typedef typename curve_type::pairing::fqk_type::value_type TC;
                    typedef typename curve_type::pairing::fqk_type::value_type UC;
                    typedef typename curve_type::pairing::g1_type::value_type ZC;
                };

                /// Keeps track of the variables that have been sent by the prover and must
                /// be multiplied together by the verifier. Both MIPP and TIPP are merged
                /// together.
                template<typename CurveType>
                struct gipa_tuz {
                    typedef CurveType curve_type;
                    typedef typename curve_type::scalar_field_type::value_type fr_type;

                    std::pair<typename curve_type::pairing::fqk_type::value_type, fr_type::value_type> tab;
                    std::pair<typename curve_type::pairing::fqk_type::value_type, fr_type::value_type> uab;
                    std::pair<typename curve_type::pairing::fqk_type::value_type, fr_type::value_type> zab;
                    std::pair<typename curve_type::pairing::fqk_type::value_type, fr_type::value_type> tc;
                    std::pair<typename curve_type::pairing::fqk_type::value_type, fr_type::value_type> uc;
                    std::pair<typename curve_type::pairing::g1_type, fr_type> zc;
                };

                /// gipa_verify_tipp_mipp recurse on the proof and statement and produces the final
                /// values to be checked by TIPP and MIPP verifier, namely, for TIPP for example:
                /// * T,U: the final commitment values of A and B
                /// * Z the final product between A and B.
                /// * Challenges are returned in inverse order as well to avoid
                /// repeating the operation multiple times later on.
                /// * There are T,U,Z vectors as well for the MIPP relationship. Both TIPP and
                /// MIPP share the same challenges however, enabling to re-use common operations
                /// between them, such as the KZG proof for commitment keys.
                template<typename CurveType, typename Hash = hashes::sha2<256>>
                std::tuple<gipa_tuz<CurveType>, std::vector<typename CurveType::scalar_field_type::value_type>,
                           std::vector<typename CurveType::scalar_field_type::value_type>>
                    gipa_verify_tipp_mipp(const r1cs_gg_ppzksnark_aggregate_proof<CurveType> &proof) {
                    std::vector<typename CurveType::scalar_field_type::value_type> challenges, challenges_inv;

                    typename CurveType::scalar_field_type::value_type default_transcript =
                        typename CurveType::scalar_field_type::value_type::zero();

                    // We first generate all challenges as this is the only consecutive process
                    // that can not be parallelized then we scale the commitments in a
                    // parallelized way
                    std::for_each(
                        boost::make_zip_iterator(
                            std::make_tuple(proof.tmipp.gipa.comms_ab.begin(), proof.tmipp.gipa.z_ab.begin(),
                                            proof.tmipp.gipa.comms_c.begin(), proof.tmipp.gipa.z_c.begin())),
                        boost::make_zip_iterator(
                            std::make_tuple(proof.tmipp.gipa.comms_ab.end(), proof.tmipp.gipa.z_ab.end(),
                                            proof.tmipp.gipa.comms_c.end(), proof.tmipp.gipa.z_c.end())),
                        [&](const std::tuple<const std::pair<r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>,
                                                             r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>> &,
                                             const std::pair<typename CurveType::pairing::fqk_type::value_type,
                                                             typename CurveType::pairing::fqk_type::value_type> &,
                                             const std::pair<r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>,
                                                             r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>> &,
                                             const std::pair<typename CurveType::g1_type::value_type,
                                                             typename CurveType::g1_type::value_type> &> &t) {
                            auto tab_l = std::get<0>(std::get<0>(t));
                            auto tab_r = std::get<1>(std::get<0>(t));

                            auto zab_l = std::get<0>(std::get<1>(t));
                            auto zab_r = std::get<1>(std::get<1>(t));

                            auto tc_l = std::get<0>(std::get<2>(t));
                            auto tc_r = std::get<1>(std::get<2>(t));

                            auto zc_l = std::get<0>(std::get<3>(t));
                            auto zc_r = std::get<1>(std::get<3>(t));

                            // Fiat-Shamir challenge
                            auto transcript = challenges.empty() ? *(challenges.end() - 1) : default_transcript;

                            std::size_t counter_nonce = 1;
                            std::array<std::uint8_t, sizeof(std::size_t)> counter_nonce_bytes;
                            crypto3::detail::pack<stream_endian::big_byte_big_bit>({counter_nonce},
                                                                                counter_nonce_bytes);
                            accumulator_set<Hash> acc;

                            hash<Hash>(counter_nonce_bytes, acc);
                            hash<Hash>(transcript, acc);
                            hash<Hash>(std::get<0>(tab_l), acc);
                            hash<Hash>(std::get<1>(tab_l), acc);
                            hash<Hash>(std::get<0>(tab_r), acc);
                            hash<Hash>(std::get<1>(tab_r), acc);
                            hash<Hash>(zab_l, acc);
                            hash<Hash>(zab_r, acc);
                            hash<Hash>(zc_l, acc);
                            hash<Hash>(zc_r, acc);
                            hash<Hash>(std::get<0>(tc_l), acc);
                            hash<Hash>(std::get<1>(tc_l), acc);
                            hash<Hash>(std::get<0>(tc_r), acc);
                            hash<Hash>(std::get<1>(tc_r), acc);

                            typename Hash::digest_type d = accumulators::extract::hash<Hash>(acc);
                            typename CurveType::scalar_field_type::value_type c;
                            multiprecision::import_bits(c.data, d);

                            challenges.emplace_back(c);
                            challenges_inv.emplace_back(c.inversed());
                        });

                    gipa_tuz<CurveType> final_res = {std::get<0>(proof.com_ab), std::get<1>(proof.com_ab), proof.ip_ab,
                                                     std::get<0>(proof.com_c),  std::get<1>(proof.com_c),  proof.agg_c};

                    // we first multiply each entry of the Z U and L vectors by the respective
                    // challenges independently
                    // Since at the end we want to multiple all "t" values together, we do
                    // multiply all of them in parrallel and then merge then back at the end.
                    // same for u and z.
                    std::for_each(
                        boost::make_zip_iterator(
                            std::make_tuple(proof.tmipp.gipa.comms_ab.begin(), proof.tmipp.gipa.z_ab.begin(),
                                            proof.tmipp.gipa.comms_c.begin(), proof.tmipp.gipa.z_c.begin(),
                                            challenges.begin(), challenges_inv.begin())),
                        boost::make_zip_iterator(
                            std::make_tuple(proof.tmipp.gipa.comms_ab.end(), proof.tmipp.gipa.z_ab.end(),
                                            proof.tmipp.gipa.comms_c.end(), proof.tmipp.gipa.z_c.end(),
                                            challenges.end(), challenges_inv.end())),
                        [&](const std::tuple<const std::pair<r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>,
                                                             r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>> &,
                                             const std::pair<typename CurveType::pairing::fqk_type::value_type,
                                                             typename CurveType::pairing::fqk_type::value_type> &,
                                             const std::pair<r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>,
                                                             r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType>> &,
                                             const std::pair<typename CurveType::g1_type::value_type,
                                                             typename CurveType::g1_type::value_type> &,
                                             const typename CurveType::scalar_field_type::value_type &,
                                             const typename CurveType::scalar_field_type::value_type &> &t) {
                            // T and U values for right and left for AB part
                            auto tab_l = std::get<0>(std::get<0>(std::get<0>(t)));
                            auto uab_l = std::get<1>(std::get<0>(std::get<0>(t)));
                            auto tab_r = std::get<0>(std::get<1>(std::get<0>(t)));
                            auto uab_r = std::get<1>(std::get<1>(std::get<0>(t)));

                            auto zab_l = std::get<0>(std::get<1>(t));
                            auto zab_r = std::get<1>(std::get<1>(t));

                            // T and U values for right and left for C part
                            auto tc_l = std::get<0>(std::get<0>(std::get<2>(t)));
                            auto uc_l = std::get<1>(std::get<0>(std::get<2>(t)));
                            auto tc_r = std::get<0>(std::get<1>(std::get<2>(t)));
                            auto uc_r = std::get<1>(std::get<1>(std::get<2>(t)));

                            auto zc_l = std::get<0>(std::get<3>(t));
                            auto zc_r = std::get<1>(std::get<3>(t));

                            // we multiple left side by x and right side by x^-1
                            vec ![
                                Op<CurveType>::TAB(tab_l, std::get<4>(t)),
                                Op<CurveType>::tab(tab_r, c_inv_repr),
                                Op::UAB(uab_l, c_repr),
                                Op::UAB(uab_r, c_inv_repr),
                                Op::ZAB(zab_l, c_repr),
                                Op::ZAB(zab_r, c_inv_repr),
                                Op::TC::<E>(tc_l, c_repr),
                                Op::TC(tc_r, c_inv_repr),
                                Op::UC(uc_l, c_repr),
                                Op::UC(uc_r, c_inv_repr),
                                Op::ZC(zc_l, c_repr),
                                Op::ZC(zc_r, c_inv_repr),
                            ]
                        });
                }

                /// verify_tipp_mipp returns a pairing equation to check the tipp proof.  $r$ is
                /// the randomness used to produce a random linear combination of A and B and
                /// used in the MIPP part with C
                template<typename CurveType>
                PairingCheck<CurveType> verify_tipp_mipp(const r1cs_gg_ppzksnark_verifying_srs<CurveType> &v_srs,
                                                         const r1cs_gg_ppzksnark_aggregate_proof<CurveType> &proof,
                                                         const typename CurveType::scalar_field_type::value_type &r_shift) {
                    // (T,U), Z for TIPP and MIPP  and all challenges
                    std::tuple<gipa_tuz<CurveType>, std::vector<typename CurveType::scalar_field_type::value_type>,
                               std::vector<typename CurveType::scalar_field_type::value_type>>
                        gtmp = gipa_verify_tipp_mipp(proof);
                    auto &final_res = std::get<0>(gtmp);
                    auto &challenges = std::get<1>(gtmp);
                    auto &challenges_inv = std::get<2>(gtmp);

                    // we reverse the order so the KZG polynomial have them in the expected
                    // order to construct them in logn time.
                    std::reverse(challenges.begin(), challenges.end());
                    std::reverse(challenges_inv.begin(), challenges_inv.end());

                    // Verify commitment keys wellformed
                    auto fvkey = proof.tmipp.gipa.final_vkey;
                    auto fwkey = proof.tmipp.gipa.final_wkey;
                }

                template<typename CurveType, typename InputPublicInputsIterator>
                bool verify_aggregate_proof(const r1cs_gg_ppzksnark_verifying_srs<CurveType> &ip_verifier_srs,
                                            const r1cs_gg_ppzksnark_processed_verification_key<CurveType> &pvk,
                                            InputPublicInputsIterator public_inputs_first,
                                            InputPublicInputsIterator public_inputs_last,
                                            const r1cs_gg_ppzksnark_aggregate_proof<CurveType> &proof) {

                    // Random linear combination of proofs
                    std::size_t counter_nonce = 1;
                    std::array<std::uint8_t, sizeof(std::size_t)> counter_nonce_bytes;
                    crypto3::detail::pack<stream_endian::big_byte_big_bit>({counter_nonce}, counter_nonce_bytes);
                    accumulator_set<hashes::sha2<256>> acc;

                    hash<hashes::sha2<256>>(counter_nonce_bytes, acc);
                    hash<hashes::sha2<256>>(std::get<0>(proof.com_ab), acc);
                    hash<hashes::sha2<256>>(std::get<1>(proof.com_ab), acc);
                    hash<hashes::sha2<256>>(std::get<0>(proof.com_c), acc);
                    hash<hashes::sha2<256>>(std::get<1>(proof.com_c), acc);

                    typename hashes::sha2<256>::digest_type d = accumulators::extract::hash<hashes::sha2<256>>(acc);
                    typename CurveType::scalar_field_type::value_type r;
                    crypto3::detail::pack(d, r.data);
                    r = r.inversed();

                    InputPublicInputsIterator vpitr = public_inputs_first;

                    while (vpitr != public_inputs_last) {
                        BOOST_ASSERT_MSG(vpitr->size() + 1 == pvk.ic.size(), "malformed verification key!");
                        ++vpitr;
                    }

                    // 1.Check TIPA proof ab
                    // 2.Check TIPA proof c
                    auto tipa_ab =
                        verify_tipp_mipp<CurveType>(ip_verifier_srs,
                                                    proof,
                                                    r    // we give the extra r as it's not part of the proof itself
                                                         // - it is simply used on top for the groth16 aggregation
                        );
                }

                /// verify_kzg_opening_g2 takes a KZG opening, the final commitment key, SRS and
                /// any shift (in TIPP we shift the v commitment by r^-1) and returns a pairing
                /// tuple to check if the opening is correct or not.
                template<typename CurveType, typename InputScalarIterator>
                PairingCheck<CurveType> verify_kzg_opening_g2(
                    const r1cs_gg_ppzksnark_verifying_srs<CurveType> &v_srs,
                    const r1cs_gg_ppzksnark_ipp2_vkey<CurveType> &final_vkey,
                    const kzg_opening<typename CurveType::g2_type> &vkey_opening,
                    InputScalarIterator challenges_first,
                    InputScalarIterator challenges_last,
                    const typename std::iterator_traits<InputScalarIterator>::value_type &r_shift,
                    const typename std::iterator_traits<InputScalarIterator>::value_type &kzg_challenge) {
                }

                /// Similar to verify_kzg_opening_g2 but for g1.
                template<typename CurveType, typename InputScalarIterator>
                PairingCheck<CurveType> verify_kzg_opening_g1(
                    const r1cs_gg_ppzksnark_verifier_srs<CurveType> &v_srs,
                    const r1cs_gg_ppzksnark_ipp2_wkey<CurveType> &final_wkey,
                    const kzg_opening<typename CurveType::g1_type> &wkey_opening, InputScalarIterator challenges_first,
                    InputScalarIterator challenges_last, ,
                    const typename std::iterator_traits<InputScalarIterator>::value_type &r_shift,
                    const typename std::iterator_traits<InputScalarIterator>::value_type &kzg_challenge) {
                }
            }    // namespace snark
        }        // namespace zk
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_R1CS_GG_PPZKSNARK_TYPES_POLICY_HPP


///////////////////////////////////////////////////////////////////////////////
// Copyright Christopher Kormanyos 2014.
// Copyright John Maddock 2014.
// Copyright Paul Bristow 2014.
// Distributed under the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//

// Implement a specialization of std::complex<> for *anything* that
// is defined as BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE.

#ifndef _BOOST_CSTDFLOAT_COMPLEX_STD_2014_02_15_HPP_
  #define _BOOST_CSTDFLOAT_COMPLEX_STD_2014_02_15_HPP_

  #if defined(__GNUC__)
  #pragma GCC system_header
  #endif

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

  namespace std
  {
    // Forward declarations.
    template<class float_type>
    class complex;

    template<>
    class complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>;

    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE real(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE imag(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE abs (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE arg (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE norm(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> conj (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> proj (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> polar(const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE&,
                                                                      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& = 0);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> sqrt (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> sin  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> cos  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> tan  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> asin (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> acos (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> atan (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> exp  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> log  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> log10(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&,
                                                                      int);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&,
                                                                      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow  (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&,
                                                                      const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow  (const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE&,
                                                                      const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> sinh (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> cosh (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> tanh (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> asinh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> acosh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> atanh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    template<class char_type, class traits_type>
    inline std::basic_ostream<char_type, traits_type>& operator<<(std::basic_ostream<char_type, traits_type>&, const std::complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    template<class char_type, class traits_type>
    inline std::basic_istream<char_type, traits_type>& operator>>(std::basic_istream<char_type, traits_type>&, std::complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>&);

    // Template specialization of the complex class.
    template<>
    class complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>
    {
    public:
      typedef BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE value_type;

      explicit complex(const complex<float>&);
      explicit complex(const complex<double>&);
      explicit complex(const complex<long double>&);

      #if defined(BOOST_NO_CXX11_CONSTEXPR)
      complex(const value_type& r = value_type(),
              const value_type& i = value_type()) : re(r),
                                                    im(i) { }

      template<typename X>
      complex(const complex<X>& x) : re(x.real()),
                                     im(x.imag()) { }

      const value_type& real() const { return re; }
      const value_type& imag() const { return im; }

      value_type& real() { return re; }
      value_type& imag() { return im; }
      #else
      BOOST_CONSTEXPR complex(const value_type& r = value_type(),
                              const value_type& i = value_type()) : re(r),
                                                                    im(i) { }

      template<typename X>
      BOOST_CONSTEXPR complex(const complex<X>& x) : re(x.real()),
                                                     im(x.imag()) { }

      value_type real() const { return re; }
      value_type imag() const { return im; }
      #endif

      void real(value_type r) { re = r; }
      void imag(value_type i) { im = i; }

      complex<value_type>& operator=(const value_type& v)
      {
        re = v;
        im = value_type(0);
        return *this;
      }

      complex<value_type>& operator+=(const value_type& v)
      {
        re += v;
        return *this;
      }

      complex<value_type>& operator-=(const value_type& v)
      {
        re -= v;
        return *this;
      }

      complex<value_type>& operator*=(const value_type& v)
      {
        re *= v;
        im *= v;
        return *this;
      }

      complex<value_type>& operator/=(const value_type& v)
      {
        re /= v;
        im /= v;
        return *this;
      }

      template<typename X>
      complex<value_type>& operator=(const complex<X>& x)
      {
        re = x.real();
        im = x.imag();
        return *this;
      }

      template<typename X>
      complex<value_type>& operator+=(const complex<X>& x)
      {
        re += x.real();
        im += x.imag();
        return *this;
      }

      template<typename X>
      complex<value_type>& operator-=(const complex<X>& x)
      {
        re -= x.real();
        im -= x.imag();
        return *this;
      }

      template<typename X>
      complex<value_type>& operator*=(const complex<X>& x)
      {
        const value_type tmp_real = (re * x.real()) - (im * x.imag());
        im = (re * x.imag()) + (im * x.real());
        re = tmp_real;
        return *this;
      }

      template<typename X>
      complex<value_type>& operator/=(const complex<X>& x)
      {
        const value_type tmp_real = (re * x.real()) + (im * x.imag());
        const value_type the_norm = std::norm(x);
        im = ((im * x.real()) - (re * x.imag())) / the_norm;
        re = tmp_real / the_norm;
        return *this;
      }

      private:
        value_type re;
        value_type im;
    };

    // Constructors from built-in complex representation of floating-point types.
    complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::complex(const complex<float>& f)        : re(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE( f.real())), im(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE( f.imag())) { }
    complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::complex(const complex<double>& d)       : re(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE( d.real())), im(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE( d.imag())) { }
    complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::complex(const complex<long double>& ld) : re(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(ld.real())), im(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(ld.imag())) { }
  } // namespace std

  namespace boost { namespace math { namespace cstdfloat { namespace detail {
  template<class float_type> std::complex<float_type> multiply_by_i(const std::complex<float_type>& x)
  {
    // Multiply x (in C) by I (the imaginary component), and return the result.
    return std::complex<float_type>(-x.imag(), x.real());
  }
  } } } } // boost::math::cstdfloat::detail

  namespace std
  {
    // ISO/IEC 14882:2011, Section 26.4.7, specific values.
    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE real(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x) { return x.real(); }
    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE imag(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x) { return x.imag(); }

    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE abs (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x) { using std::sqrt;  return sqrt ((real(x) * real(x)) + (imag(x) * imag(x))); }
    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE arg (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x) { using std::atan2; return atan2(x.imag(), x.real()); }
    inline BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE norm(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x) { return (real(x) * real(x)) + (imag(x) * imag(x)); }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> conj (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(x.real(), -x.imag()); }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> proj (const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE m = (std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::max)();
      if ((x.real() > m)
        || (x.real() < -m)
        || (x.imag() > m)
        || (x.imag() < -m))
      {
        // We have an infinity, return a normalized infinity, respecting the sign of the imaginary part:
         return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::infinity(), x.imag() < 0 ? -0 : 0);
      }
      return x;
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> polar(const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& rho,
                                                                      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& theta)
    {
      using std::sin;
      using std::cos;

      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(rho * cos(theta), rho * sin(theta));
    }

    // Global add, sub, mul, div.
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator+(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u.real() + v.real(), u.imag() + v.imag()); }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator-(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u.real() - v.real(), u.imag() - v.imag()); }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator*(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v)
    {
      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>((u.real() * v.real()) - (u.imag() * v.imag()),
                                                                  (u.real() * v.imag()) + (u.imag() * v.real()));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator/(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v)
    {
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE the_norm = std::norm(v);

      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(((u.real() * v.real()) + (u.imag() * v.imag())) / the_norm,
                                                                  ((u.imag() * v.real()) - (u.real() * v.imag())) / the_norm);
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator+(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u.real() + v, u.imag()); }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator-(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u.real() - v, u.imag()); }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator*(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u.real() * v, u.imag() * v); }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator/(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u, const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u.real() / v, u.imag() / v); }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator+(const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u + v.real(),     v.imag()); }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator-(const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u - v.real(),    -v.imag()); }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator*(const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(u * v.real(), u * v.imag()); }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator/(const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& u, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& v) { const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE v_norm = norm(v); return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>((u * v.real()) / v_norm, (-u * v.imag()) / v_norm); }

    // Unary plus / minus.
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator+(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u) { return u; }
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> operator-(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& u) { return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(-u.real(), -u.imag()); }

    // Equality and inequality.
    inline bool operator==(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& y) { return ((x.real() == y.real()) && (x.imag() == y.imag())); }
    inline bool operator==(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x, const         BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE&  y) { return ((x.real() == y)        && (x.imag() == BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(0))); }
    inline bool operator==(const         BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE&  x, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& y) { return ((x        == y.real()) && (y.imag() == BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(0))); }
    inline bool operator!=(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& y) { return ((x.real() != y.real()) || (x.imag() != y.imag())); }
    inline bool operator!=(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x, const         BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE&  y) { return ((x.real() != y)        || (x.imag() != BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(0))); }
    inline bool operator!=(const         BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE&  x, const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& y) { return ((x        != y.real()) || (y.imag() != BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(0))); }

    // ISO/IEC 14882:2011, Section 26.4.8, transcendentals.
    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> sqrt(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::fabs;
      using std::sqrt;

      // Compute sqrt(x) for x in C:
      // sqrt(x) = (s       , xi / 2s) : for xr > 0,
      //           (|xi| / 2s, +-s)    : for xr < 0,
      //           (sqrt(xi), sqrt(xi) : for xr = 0,
      // where s = sqrt{ [ |xr| + sqrt(xr^2 + xi^2) ] / 2 },
      // and the +- sign is the same as the sign of xi.

      if(x.real() > 0)
      {
        const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE s = sqrt((fabs(x.real()) + std::abs(x)) / 2);

        return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(s, x.imag() / (s * 2));
      }
      else if(x.real() < 0)
      {
        const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE s = sqrt((fabs(x.real()) + std::abs(x)) / 2);

        const bool imag_is_neg = (x.imag() < 0);

        return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(fabs(x.imag()) / (s * 2), (imag_is_neg ? -s : s));
      }
      else
      {
        const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sqrt_xi_half = sqrt(x.imag() / 2);

        return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(sqrt_xi_half, sqrt_xi_half);
      }
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> sin(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::sin;
      using std::cos;
      using std::exp;

      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sin_x  = sin (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cos_x  = cos (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_yp = exp (x.imag());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_ym = BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) / exp_yp;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sinh_y = (exp_yp - exp_ym) / 2;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cosh_y = (exp_yp + exp_ym) / 2;

      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(sin_x * cosh_y, cos_x * sinh_y);
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> cos(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::sin;
      using std::cos;
      using std::exp;

      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sin_x  = sin (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cos_x  = cos (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_yp = exp (x.imag());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_ym = BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) / exp_yp;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sinh_y = (exp_yp - exp_ym) / 2;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cosh_y = (exp_yp + exp_ym) / 2;

      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(cos_x * cosh_y, -(sin_x * sinh_y));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> tan(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::sin;
      using std::cos;
      using std::exp;

      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sin_x  = sin (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cos_x  = cos (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_yp = exp (x.imag());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_ym = BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) / exp_yp;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sinh_y = (exp_yp - exp_ym) / 2;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cosh_y = (exp_yp + exp_ym) / 2;

      return (  complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(sin_x * cosh_y,  cos_x * sinh_y)
              / complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(cos_x * cosh_y, -sin_x * sinh_y));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> asin(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      return -boost::math::cstdfloat::detail::multiply_by_i(std::log(boost::math::cstdfloat::detail::multiply_by_i(x) + std::sqrt(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) - (x * x))));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> acos(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      return boost::math::constants::half_pi<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>() - std::asin(x);
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> atan(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> izz = boost::math::cstdfloat::detail::multiply_by_i(x);

      return boost::math::cstdfloat::detail::multiply_by_i(std::log(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) - izz) - std::log(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) + izz)) / 2;
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> exp(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::exp;

      return std::polar(exp(x.real()), x.imag());
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> log(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::atan2;
      using std::log;

      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(log(std::norm(x)) / 2, atan2(x.imag(), x.real()));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> log10(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      return std::log(x) / boost::math::constants::ln_ten<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>();
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x,
                                                                    int p)
    {
      const bool re_isneg  = (x.real() < 0);
      const bool re_isnan  = (x.real() != x.real());
      const bool re_isinf  = ((!re_isneg) ? bool(+x.real() > (std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::max)())
                                          : bool(-x.real() > (std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::max)()));

      const bool im_isneg  = (x.imag() < 0);
      const bool im_isnan  = (x.imag() != x.imag());
      const bool im_isinf  = ((!im_isneg) ? bool(+x.imag() > (std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::max)())
                                          : bool(-x.imag() > (std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::max)()));

      if(re_isnan || im_isnan) { return x; }

      if(re_isinf || im_isinf)
      {
        return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::quiet_NaN(),
                                                                    std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::quiet_NaN());
      }

      if(p < 0)
      {
        if(std::abs(x) < (std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::min)())
        {
          return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::infinity(),
                                                                      std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::infinity());
        }
        else
        {
          return BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) / std::pow(x, -p);
        }
      }

      if(p == 0)
      {
        return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1));
      }
      else
      {
        if(p == 1) { return x; }

        if(std::abs(x) > (std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::max)())
        {
          const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE re = (re_isneg ? -std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::infinity()
                                                                           : +std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::infinity());

          const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE im = (im_isneg ? -std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::infinity()
                                                                           : +std::numeric_limits<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>::infinity());

          return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(re, im);
        }

        if     (p == 2) { return  (x * x); }
        else if(p == 3) { return ((x * x) * x); }
        else if(p == 4) { const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> x2 = (x * x); return (x2 * x2); }
        else
        {
          // The variable xn stores the binary powers of x.
          complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> result(((p % 2) != 0) ? x : complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1)));
          complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> xn    (x);

          int p2 = p;

          while((p2 /= 2) != 0)
          {
            // Square xn for each binary power.
            xn *= xn;

            const bool has_binary_power = ((p2 % 2) != 0);

            if(has_binary_power)
            {
              // Multiply the result with each binary power contained in the exponent.
              result *= xn;
            }
          }

          return result;
        }
      }
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x,
                                                                    const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& a)
    {
      return std::exp(a * std::log(x));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x,
                                                                    const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& a)
    {
      return std::exp(a * std::log(x));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> pow(const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE& x,
                                                                    const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& a)
    {
      return std::exp(a * std::log(x));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> sinh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::sin;
      using std::cos;
      using std::exp;

      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sin_y  = sin (x.imag());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cos_y  = cos (x.imag());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_xp = exp (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_xm = BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) / exp_xp;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sinh_x = (exp_xp - exp_xm) / 2;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cosh_x = (exp_xp + exp_xm) / 2;

      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(cos_y * sinh_x, cosh_x * sin_y);
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> cosh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      using std::sin;
      using std::cos;
      using std::exp;

      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sin_y  = sin (x.imag());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cos_y  = cos (x.imag());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_xp = exp (x.real());
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE exp_xm = BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) / exp_xp;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE sinh_x = (exp_xp - exp_xm) / 2;
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE cosh_x = (exp_xp + exp_xm) / 2;

      return complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(cos_y * cosh_x, sin_y * sinh_x);
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> tanh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> ex_plus  = std::exp(x);
      const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> ex_minus = BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) / ex_plus;

      return (ex_plus - ex_minus) / (ex_plus + ex_minus);
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> asinh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      return std::log(x + std::sqrt((x * x) + BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1)));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> acosh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      const BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE my_one(1);

      const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> zp(x.real() + my_one, x.imag());
      const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> zm(x.real() - my_one, x.imag());

      return std::log(x + (zp * std::sqrt(zm / zp)));
    }

    inline complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> atanh(const complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      return (std::log(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) + x) - std::log(BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE(1) - x)) / 2.0;
    }

    template<class char_type, class traits_type>
    inline std::basic_ostream<char_type, traits_type>& operator<<(std::basic_ostream<char_type, traits_type>& os, const std::complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      std::basic_ostringstream<char_type, traits_type> ostr;

      ostr.flags(os.flags());
      ostr.imbue(os.getloc());
      ostr.precision(os.precision());

      ostr << char_type('(')
           << x.real()
           << char_type(',')
           << x.imag()
           << char_type(')');

      return (os << ostr.str());
    }

    template<class char_type, class traits_type>
    inline std::basic_istream<char_type, traits_type>& operator>>(std::basic_istream<char_type, traits_type>& is, std::complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>& x)
    {
      BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE rx;
      BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE ix;

      char_type the_char;

      static_cast<void>(is >> the_char);

      if(the_char == static_cast<char_type>('('))
      {
        static_cast<void>(is >> rx >> the_char);

        if(the_char == static_cast<char_type>(','))
        {
          static_cast<void>(is >> ix >> the_char);

          if(the_char == static_cast<char_type>(')'))
          {
            x = complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(rx, ix);
          }
          else
          {
            is.setstate(ios_base::failbit);
          }
        }
        else if(the_char == static_cast<char_type>(')'))
        {
          x = rx;
        }
        else
        {
          is.setstate(ios_base::failbit);
        }
      }
      else
      {
        static_cast<void>(is.putback(the_char));

        static_cast<void>(is >> rx);

        x = rx;
      }

      return is;
    }
  } // namespace std

#endif // _BOOST_CSTDFLOAT_COMPLEX_STD_2014_02_15_HPP_


// Boost.Geometry

// Copyright (c) 2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_PROJECTIONS_CONSTANTS_HPP
#define BOOST_GEOMETRY_PROJECTIONS_CONSTANTS_HPP

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

namespace boost { namespace geometry { namespace projections
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename T>
inline T fourth_pi() { return T(0.78539816339744830961566084581988); }
template <typename T>
inline T third_pi() { return boost::math::constants::third_pi<T>(); }
template <typename T>
inline T half_pi() { return boost::math::constants::half_pi<T>(); }
template <typename T>
inline T pi() { return boost::math::constants::pi<T>(); }
template <typename T>
inline T one_and_half_pi() { return T(4.7123889803846898576939650749193); }
template <typename T>
inline T two_pi() { return boost::math::constants::two_pi<T>(); }
template <typename T>
inline T two_and_half_pi() { return T(7.8539816339744830961566084581988); }

template <typename T>
inline T two_div_pi() { return boost::math::constants::two_div_pi<T>(); }
template <typename T>
inline T half_pi_sqr() { return T(2.4674011002723396547086227499689); }
template <typename T>
inline T pi_sqr() { return boost::math::constants::pi_sqr<T>(); }

template <typename T>
inline T sixth() { return boost::math::constants::sixth<T>(); }
template <typename T>
inline T third() { return boost::math::constants::third<T>(); }
template <typename T>
inline T two_thirds() { return boost::math::constants::two_thirds<T>(); }

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}}} // namespace boost::geometry::projections
#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PROJECTS_HPP


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

#include "quality.h"
#include "PGR_5.h"
#include "normal_component_box_surface.h"

using namespace std;
using namespace Eigen;
using namespace KDL;
const double PPI_  = 3.141592653589793238463;

const double PI_  = 3.141592653589793238463;

//////////////////////////////////  BOX

std::vector<double> box;
//////////////////////////////////////////////

/////////////////////////////////// CONTACT POINTS

std::vector<double> contact_points;
///////////////////////////////////////

////////////////////////////////////  CONTACT FORCE

std::vector<double> contact_forces;
/////////////////////////////////////////////

////////////////////////////////////// JOINTS

std::vector<double> joints;
////////////////////////////////////////////

int n_c = 0;
int n_q = 0;
double quality_i = 0;
double contact_stiffness = 100;
double joint_stiffness = 1;
double mu = 1.5;
double f_i_max = 200;

//////////////////////////////////////////// ROBOT SCTRUCTURE

KDL::Tree hand_tree("RootName");

  KDL::Chain chain_palm;

  KDL::Chain chain_finger_right_0;
  KDL::Chain chain_finger_left_0;
  KDL::Chain chain_finger_right_1;
  KDL::Chain chain_finger_left_1;
  KDL::Chain chain_finger_right_2;
  KDL::Chain chain_finger_left_2;

  KDL::Chain chain_palm_finger_left_0; // return the result
  KDL::Chain chain_palm_finger_left_1;
  KDL::Chain chain_palm_finger_left_2;
  KDL::Chain chain_palm_finger_right_0;
  KDL::Chain chain_palm_finger_right_1;
  KDL::Chain chain_palm_finger_right_2;

  KDL::JntArray jointpositions_right_0 = JntArray(8);
  KDL::JntArray jointpositions_right_1 = JntArray(8);
  KDL::JntArray jointpositions_right_2 = JntArray(8);
  KDL::JntArray jointpositions_left_0 = JntArray(8);
  KDL::JntArray jointpositions_left_1 = JntArray(8);
  KDL::JntArray jointpositions_left_2 = JntArray(8);

  KDL::Jacobian jacobian_right_0;
  KDL::Jacobian jacobian_left_0;
  KDL::Jacobian jacobian_right_1;
  KDL::Jacobian jacobian_left_1;
  KDL::Jacobian jacobian_right_2;
  KDL::Jacobian jacobian_left_2;

  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_right_0;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_left_0;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_right_1;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_left_1;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_right_2;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_left_2;

///////////////////////////////////////////////////////////

//------------------------------------------------------------------------------------------
void createStructureKDL()
{

  chain_palm.addSegment(Segment("palm_trasl_x_link",Joint(Joint::TransX),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_trasl_y_link",Joint(Joint::TransY),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_trasl_z_link",Joint(Joint::TransZ),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_rot_x_link",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_rot_y_link",Joint(Joint::RotY),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_rot_z_link",Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.0))));

  chain_finger_right_0.addSegment(Segment("finger_right_proximal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));
  chain_finger_right_0.addSegment(Segment("finger_right_distal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_left_0.addSegment(Segment("finger_left_proximal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.3))));
  chain_finger_left_0.addSegment(Segment("finger_left_distal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_right_1.addSegment(Segment("finger_right_proximal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));
  chain_finger_right_1.addSegment(Segment("finger_right_distal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_left_1.addSegment(Segment("finger_left_proximal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.3))));
  chain_finger_left_1.addSegment(Segment("finger_left_distal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_right_2.addSegment(Segment("finger_right_proximal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));
  chain_finger_right_2.addSegment(Segment("finger_right_distal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_left_2.addSegment(Segment("finger_left_proximal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.3))));
  chain_finger_left_2.addSegment(Segment("finger_left_distal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  hand_tree.addChain(chain_palm,"RootName");
  hand_tree.addChain(chain_finger_right_0,"palm_rot_z_link");
  hand_tree.addChain(chain_finger_left_0, "palm_rot_z_link");
  hand_tree.addChain(chain_finger_right_1,"palm_rot_z_link");
  hand_tree.addChain(chain_finger_left_1, "palm_rot_z_link");
  hand_tree.addChain(chain_finger_right_2,"palm_rot_z_link");
  hand_tree.addChain(chain_finger_left_2, "palm_rot_z_link");

  hand_tree.getChain("RootName", "finger_right_distal_link_0", chain_palm_finger_right_0);
  hand_tree.getChain("RootName", "finger_left_distal_link_0", chain_palm_finger_left_0);
  hand_tree.getChain("RootName", "finger_right_distal_link_1", chain_palm_finger_right_1);
  hand_tree.getChain("RootName", "finger_left_distal_link_1", chain_palm_finger_left_1);
  hand_tree.getChain("RootName", "finger_right_distal_link_2", chain_palm_finger_right_2);
  hand_tree.getChain("RootName", "finger_left_distal_link_2", chain_palm_finger_left_2);

}
//-------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------
void setJoints(std::vector<double>  joints_in)
{
  for(int i=0 ; i < 6 ; i++)
      jointpositions_right_0(i) = jointpositions_left_0(i) = jointpositions_right_1(i) = jointpositions_left_1(i) = jointpositions_right_2(i) = jointpositions_left_2(i) = joints[i];

    jointpositions_right_0(6) = joints_in[6];
    jointpositions_right_0(7) = joints_in[7];

    jointpositions_left_0(6) = joints_in[8];
    jointpositions_left_0(7) = joints_in[9];

    jointpositions_right_1(6) = joints_in[10];
    jointpositions_right_1(7) = joints_in[11];

    jointpositions_left_1(6) = joints_in[12];
    jointpositions_left_1(7) = joints_in[13];

    jointpositions_right_2(6) = joints_in[14];
    jointpositions_right_2(7) = joints_in[15];

    jointpositions_left_2(6) = joints_in[16];
    jointpositions_left_2(7) = joints_in[17];

  jointpositions_right_0.resize(chain_palm_finger_right_0.getNrOfJoints());
  jointpositions_left_0.resize(chain_palm_finger_left_0.getNrOfJoints());

  jointpositions_right_1.resize(chain_palm_finger_right_1.getNrOfJoints());
  jointpositions_left_1.resize(chain_palm_finger_left_1.getNrOfJoints());

  jointpositions_right_2.resize(chain_palm_finger_right_2.getNrOfJoints());
  jointpositions_left_2.resize(chain_palm_finger_left_2.getNrOfJoints());
}

void initJacobian()
{
    jacobian_right_0.resize(chain_palm_finger_right_0.getNrOfJoints());
    jacobian_left_0.resize(chain_palm_finger_left_0.getNrOfJoints());

    jnt_to_jac_right_0.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_right_0));
    jnt_to_jac_left_0.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_left_0));

    jacobian_right_1.resize(chain_palm_finger_right_1.getNrOfJoints());
    jacobian_left_1.resize(chain_palm_finger_left_1.getNrOfJoints());

    jnt_to_jac_right_1.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_right_1));
    jnt_to_jac_left_1.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_left_1));

    jacobian_right_2.resize(chain_palm_finger_right_2.getNrOfJoints());
    jacobian_left_2.resize(chain_palm_finger_left_2.getNrOfJoints());

    jnt_to_jac_right_2.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_right_2));
    jnt_to_jac_left_2.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_left_2));
}

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

  ros::init(argc, argv, "Test_file_4_finger");
  ros::NodeHandle nh;

  nh.param<std::vector<double>>("box", box, std::vector<double>{1, 1, 1});
  nh.param<std::vector<double>>("contact_points", contact_points, std::vector<double>{1,1,1,1,1,1});
  nh.param<std::vector<double>>("contact_forces", contact_forces, std::vector<double>{1,1,1,1,1,1});
  nh.param<std::vector<double>>("joints", joints, std::vector<double>{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
  nh.param<double>("mu", mu, 0.5);
  nh.param<double>("f_i_max", f_i_max, 1);
  nh.param<int>("n_c", n_c, 0);
  nh.param<int>("n_q", n_q, 0);

  if(n_c <= 0)
    return 0;

  createStructureKDL();
  setJoints(joints);
  initJacobian();

  Eigen::MatrixXd Skew_Matrix1(3,3);
  Eigen::MatrixXd Skew_Matrix2(3,3);
  Eigen::MatrixXd Skew_Matrix3(3,3);
  Eigen::MatrixXd Skew_Matrix4(3,3);
  Eigen::MatrixXd Skew_Matrix5(3,3);
  Eigen::MatrixXd Skew_Matrix6(3,3);

  Skew_Matrix1(0,0) = Skew_Matrix1(1,1) = Skew_Matrix1(2,2) = 0;
      Skew_Matrix1(0,1) = - contact_points[2]; // -rz
      Skew_Matrix1(0,2) = contact_points[1];   // ry
      Skew_Matrix1(1,0) = contact_points[2];   // rz
      Skew_Matrix1(2,0) = - contact_points[1]; // -ry
      Skew_Matrix1(1,2) = - contact_points[0]; // -rx
      Skew_Matrix1(2,1) = contact_points[0];   // rx
  // left

 Skew_Matrix2(0,0) = Skew_Matrix2(1,1) = Skew_Matrix2(2,2) = 0;
      Skew_Matrix2(0,1) = - contact_points[5]; // -rz
      Skew_Matrix2(0,2) = contact_points[4];   // ry
      Skew_Matrix2(1,0) = contact_points[5];   // rz
      Skew_Matrix2(2,0) = - contact_points[4]; // -ry
      Skew_Matrix2(1,2) = - contact_points[3]; // -rx
      Skew_Matrix2(2,1) = contact_points[3];   // rx
  // right

  Skew_Matrix3(0,0) = Skew_Matrix3(1,1) = Skew_Matrix3(2,2) = 0;
      Skew_Matrix3(0,1) = - contact_points[8]; // -rz
      Skew_Matrix3(0,2) = contact_points[7];   // ry
      Skew_Matrix3(1,0) = contact_points[8];   // rz
      Skew_Matrix3(2,0) = - contact_points[7]; // -ry
      Skew_Matrix3(1,2) = - contact_points[6]; // -rx
      Skew_Matrix3(2,1) = contact_points[6];   // rx
  // up

 Skew_Matrix4(0,0) = Skew_Matrix4(1,1) = Skew_Matrix4(2,2) = 0;
      Skew_Matrix4(0,1) = - contact_points[11]; // -rz
      Skew_Matrix4(0,2) = contact_points[10];   // ry
      Skew_Matrix4(1,0) = contact_points[11];   // rz
      Skew_Matrix4(2,0) = - contact_points[10]; // -ry
      Skew_Matrix4(1,2) = - contact_points[9]; // -rx
      Skew_Matrix4(2,1) = contact_points[9];   // rx
  // down

  Skew_Matrix5(0,0) = Skew_Matrix5(1,1) = Skew_Matrix5(2,2) = 0;
      Skew_Matrix5(0,1) = - contact_points[14]; // -rz
      Skew_Matrix5(0,2) = contact_points[13];   // ry
      Skew_Matrix5(1,0) = contact_points[14];   // rz
      Skew_Matrix5(2,0) = - contact_points[13]; // -ry
      Skew_Matrix5(1,2) = - contact_points[12]; // -rx
      Skew_Matrix5(2,1) = contact_points[12];   // rx
  // front of

 Skew_Matrix6(0,0) = Skew_Matrix6(1,1) = Skew_Matrix6(2,2) = 0;
      Skew_Matrix6(0,1) = - contact_points[17]; // -rz
      Skew_Matrix6(0,2) = contact_points[16];   // ry
      Skew_Matrix6(1,0) = contact_points[17];   // rz
      Skew_Matrix6(2,0) = - contact_points[16]; // -ry
      Skew_Matrix6(1,2) = - contact_points[15]; // -rx
      Skew_Matrix6(2,1) = contact_points[15];   // rx
  // behind

Eigen::MatrixXd G_c_t = MatrixXd::Zero(6, 6*n_c);
Eigen::MatrixXd G1 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G2 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G3 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G4 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G5 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G6 = MatrixXd::Zero(6,6);

Eigen::MatrixXd R1(3,3);
Eigen::MatrixXd R2(3,3);
Eigen::MatrixXd R3(3,3);
Eigen::MatrixXd R4(3,3);
Eigen::MatrixXd R5(3,3);
Eigen::MatrixXd R6(3,3);

R1 << 1, 0, 0,
       0, cos(-90*PPI_/180), -sin(-90*PPI_/180),
       0, sin(-90*PPI_/180), cos(-90*PPI_/180);
// left

R2 << 1, 0, 0,
      0, cos(90*PPI_/180), -sin(90*PPI_/180),
      0, sin(90*PPI_/180), cos(90*PPI_/180);
// right

R3 << 1, 0, 0,
      0, cos(180*PPI_/180), -sin(180*PPI_/180),
      0, sin(180*PPI_/180), cos(180*PPI_/180);
// up

R4 << 1, 0, 0,
      0, 1, 0,
      0, 0, 1;
// down

R5 <<  cos(-90*PPI/180),  0, sin(-90*PPI/180),
                   0   ,  1,    0 ,
      -sin(-90*PPI/180),  0, cos(-90*PPI/180);
//front of

R6 << cos(90*PPI/180),  0, sin(90*PPI/180),
                 0   ,  1,    0 ,
      -sin(90*PPI/180), 0, cos(90*PPI/180);
//behind

G1.block<3,3>(0,0) = R1.transpose();
G1.block<3,3>(3,3) = R1.transpose();
G1.block<3,3>(3,0) = Skew_Matrix1 * R1.transpose();

G2.block<3,3>(0,0) = R2.transpose();
G2.block<3,3>(3,3) = R2.transpose();
G2.block<3,3>(3,0) = Skew_Matrix2 * R2.transpose();

G3.block<3,3>(0,0) = R3.transpose();
G3.block<3,3>(3,3) = R3.transpose();
G3.block<3,3>(3,0) = Skew_Matrix3 * R3.transpose();

G4.block<3,3>(0,0) = R4.transpose();
G4.block<3,3>(3,3) = R4.transpose();
G4.block<3,3>(3,0) = Skew_Matrix4 * R4.transpose();

G5.block<3,3>(0,0) = R5.transpose();
G5.block<3,3>(3,3) = R5.transpose();
G5.block<3,3>(3,0) = Skew_Matrix5 * R5.transpose();

G6.block<3,3>(0,0) = R6.transpose();
G6.block<3,3>(3,3) = R6.transpose();
G6.block<3,3>(3,0) = Skew_Matrix6 * R6.transpose();

G_c_t.block<6,6>(0,0) = G1;
G_c_t.block<6,6>(0,6) = G2;
G_c_t.block<6,6>(0,12) = G3;
G_c_t.block<6,6>(0,18) = G4;
G_c_t.block<6,6>(0,24) = G5;
G_c_t.block<6,6>(0,30) = G6;

Eigen::MatrixXd Jacobian = MatrixXd::Zero(6*n_c,n_q);

jnt_to_jac_right_0->JntToJac(jointpositions_right_0, jacobian_right_0);
jnt_to_jac_left_0->JntToJac(jointpositions_left_0, jacobian_left_0);

jnt_to_jac_right_1->JntToJac(jointpositions_right_1, jacobian_right_1);
jnt_to_jac_left_1->JntToJac(jointpositions_left_1, jacobian_left_1);

jnt_to_jac_right_2->JntToJac(jointpositions_right_2, jacobian_right_2);
jnt_to_jac_left_2->JntToJac(jointpositions_left_2, jacobian_left_2);

Jacobian.block<6,8>(0,0) = jacobian_right_0.data;                  // first contact - first finger

Jacobian.block<6,6>(6,0) = jacobian_left_0.data.block<6,6>(0,0);   // second contact - second finger
Jacobian.block<6,2>(6,8) = jacobian_left_0.data.block<6,2>(0,6);

Jacobian.block<6,6>(12,0) = jacobian_right_1.data.block<6,6>(0,0); // third contact - third finger
Jacobian.block<6,2>(12,10) = jacobian_right_1.data.block<6,2>(0,6);

Jacobian.block<6,6>(18,0) = jacobian_left_1.data.block<6,6>(0,0);  // four-th contact - four-th finger
Jacobian.block<6,2>(18,12) = jacobian_left_1.data.block<6,2>(0,6);

Jacobian.block<6,6>(24,0) = jacobian_right_2.data.block<6,6>(0,0);  // five-th contact - five-th finger
Jacobian.block<6,2>(24,14) = jacobian_right_2.data.block<6,2>(0,6);

Jacobian.block<6,6>(30,0) = jacobian_left_2.data.block<6,6>(0,0);  // six-th contact - six-th finger
Jacobian.block<6,2>(30,16) = jacobian_left_2.data.block<6,2>(0,6);

    Eigen::MatrixXd R_c = MatrixXd::Zero(3*n_c,3*n_c);
    int k = 0;
    for(int i = 0 ; i < n_c ; i++)
    {
      R_c.block<3,3>(k,k) = MatrixXd::Identity(3,3);
      k += 3;
    }

    Eigen::VectorXd f_c_(contact_forces.size());
    for(int i = 0 ; i < contact_forces.size() ; i++ )
      f_c_(i) = contact_forces[i];

    Eigen::MatrixXd Contact_Stiffness_Matrix = MatrixXd::Zero(3,3);   // Kis
      for(int i = 0 ; i < Contact_Stiffness_Matrix.rows() ; i++) //Kis
          Contact_Stiffness_Matrix(i,i) = contact_stiffness;

    Eigen::MatrixXd Joint_Stiffness_Matrix = MatrixXd::Zero(n_q,n_q);    // Kp
      for(int j = 0 ; j < Joint_Stiffness_Matrix.rows() ; j++) //Kp
          Joint_Stiffness_Matrix(j,j) = joint_stiffness;

  //quality_i = quality_pcr_pgr_5(f_c_, G_c, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);
 // quality_i = quality_pcr_pgr_5(f_c_, G_b, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);

  quality_i = quality_pcr_pgr_5(f_c_, G_c_t, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);

  cout << " quality : " << quality_i << endl;

  ros::spinOnce();
  return 0;
}