//
// $Id: convert2dense.hpp 5313 2013-12-17 18:06:54Z chambm $
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef CONVERT2DENSE_H
#define CONVERT2DENSE_H

#include <vector>
#include <iostream>
#include <numeric>
#include <boost/assert.hpp>
#include <boost/cstdint.hpp>
#include "pwiz/utility/findmf/base/resample/masscomparefunctors.hpp"
#include "pwiz/utility/findmf/base/resample/breakspec.hpp"
#include "pwiz/utility/findmf/base/resample/bin1d.hpp"

namespace ralab
{
  namespace base
  {
    namespace resample
    {
      typedef boost::int32_t int32_t;
      struct Convert2Dense
      {
        ralab::base::resample::Bin1D bin_;
        std::vector<int32_t> idx_;//small workder vecs
        std::vector<double> weight_;
        double am_; //parameter describing the sampling width
        Convert2Dense(double am = 0.1):bin_(),idx_(),weight_(),am_(){
        }

        /// computes split points of an map.
        std::size_t defBreak(std::pair<double, double> & mzrange, double ppm ){
          ralab::base::resample::PPMCompFunctor<double> ppmf(ppm);
          ralab::base::resample::breaks( mzrange.first - 1. , mzrange.second + 1. , ppmf , bin_.breaks_ );
          bin_.reset();
          return bin_.breaks_.size();
        }

        /// Converts a sparse spec to a dense spec
        template<typename Tmass, typename Tintens, typename Tout >
        void convert2dense(Tmass beginMass, Tmass endMass, Tintens intens,
                           Tout ass
                           )
        {

          for( ; beginMass != (endMass -1) ; ++beginMass, ++intens ){
              double mass1 = *beginMass;
              double mass2 = *(beginMass+1);
              double predmass2 = mass1 + (am_* sqrt(mass1))*1.01;
              if(mass2 > predmass2){
                  mass2 = predmass2;
                }

              double deltamass = mass2-mass1;
              double deltamasshalf;
              if(true){
                  deltamasshalf= deltamass/2.;
                }
              else{
                  deltamasshalf = deltamass;
                }

              bin_(mass1-deltamasshalf,mass2-deltamasshalf,idx_,weight_);

              double intensd = static_cast<double>(*intens);
              double sum = std::accumulate(weight_.begin(),weight_.end(),0.);
              BOOST_ASSERT(fabs(deltamass- sum) < 1e-11);

              double check = 0.;
              for(std::size_t i = 0 ; i < idx_.size();++i){
                  if((idx_[i]>=0) &(idx_[i] < static_cast<int32_t>(bin_.breaks_.size() - 1)))
                    {
                      double bb= intensd * weight_[i]/deltamass;
                      *(ass + idx_[i])  += bb;
                      check += bb;
                    }
                }
              BOOST_ASSERT( fabs(check - intensd) < 1e-3 );
            }
        }//convert2dense

        void getMids(std::vector<double> & mids)
        {
          ralab::base::resample::getMids(bin_.breaks_, mids );
        }

        /// Converts a sparse spec to a dense spec
        template<typename Tmass, typename Tintens >
        void convert2dense(Tmass beginMass, Tmass endMass, Tintens intens,
                           std::vector<typename std::iterator_traits<Tintens>::value_type > & gg
                           ){
          gg.resize(bin_.breaks_.size() - 1);
          convert2dense(beginMass,endMass, intens, gg.begin());
        }
      };

    }
  }
}
#endif // CONVERT2DENSE_H


#include "engine/polyline_compressor.hpp"

#include <algorithm>
#include <boost/assert.hpp>
#include <cmath>
#include <cstddef>
#include <cstdlib>

namespace osrm
{
namespace engine
{
namespace detail // anonymous to keep TU local
{

std::string encode(int number_to_encode)
{
    std::string output;
    while (number_to_encode >= 0x20)
    {
        const int next_value = (0x20 | (number_to_encode & 0x1f)) + 63;
        output += static_cast<char>(next_value);
        number_to_encode >>= 5;
    }

    number_to_encode += 63;
    output += static_cast<char>(number_to_encode);
    return output;
}

std::string encode(std::vector<int> &numbers)
{
    std::string output;
    for (auto &number : numbers)
    {
        if (number < 0)
        {
            const unsigned binary = std::llabs(number);
            const unsigned twos = (~binary) + 1u;
            const unsigned shl = twos << 1u;
            number = static_cast<int>(~shl);
        }
        else
        {
            number <<= 1u;
        }
    }
    for (const int number : numbers)
    {
        output += encode(number);
    }
    return output;
}
}

std::vector<util::Coordinate> decodePolyline(const std::string &geometry_string)
{
    std::vector<util::Coordinate> new_coordinates;
    int index = 0, len = geometry_string.size();
    int lat = 0, lng = 0;

    while (index < len)
    {
        int b, shift = 0, result = 0;
        do
        {
            b = geometry_string.at(index++) - 63;
            result |= (b & 0x1f) << shift;
            shift += 5;
        } while (b >= 0x20 && index < len);
        int dlat = ((result & 1) != 0 ? ~(result >> 1) : (result >> 1));
        lat += dlat;

        shift = 0;
        result = 0;
        do
        {
            b = geometry_string.at(index++) - 63;
            result |= (b & 0x1f) << shift;
            shift += 5;
        } while (b >= 0x20 && index < len);
        int dlng = ((result & 1) != 0 ? ~(result >> 1) : (result >> 1));
        lng += dlng;

        util::Coordinate p;
        p.lat =
            util::FixedLatitude{static_cast<std::int32_t>(lat * detail::POLYLINE_TO_COORDINATE)};
        p.lon =
            util::FixedLongitude{static_cast<std::int32_t>(lng * detail::POLYLINE_TO_COORDINATE)};
        new_coordinates.push_back(p);
    }

    return new_coordinates;
}
}
}


// Copyright (c) 2018 yshurik
//
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
//
// The use in another cyptocurrency project the code is licensed under
// Jelurida Public License (JPL). See https://www.jelurida.com/resources/jpl

#include "pegdata.h"
#include "utilstrencodings.h"

#include <map>
#include <set>
#include <cstdint>
#include <utility>
#include <algorithm>
#include <type_traits>

#include <boost/multiprecision/cpp_int.hpp>

#include <zconf.h>
#include <zlib.h>

using namespace std;
using namespace boost;

int64_t RatioPart(int64_t nValue,
                  int64_t nPartValue,
                  int64_t nTotalValue) {
    if (nPartValue == 0 || nTotalValue == 0)
        return 0;

    bool has_overflow = false;
    if (std::is_same<int64_t,long>()) {
        long m_test;
        has_overflow = __builtin_smull_overflow(nValue, nPartValue, &m_test);
    } else if (std::is_same<int64_t,long long>()) {
        long long m_test;
        has_overflow = __builtin_smulll_overflow(nValue, nPartValue, &m_test);
    } else {
        assert(0); // todo: compile error
    }

    if (has_overflow) {
        multiprecision::uint128_t v128(nValue);
        multiprecision::uint128_t part128(nPartValue);
        multiprecision::uint128_t f128 = (v128*part128)/nTotalValue;
        return f128.convert_to<int64_t>();
    }

    return (nValue*nPartValue)/nTotalValue;
}

CPegData::CPegData(std::string pegdata64)
{
    if (pegdata64.empty()) {
        peglevel = CPegLevel(0,0,0,0,0,0);
        return; // defaults, zeros
    }

    string pegdata = DecodeBase64(pegdata64);
    CDataStream finp(pegdata.data(),
                     pegdata.data() + pegdata.size(),
                     SER_NETWORK, CLIENT_VERSION);
    bool ok = Unpack(finp);
    if (!ok) {
        // try prev versions
        CDataStream finp(pegdata.data(),
                         pegdata.data() + pegdata.size(),
                         SER_DISK, CLIENT_VERSION);

        bool ok = Unpack2(finp);
        if (!ok) {
            // try prev versions
            CDataStream finp(pegdata.data(),
                              pegdata.data() + pegdata.size(),
                              SER_DISK, CLIENT_VERSION);
            ok = Unpack1(finp);

            if (!ok) {
                // peglevel to inicate invalid
                peglevel = CPegLevel();
            }
        }
    }
}

bool CPegData::IsValid() const
{
    if (!peglevel.IsValid()) return false;

    // match total
    if ((nReserve+nLiquid) != fractions.Total()) return false;

    // validate liquid/reserve match peglevel
    int nSupplyEffective = peglevel.nSupply+peglevel.nShift;
    bool fPartial = peglevel.nShiftLastPart >0 && peglevel.nShiftLastTotal >0;
    if (fPartial) {
        nSupplyEffective++;
        int64_t nLiquidWithoutPartial = fractions.High(nSupplyEffective);
        int64_t nReserveWithoutPartial = fractions.Low(nSupplyEffective-1);
        if (nLiquid < nLiquidWithoutPartial) return false;
        if (nReserve < nReserveWithoutPartial) return false;
    }
    else {
        int64_t nLiquidCalc = fractions.High(nSupplyEffective);
        int64_t nReserveCalc = fractions.Low(nSupplyEffective);
        if (nLiquid != nLiquidCalc) return false;
        if (nReserve != nReserveCalc) return false;
    }

    return true;
}

bool CPegData::Pack(CDataStream & fout) const {
    fout << nVersion;
    fractions.Pack(fout);
    peglevel.Pack(fout);
    fout << nReserve;
    fout << nLiquid;
    fout << nId;
    return true;
}

bool CPegData::Unpack(CDataStream & finp) {
    try {
        finp >> nVersion;
        if (!fractions.Unpack(finp)) return false;
        if (!peglevel.Unpack(finp)) return false;
        finp >> nReserve;
        finp >> nLiquid;
        finp >> nId;

        if (!IsValid())
            return false;
    }
    catch (std::exception &) {
        return false;
    }

    fractions = fractions.Std();
    return true;
}

std::string CPegData::ToString() const {
    CDataStream fout(SER_NETWORK, CLIENT_VERSION);
    Pack(fout);
    return EncodeBase64(fout.str());
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE r1cs_ppzksnark_test

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/fields/mnt4/base_field.hpp>
#include <nil/crypto3/algebra/fields/mnt4/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt4.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt4.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>
#include <nil/crypto3/algebra/fields/mnt6/base_field.hpp>
#include <nil/crypto3/algebra/fields/mnt6/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt6.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt6.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/mnt6.hpp>

#include <nil/crypto3/algebra/random_element.hpp>

#include <nil/crypto3/zk/components/algebra/pairing/detail/mnt4.hpp>
#include <nil/crypto3/zk/components/algebra/pairing/detail/mnt6.hpp>
//#include <nil/crypto3/zk/components/algebra/pairing/pairing_params.hpp>

#include <nil/crypto3/zk/components/algebra/fields/element_fp2.hpp>
#include <nil/crypto3/zk/components/algebra/fields/element_fp3.hpp>
#include <nil/crypto3/zk/components/algebra/fields/element_fp4.hpp>
#include <nil/crypto3/zk/components/algebra/fields/element_fp6_2over3.hpp>
#include <nil/crypto3/zk/snark/components/verifiers/r1cs_ppzksnark_verifier_component.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_ppzksnark.hpp>

#include "../pairing/weierstrass_miller_loop.hpp"
#include "../r1cs_examples.hpp"

using namespace nil::crypto3::zk;
using namespace nil::crypto3::zk::snark;
using namespace nil::crypto3::algebra;

template<typename ppT_A, typename ppT_B>
void test_verifier() {
    typedef typename ppT_A::scalar_field_type FieldT_A;
    typedef typename ppT_B::scalar_field_type FieldT_B;

    const std::size_t num_constraints = 50;
    const std::size_t primary_input_size = 3;

    r1cs_example<FieldT_A> example =
        generate_r1cs_example_with_field_input<FieldT_A>(num_constraints, primary_input_size);
    BOOST_CHECK(example.primary_input.size() == primary_input_size);

    BOOST_CHECK(example.constraint_system.is_satisfied(example.primary_input, example.auxiliary_input));
    const r1cs_ppzksnark_keypair<ppT_A> keypair = r1cs_ppzksnark_generator<ppT_A>(example.constraint_system);
    const r1cs_ppzksnark_proof<ppT_A> pi =
        r1cs_ppzksnark_prover<ppT_A>(keypair.first, example.primary_input, example.auxiliary_input);
    bool bit = r1cs_ppzksnark_verifier_strong_input_consistency<ppT_A>(keypair.second, example.primary_input, pi);
    BOOST_CHECK(bit);

    const std::size_t elt_size = FieldT_A::size_in_bits();
    const std::size_t primary_input_size_in_bits = elt_size * primary_input_size;
    const std::size_t vk_size_in_bits =
        r1cs_ppzksnark_verification_key_variable<ppT_B>::size_in_bits(primary_input_size);

    components::blueprint<FieldT_B> bp;
    components::blueprint_variable_vector<FieldT_B> vk_bits;
    vk_bits.allocate(bp, vk_size_in_bits);

    components::blueprint_variable_vector<FieldT_B> primary_input_bits;
    primary_input_bits.allocate(bp, primary_input_size_in_bits);

    r1cs_ppzksnark_proof_variable<ppT_B> proof(bp);

    r1cs_ppzksnark_verification_key_variable<ppT_B> vk(bp, vk_bits, primary_input_size);

    components::blueprint_variable<FieldT_B> result;
    result.allocate(bp);

    r1cs_ppzksnark_verifier_component<ppT_B> verifier(bp, vk, primary_input_bits, elt_size, proof, result);

    proof.generate_r1cs_constraints();
    verifier.generate_r1cs_constraints();

    std::vector<bool> input_as_bits;
    for (const FieldT_A &el : example.primary_input) {
        std::vector<bool> v = algebra::convert_field_element_to_bit_vector<FieldT_A>(el, elt_size);
        input_as_bits.insert(input_as_bits.end(), v.begin(), v.end());
    }

    primary_input_bits.fill_with_bits(bp, input_as_bits);

    vk.generate_r1cs_witness(keypair.second);
    proof.generate_r1cs_witness(pi);
    verifier.generate_r1cs_witness();
    bp.val(result) = FieldT_B::one();

    std::cout << "positive test:\n" << std::endl;
    BOOST_CHECK(bp.is_satisfied());

    bp.val(primary_input_bits[0]) = FieldT_B::one() - bp.val(primary_input_bits[0]);
    verifier.generate_r1cs_witness();
    bp.val(result) = FieldT_B::one();

    std::cout << "negative test:" << std::endl;
    BOOST_CHECK(!bp.is_satisfied());
    std::cout << "number of constraints for verifier:" << bp.num_constraints() << std::endl;
}

template<typename ppT_A, typename ppT_B>
void test_hardcoded_verifier() {
    typedef typename ppT_A::scalar_field_type FieldT_A;
    typedef typename ppT_B::scalar_field_type FieldT_B;

    const std::size_t num_constraints = 50;
    const std::size_t primary_input_size = 3;

    r1cs_example<FieldT_A> example =
        generate_r1cs_example_with_field_input<FieldT_A>(num_constraints, primary_input_size);
    BOOST_CHECK(example.primary_input.size() == primary_input_size);

    BOOST_CHECK(example.constraint_system.is_satisfied(example.primary_input, example.auxiliary_input));
    const r1cs_ppzksnark_keypair<ppT_A> keypair = r1cs_ppzksnark_generator<ppT_A>(example.constraint_system);
    const r1cs_ppzksnark_proof<ppT_A> pi =
        r1cs_ppzksnark_prover<ppT_A>(keypair.first, example.primary_input, example.auxiliary_input);
    bool bit = r1cs_ppzksnark_verifier_strong_input_consistency<ppT_A>(keypair.second, example.primary_input, pi);
    BOOST_CHECK(bit);

    const std::size_t elt_size = FieldT_A::size_in_bits();
    const std::size_t primary_input_size_in_bits = elt_size * primary_input_size;

    components::blueprint<FieldT_B> bp;
    r1cs_ppzksnark_preprocessed_r1cs_ppzksnark_verification_key_variable<ppT_B> hardcoded_vk(bp, keypair.second);
    components::blueprint_variable_vector<FieldT_B> primary_input_bits;
    primary_input_bits.allocate(bp, primary_input_size_in_bits);

    r1cs_ppzksnark_proof_variable<ppT_B> proof(bp);

    components::blueprint_variable<FieldT_B> result;
    result.allocate(bp);

    r1cs_ppzksnark_online_verifier_component<ppT_B> online_verifier(bp, hardcoded_vk, primary_input_bits, elt_size,
                                                                    proof, result);

    proof.generate_r1cs_constraints();
    online_verifier.generate_r1cs_constraints();

    std::vector<bool> input_as_bits;
    for (const FieldT_A &el : example.primary_input) {
        std::vector<bool> v = algebra::convert_field_element_to_bit_vector<FieldT_A>(el, elt_size);
        input_as_bits.insert(input_as_bits.end(), v.begin(), v.end());
    }

    primary_input_bits.fill_with_bits(bp, input_as_bits);

    proof.generate_r1cs_witness(pi);
    online_verifier.generate_r1cs_witness();
    bp.val(result) = FieldT_B::one();

    printf("positive test:\n");
    BOOST_CHECK(bp.is_satisfied());

    bp.val(primary_input_bits[0]) = FieldT_B::one() - bp.val(primary_input_bits[0]);
    online_verifier.generate_r1cs_witness();
    bp.val(result) = FieldT_B::one();

    printf("negative test:\n");
    BOOST_CHECK(!bp.is_satisfied());
    std::cout << "number of constraints for verifier: " << bp.num_constraints();
}

template<typename FpExtT, template<class> class VarT, template<class> class MulT>
void test_mul() {
    typedef typename FpExtT::my_Fp FieldType;

    components::blueprint<FieldType> bp;
    VarT<FpExtT> x(bp);
    VarT<FpExtT> y(bp);
    VarT<FpExtT> xy(bp);
    MulT<FpExtT> mul(bp, x, y, xy);
    mul.generate_r1cs_constraints();

    for (size_t i = 0; i < 10; ++i) {
        const typename FpExtT::value_type x_val = algebra::random_element<FpExtT>();
        const typename FpExtT::value_type y_val = algebra::random_element<FpExtT>();
        x.generate_r1cs_witness(x_val);
        y.generate_r1cs_witness(y_val);
        mul.generate_r1cs_witness();
        const typename FpExtT::value_type res = xy.get_element();
        BOOST_CHECK(res == x_val * y_val);
        BOOST_CHECK(bp.is_satisfied());
    }
    std::cout << "number of constraints:" << bp.num_constraints() << std::endl;
}

template<typename FpExtT, template<class> class VarT, template<class> class SqrT>
void test_sqr() {
    typedef typename FpExtT::my_Fp FieldType;

    components::blueprint<FieldType> bp;
    VarT<FpExtT> x(bp);
    VarT<FpExtT> xsq(bp);
    SqrT<FpExtT> sqr(bp, x, xsq);
    sqr.generate_r1cs_constraints();

    for (size_t i = 0; i < 10; ++i) {
        const typename FpExtT::value_type x_val = algebra::random_element<FpExtT>();
        x.generate_r1cs_witness(x_val);
        sqr.generate_r1cs_witness();
        const typename FpExtT::value_type res = xsq.get_element();
        BOOST_CHECK(res == x_val.squared());
        BOOST_CHECK(bp.is_satisfied());
    }
    std::cout << "number of constraints: " << bp.num_constraints() << std::endl;
}

template<typename CurveType, template<class> class VarT, template<class> class CycloSqrT>
void test_cyclotomic_sqr() {
    typedef algebra::Fqk<CurveType> FpExtT;
    typedef typename FpExtT::my_Fp FieldType;

    components::blueprint<FieldType> bp;
    VarT<FpExtT> x(bp);
    VarT<FpExtT> xsq(bp);
    CycloSqrT<FpExtT> sqr(bp, x, xsq);
    sqr.generate_r1cs_constraints();

    for (size_t i = 0; i < 10; ++i) {
        FpExtT::value_type x_val = algebra::random_element<FpExtT>();
        x_val = final_exponentiation<CurveType>(x_val);

        x.generate_r1cs_witness(x_val);
        sqr.generate_r1cs_witness();
        const typename FpExtT::value_type res = xsq.get_element();
        BOOST_CHECK(res == x_val.squared());
        BOOST_CHECK(bp.is_satisfied());
    }
    std::cout << "number of constraints: " << bp.num_constraints() << std::endl;
}

template<typename FpExtT, template<class> class VarT>
void test_Frobenius() {
    typedef typename FpExtT::my_Fp FieldType;

    for (size_t i = 0; i < 100; ++i) {
        components::blueprint<FieldType> bp;
        VarT<FpExtT> x(bp);
        VarT<FpExtT> x_frob = x.Frobenius_map(i);

        const typename FpExtT::value_type x_val = algebra::random_element<FpExtT>();
        x.generate_r1cs_witness(x_val);
        x_frob.evaluate();
        const typename FpExtT::value_type res = x_frob.get_element();
        BOOST_CHECK(res == x_val.Frobenius_map(i));
        BOOST_CHECK(bp.is_satisfied());
    }
}

template<typename CurveType>
void test_full_pair() {
    typedef typename CurveType::scalar_field_type FieldType;
    typedef typename pairing::CurveType::pairing::pair_curve_type::pairing pairing_policy;

    components::blueprint<FieldType> bp;
    pairing::CurveType::pairing::pair_curve_type::template g1_type<>::value_type P_val =
        algebra::random_element<pairing::CurveType::pairing::pair_curve_type::scalar_field_type>() *
        pairing::CurveType::pairing::pair_curve_type::template g1_type<>::value_type::one();
    pairing::CurveType::pairing::pair_curve_type::template g2_type<>::value_type Q_val =
        algebra::random_element<pairing::CurveType::pairing::pair_curve_type::scalar_field_type>() *
        pairing::CurveType::pairing::pair_curve_type::template g2_type<>::value_type::one();

    element_g1<CurveType> P(bp);
    element_g2<CurveType> Q(bp);
    g1_precomputation<CurveType> prec_P;
    g2_precomputation<CurveType> prec_Q;

    precompute_G1_component<CurveType> compute_prec_P(bp, P, prec_P);
    precompute_G2_component<CurveType> compute_prec_Q(bp, Q, prec_Q);

    Fqk_variable<CurveType> miller_result(bp);
    mnt_miller_loop_component<CurveType> miller(bp, prec_P, prec_Q, miller_result);
    components::blueprint_variable<FieldType> result_is_one;
    result_is_one.allocate(bp);
    final_exp_component<CurveType> finexp(bp, miller_result, result_is_one);

    compute_prec_P.generate_r1cs_constraints();
    compute_prec_Q.generate_r1cs_constraints();
    miller.generate_r1cs_constraints();
    finexp.generate_r1cs_constraints();

    P.generate_r1cs_witness(P_val);
    compute_prec_P.generate_r1cs_witness();
    Q.generate_r1cs_witness(Q_val);
    compute_prec_Q.generate_r1cs_witness();
    miller.generate_r1cs_witness();
    finexp.generate_r1cs_witness();
    BOOST_CHECK(bp.is_satisfied());

    typename pairing_policy::affine_ate_g1_precomp native_prec_P = pairing_policy::affine_ate_precompute_g1(P_val);
    typename pairing_policy::affine_ate_g2_precomp native_prec_Q = pairing_policy::affine_ate_precompute_g2(Q_val);
    typename pairing_policy::Fqk native_miller_result =
        pairing_policy::affine_ate_miller_loop(native_prec_P, native_prec_Q);

    typename pairing_policy::Fqk native_finexp_result = pairing_policy::final_exponentiation(native_miller_result);
    printf("Must match:\n");
    finexp.result->get_element().print();
    native_finexp_result.print();

    BOOST_CHECK(finexp.result->get_element() == native_finexp_result);

    std::cout << "number of constraints for full pairing: " << bp.num_constraints() << std::endl;
}

template<typename CurveType>
void test_full_precomputed_pair() {
    typedef typename CurveType::scalar_field_type FieldType;
    typedef typename pairing::CurveType::pairing::pair_curve_type::pairing pairing_policy;

    components::blueprint<FieldType> bp;
    pairing::CurveType::pairing::pair_curve_type::template g1_type<>::value_type P_val =
        algebra::random_element<pairing::CurveType::pairing::pair_curve_type::scalar_field_type>() *
        pairing::CurveType::pairing::pair_curve_type::template g1_type<>::value_type::one();
    pairing::CurveType::pairing::pair_curve_type::template g2_type<>::value_type Q_val =
        algebra::random_element<pairing::CurveType::pairing::pair_curve_type::scalar_field_type>() *
        pairing::CurveType::pairing::pair_curve_type::template g2_type<>::value_type::one();

    g1_precomputation<CurveType> prec_P(bp, P_val);
    g2_precomputation<CurveType> prec_Q(bp, Q_val);

    Fqk_variable<CurveType> miller_result(bp);
    mnt_miller_loop_component<CurveType> miller(bp, prec_P, prec_Q, miller_result);
    components::blueprint_variable<FieldType> result_is_one;
    result_is_one.allocate(bp);
    final_exp_component<CurveType> finexp(bp, miller_result, result_is_one);

    miller.generate_r1cs_constraints();
    finexp.generate_r1cs_constraints();

    miller.generate_r1cs_witness();
    finexp.generate_r1cs_witness();
    BOOST_CHECK(bp.is_satisfied());

    typename pairing_policy::affine_ate_g1_precomp native_prec_P = pairing_policy::affine_ate_precompute_g1(P_val);
    typename pairing_policy::affine_ate_g2_precomp native_prec_Q = pairing_policy::affine_ate_precompute_g2(Q_val);
    typename pairing_policy::Fqk native_miller_result =
        pairing_policy::affine_ate_miller_loop(native_prec_P, native_prec_Q);

    typename pairing_policy::Fqk native_finexp_result = pairing_policy::final_exponentiation(native_miller_result);
    printf("Must match:\n");
    finexp.result->get_element().print();
    native_finexp_result.print();

    BOOST_CHECK(finexp.result->get_element() == native_finexp_result);

    std::cout << "number of constraints for full precomputed pairing: " << bp.num_constraints() << std::endl;
}

BOOST_AUTO_TEST_SUITE(benes_components_test_suite)

BOOST_AUTO_TEST_CASE(benes_components_mnt4_test) {

    std::cout << "Benes components test for mnt4-298 started" << std::endl;
    using curve_type = typename algebra::curves::mnt4<298>;
    using fq2_type = typename curve_type::template g2_type<>::field_type;
    using fq4_type = typename curve_type::gt_type;

    test_mul<fq2_type, element_fp2, element_fp2_mul>();
    test_sqr<fq2_type, element_fp2, element_fp2_squared>();

    test_mul<fq4_type, element_fp4, element_fp4_mul>();
    test_sqr<fq4_type, element_fp4, element_fp4_squared>();
    test_cyclotomic_sqr<curve_type, element_fp4,
        element_fp4_cyclotomic_squared>();
    test_exponentiation_component<fq4_type, element_fp4, element_fp4_mul, element_fp4_squared,
                                  algebra::mnt4_q_limbs>(curve_type::pairing::final_exponent_last_chunk_abs_of_w0);
    test_Frobenius<fq4_type, element_fp4>();

    test_element_g2_is_well_formed<curve_type>();

    test_element_g1_precomp<curve_type>();

    test_element_g2_precomp<curve_type>();

    test_mnt_miller_loop<curve_type>();

    test_mnt_e_over_e_miller_loop<curve_type>();

    test_mnt_e_times_e_over_e_miller_loop<curve_type>();

    test_full_pairing<curve_type>();

    test_full_precomputed_pairing<curve_type>();

    test_verifier<curve_type,
        typename curve_type::pairing::pair_curve_type>();

    test_hardcoded_verifier<curve_type,
        typename curve_type::pairing::pair_curve_type>();
}

BOOST_AUTO_TEST_CASE(benes_components_mnt6_test) {

    std::cout << "Benes components test for mnt6-298 started" << std::endl;

    using curve_type = typename algebra::curves::mnt6<298>;
    using fq3_type = typename curve_type::template g2_type<>::field_type;
    using fq6_2over3_type = typename curve_type::gt_type;

    test_mul<fq3_type, element_fp3, element_fp3_mul>();
    test_sqr<fq3_type, element_fp3, element_fp3_squared>();

    test_mul<fq6_2over3_type, components::element_fp6_2over3, element_fp6_2over3_mul>();
    test_sqr<fq6_2over3_type, components::element_fp6_2over3, element_fp6_2over3_squared>();
    test_cyclotomic_sqr<curve_type, components::element_fp6_2over3, element_fp6_2over3_cyclotomic_squared>();
    test_exponentiation_component<fq6_2over3_type, components::element_fp6_2over3, element_fp6_2over3_mul, element_fp6_2over3_squared,
                                  algebra::mnt6_q_limbs>(curve_type::pairing::final_exponent_last_chunk_abs_of_w0);
    test_Frobenius<fq6_2over3_type, components::element_fp6_2over3>();

    test_element_g2_is_well_formed<curve_type>();

    test_element_g1_precomp<curve_type>();

    test_element_g2_precomp<curve_type>();

    test_mnt_miller_loop<curve_type>();

    test_mnt_e_over_e_miller_loop<curve_type>();

    test_mnt_e_times_e_over_e_miller_loop<curve_type>();

    test_full_pairing<curve_type>();

    test_full_precomputed_pairing<curve_type>();

    test_verifier<curve_type,
        typename curve_type::pairing::pair_curve_type>();

    test_hardcoded_verifier<curve_type,
        typename curve_type::pairing::pair_curve_type>();
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_PYTHON_STATIC_LIB
#include <string>
#include <iostream>
#include <vector>
#include <new>
#include <algorithm>
#include <boost/python.hpp>

#include <boost/foreach.hpp>
#include <boost/range/value_type.hpp>

#include "PQP.h"

#define PQP_SEQSIZE_ERROR (-6) // A value smaller than any other error codes

typedef std::vector<PQP_REAL> real_vector;
enum contact_type { all = PQP_ALL_CONTACTS, first = PQP_FIRST_CONTACT };

class PQP_Model2 : public PQP_Model {
public:
  int AddTri(const real_vector p1, const real_vector p2, const real_vector p3, int id) {
    if (p1.size() == 3 && p2.size() == 3 && p3.size() == 3) {
      PQP_REAL q1[3];
      q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
      PQP_REAL q2[3];
      q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
      PQP_REAL q3[3];
      q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
      PQP_Model::AddTri(q1, q2, q3, id);
    } else {
      std::cerr << "the number of floats is not three" << std::endl;
      return PQP_SEQSIZE_ERROR;
    }

    return PQP_OK;
  }

  real_vector const& get_list() const {
    return v_;
  }

  void add(real_vector const& that) {
    std::copy(that.begin(), that.end(), std::back_inserter(v_));
  }

private:
  real_vector v_;

};

template<typename T_>
class vector_to_pylist_converter {
public:
  typedef T_ native_type;

  static PyObject* convert(native_type const& v) {
    namespace py = boost::python;
    py::list retval;
    BOOST_FOREACH(typename boost::range_value<native_type>::type i, v)
      {
	retval.append(py::object(i));
      }
    return py::incref(retval.ptr());
  }
};

template<typename T_>
class pylist_to_vector_converter {
public:
  typedef T_ native_type;

  static void* convertible(PyObject* pyo) {
    if (!PySequence_Check(pyo))
      return 0;

    return pyo;
  }

  static void construct(PyObject* pyo, boost::python::converter::rvalue_from_python_stage1_data* data) {
    namespace py = boost::python;
    native_type* storage =
      new(reinterpret_cast<py::converter::rvalue_from_python_storage<native_type>*>(data)->storage.bytes) native_type();
    for (py::ssize_t i = 0, l = PySequence_Size(pyo); i < l; ++i) {
      storage->push_back(
			 py::extract<typename boost::range_value<native_type>::type>(
										     PySequence_GetItem(pyo, i)));
    }
    data->convertible = storage;
  }
};

int PQP_Collide2(PQP_CollideResult *result,
		 real_vector R1, real_vector T1, PQP_Model2 *o1,
		 real_vector R2, real_vector T2, PQP_Model2 *o2, enum contact_type ctype) {
  if (R1.size() != 9) return PQP_SEQSIZE_ERROR;
  if (T1.size() != 3) return PQP_SEQSIZE_ERROR;
  if (R2.size() != 9) return PQP_SEQSIZE_ERROR;
  if (T2.size() != 3) return PQP_SEQSIZE_ERROR;

  PQP_REAL R1_[3][3];
  PQP_REAL T1_[3];
  PQP_REAL R2_[3][3];
  PQP_REAL T2_[3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R1_[i][j] = R1[i*3+j];
    }
  }
  for (int i = 0; i < 3; i++) { T1_[i] = T1[i]; }
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R2_[i][j] = R2[i*3+j];
    }
  }
  for (int i = 0; i < 3; i++) { T2_[i] = T2[i]; }

  return PQP_Collide(result, R1_, T1_, o1, R2_, T2_, o2, ctype);
}

int PQP_Distance2(PQP_DistanceResult *result,
		  real_vector R1, real_vector T1, PQP_Model2 *o1,
		  real_vector R2, real_vector T2, PQP_Model2 *o2,
		  PQP_REAL rel_err, PQP_REAL abs_err) {
  if (R1.size() != 9) return PQP_SEQSIZE_ERROR;
  if (T1.size() != 3) return PQP_SEQSIZE_ERROR;
  if (R2.size() != 9) return PQP_SEQSIZE_ERROR;
  if (T2.size() != 3) return PQP_SEQSIZE_ERROR;

  PQP_REAL R1_[3][3];
  PQP_REAL T1_[3];
  PQP_REAL R2_[3][3];
  PQP_REAL T2_[3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R1_[i][j] = R1[i*3+j];
    }
  }
  for (int i = 0; i < 3; i++) { T1_[i] = T1[i]; }
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R2_[i][j] = R2[i*3+j];
    }
  }
  for (int i = 0; i < 3; i++) { T2_[i] = T2[i]; }

  return PQP_Distance(result, R1_, T1_, o1, R2_, T2_, o2, rel_err, abs_err);
}

BOOST_PYTHON_MODULE( libcpqp )
{
  using namespace boost::python;

  to_python_converter<real_vector, vector_to_pylist_converter<real_vector> >();
  converter::registry::push_back(
				 &pylist_to_vector_converter<real_vector>::convertible,
				 &pylist_to_vector_converter<real_vector>::construct,
				 boost::python::type_id<real_vector>());

  class_<PQP_Model2>("PQP_Model")
    .def("BeginModel", &PQP_Model2::BeginModel)
    .def("AddTri", &PQP_Model2::AddTri)
    .def("EndModel", &PQP_Model2::EndModel)
    .def("MemUsage", &PQP_Model2::MemUsage)
    .def("get_list", &PQP_Model2::get_list, return_value_policy<copy_const_reference>())
    .def("add", (void(PQP_Model2::*)(real_vector const&))&PQP_Model2::add)
    ;

  class_<PQP_CollideResult>("PQP_CollideResult")
    .def("NumBVTests", &PQP_CollideResult::NumBVTests)
    .def("NumTriTests", &PQP_CollideResult::NumTriTests)
    .def("NumPairs", &PQP_CollideResult::NumPairs)
    .def("Id1", &PQP_CollideResult::Id1)
    .def("Id2", &PQP_CollideResult::Id2)
    ;

  class_<PQP_DistanceResult>("PQP_DistanceResult")
    .def("NumBVTests", &PQP_DistanceResult::NumBVTests)
    .def("NumTriTests", &PQP_DistanceResult::NumTriTests)
    .def("Distance", &PQP_DistanceResult::Distance)
    ;

  def("PQP_Collide", &PQP_Collide2);
  def("PQP_Distance", &PQP_Distance2);

  enum_<contact_type>("contact_type")
    .value("all", all)
    .value("first", first)
    ;
}


#include <boost/numeric/ublas/experimental/sparse_view.hpp>


/***************************************************************************
 *   Copyright (C) 2016 - 2017 by                                          *
 *      Tarek Taha, KURI  <tataha@tarektaha.com>                           *
 *                                                                         *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   51 Franklin Steet, Fifth Floor, Boston, MA  02111-1307, USA.          *
 ***************************************************************************/
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>

ros::Publisher planePub;
ros::Publisher wrenchPub;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2 cloud_filtered;
  // Convert to PCL data type
  pcl_conversions::toPCL(*input, *cloud);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 planeOutput,wrenchOutput;
  pcl_conversions::fromPCL(cloud_filtered, planeOutput);
  //planeOutput = *input;
  //wrenchOutput = *input;

  ROS_INFO("Received Data");

  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*input,pcl_pc2);

  pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(pcl_pc2,*inputCloud);

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
   // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud(inputCloud);
  seg.segment (*inliers, *coefficients);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  PointCloud::Ptr planePoints(new PointCloud);
  PointCloud::Ptr wrenchPoints(new PointCloud);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
  }
  else
  {
    std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " "<< coefficients->values[2] << " " << coefficients->values[3] << std::endl;
    std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
    // Extract the inliers
    extract.setInputCloud (inputCloud);
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (*planePoints);
    std::cerr << "PointCloud representing the planar component: " << planePoints->width * planePoints->height << " data points." << std::endl;

    pcl::toROSMsg(*planePoints, planeOutput);
    planeOutput.header.frame_id = "camera_rgb_optical_frame";
    planeOutput.header.stamp = ros::Time::now();

    //std::cerr << "depth: "<< planeOutput->fields << std::endl;
    //ROS_INFO(planeOutput.z())

    printf ("Cloud: width = %d, height = %d\n", planePoints->width, planePoints->height);
    BOOST_FOREACH (const pcl::PointXYZ& pt, planePoints->points)
    printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);

    planePub.publish(planeOutput);
    // Extract the outliers
    extract.setInputCloud (inputCloud);
    extract.setIndices (inliers);
    extract.setNegative (true);
    extract.filter (*wrenchPoints);
    std::cerr << "PointCloud representing the wrench component: " << wrenchPoints->width * wrenchPoints->height << " data points." << std::endl;

    pcl::toROSMsg(*wrenchPoints, wrenchOutput);
    planeOutput.header.frame_id = "camera_rgb_optical_frame";
    planeOutput.header.stamp = ros::Time::now();

    wrenchPub.publish(wrenchOutput);
    /*
    for (size_t i = 0; i < inliers->indices.size (); ++i)
    {
      std::cerr << inliers->indices[i] << "    " << inputCloud->points[inliers->indices[i]].x << " "<< inputCloud->points[inliers->indices[i]].y << " "<< inputCloud->points[inliers->indices[i]].z << std::endl;
    }
    */
  }

}

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

  // Initialize ROS
  ros::init (argc, argv, "kuri_wrench_detection");
  ros::NodeHandle nh;
  std::string topic = nh.resolveName("point_cloud");;
  // Create a ROS subscriber for the input point cloud
  // ros::Subscriber sub = nh.subscribe ("/camera/depth_registered/points", 1, cloud_cb);
  ros::Subscriber sub = nh.subscribe ("/kinect2/qhd/points", 1, cloud_cb);
  planePub      = nh.advertise<sensor_msgs::PointCloud2>("/plane_points", 1);
  wrenchPub     = nh.advertise<sensor_msgs::PointCloud2>("/wrench_points", 1);

  // Spin
  ros::spin ();
}


/* ************************************************************************
 * Copyright 2016 Advanced Micro Devices, Inc.
 * ************************************************************************ */

#include <boost/program_options.hpp>
#include <iostream>
#include <stdio.h>

#include "testing_getf2_getrf.hpp"
#include "testing_getf2_getrf_batched.hpp"
#include "testing_getf2_getrf_strided_batched.hpp"
#include "testing_geqr2_geqrf.hpp"
#include "testing_geqr2_geqrf_batched.hpp"
#include "testing_geqr2_geqrf_strided_batched.hpp"
#include "testing_gelq2_gelqf.hpp"
#include "testing_gelq2_gelqf_batched.hpp"
#include "testing_gelq2_gelqf_strided_batched.hpp"
#include "testing_getrs.hpp"
#include "testing_getrs_batched.hpp"
#include "testing_getrs_strided_batched.hpp"
#include "testing_potf2_potrf.hpp"
#include "testing_potf2_potrf_batched.hpp"
#include "testing_potf2_potrf_strided_batched.hpp"
#include "testing_larfg.hpp"
#include "testing_larf.hpp"
#include "testing_larft.hpp"
#include "testing_larfb.hpp"
#include "testing_lacgv.hpp"
#include "testing_laswp.hpp"
#include "testing_orgxr_ungxr.hpp"
#include "testing_ormxr_unmxr.hpp"
#include "testing_orglx_unglx.hpp"
#include "testing_ormlx_unmlx.hpp"
#include "testing_orgbr_ungbr.hpp"
#include "testing_ormbr_unmbr.hpp"
#include "utility.h"

namespace po = boost::program_options;

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

  //disable unit_check in client benchmark, it is only
  // used in gtest unit test
  argus.unit_check = 0;

  // enable timing check,otherwise no performance data collected
  argus.timing = 1;

  std::string function;
  char precision;

  rocblas_int device_id;
  vector<rocblas_int> range = {-1, -1, -1};

  po::options_description desc("rocsolver client command line options");
  desc.add_options()("help,h", "produces this help message")
      // clang-format off
//        ("range",
//         po::value<vector<rocblas_int>>(&range)->multitoken(),
//         "Range matrix size testing: BLAS-3 benchmarking only. Accept three positive integers. "
//         "Usage: "
//         "--range start end step"
//         ". e.g "
//         "--range 100 1000 200"
//         ". Diabled if not specified. If enabled, user specified m,n,k will be nullified")

        ("sizem,m",
         po::value<rocblas_int>(&argus.M)->default_value(1024),
         "Specific matrix size testing: the number of rows of a matrix.")

        ("sizen,n",
         po::value<rocblas_int>(&argus.N)->default_value(1024),
         "Specific matrix/vector/order size testing: the number of columns of a matrix,"
         "or the order of a system or transformation.")

        ("sizek,k",
         po::value<rocblas_int>(&argus.K)->default_value(1024),
         "Specific...  the number of columns in "
         "A & C  and rows in B.")

        ("k1",
         po::value<rocblas_int>(&argus.k1)->default_value(1),
         "First index for row interchange, used with laswp. ")

        ("k2",
         po::value<rocblas_int>(&argus.k2)->default_value(2),
         "Last index for row interchange, used with laswp. ")

        ("lda",
         po::value<rocblas_int>(&argus.lda)->default_value(1024),
         "Specific leading dimension of matrix A, is only applicable to "
         "BLAS-2 & BLAS-3: the number of rows.")

        ("ldb",
         po::value<rocblas_int>(&argus.ldb)->default_value(1024),
         "Specific leading dimension of matrix B, is only applicable to BLAS-2 & BLAS-3: the number "
         "of rows.")

        ("ldc",
         po::value<rocblas_int>(&argus.ldc)->default_value(1024),
         "Specific leading dimension of matrix C, is only applicable to BLAS-2 & "
         "BLAS-3: the number of rows.")

        ("ldv",
         po::value<rocblas_int>(&argus.ldv)->default_value(1024),
         "Specific leading dimension.")

        ("ldt",
         po::value<rocblas_int>(&argus.ldt)->default_value(1024),
         "Specific leading dimension.")

        ("bsa",
         po::value<rocblas_int>(&argus.bsa)->default_value(1024*1024),
         "Specific stride of strided_batched matrix A, is only applicable to strided batched"
         "BLAS-2 and BLAS-3: second dimension * leading dimension.")

        ("bsb",
         po::value<rocblas_int>(&argus.bsb)->default_value(1024*1024),
         "Specific stride of strided_batched matrix B, is only applicable to strided batched"
         "BLAS-2 and BLAS-3: second dimension * leading dimension.")

        ("bsc",
         po::value<rocblas_int>(&argus.bsc)->default_value(1024*1024),
         "Specific stride of strided_batched matrix B, is only applicable to strided batched"
         "BLAS-2 and BLAS-3: second dimension * leading dimension.")

        ("bsp",
         po::value<rocblas_int>(&argus.bsp)->default_value(1024),
         "Specific stride of batched pivots vector Ipiv, is only applicable to batched and strided_batched"
         "factorizations: min(first dimension, second dimension).")

        ("incx",
         po::value<rocblas_int>(&argus.incx)->default_value(1),
         "increment between values in x vector")

        ("incy",
         po::value<rocblas_int>(&argus.incy)->default_value(1),
         "increment between values in y vector")

        ("alpha",
          po::value<double>(&argus.alpha)->default_value(1.0), "specifies the scalar alpha")

        ("beta",
         po::value<double>(&argus.beta)->default_value(0.0), "specifies the scalar beta")

        ("function,f",
         po::value<std::string>(&function)->default_value("potf2"),
         "LAPACK function to test. Options: potf2, getf2, getrf, getrs")

        ("precision,r",
         po::value<char>(&precision)->default_value('s'), "Options: h,s,d,c,z")

        ("transposeA",
         po::value<char>(&argus.transA_option)->default_value('N'),
         "N = no transpose, T = transpose, C = conjugate transpose")

        ("transposeB",
         po::value<char>(&argus.transB_option)->default_value('N'),
         "N = no transpose, T = transpose, C = conjugate transpose")

        ("transposeH",
         po::value<char>(&argus.transH_option)->default_value('N'),
         "N = no transpose, T = transpose, C = conjugate transpose")

        ("side",
         po::value<char>(&argus.side_option)->default_value('L'),
         "L = left, R = right. Only applicable to certain routines")

        ("uplo",
         po::value<char>(&argus.uplo_option)->default_value('U'),
         "U = upper, L = lower. Only applicable to certain routines") // xsymv xsyrk xsyr2k xtrsm
                                                                     // xtrmm
        ("diag",
         po::value<char>(&argus.diag_option)->default_value('N'),
         "U = unit diagonal, N = non unit diagonal. Only applicable to certain routines") // xtrsm
                                                                                // xtrmm
        ("direct",
         po::value<char>(&argus.direct_option)->default_value('F'),
         "F = forward, B = backward. Only applicable to certain routines") // xtrsm

        ("storev",
         po::value<char>(&argus.storev)->default_value('C'),
         "C = column_wise, R = row_wise. Only applicable to certain routines") // xtrsm

        ("batch",
         po::value<rocblas_int>(&argus.batch_count)->default_value(1),
         "Number of matrices. Only applicable to batched routines") // xtrsm xtrmm xgemm

        ("verify,v",
         po::value<rocblas_int>(&argus.norm_check)->default_value(0),
         "Validate GPU results with CPU? 0 = No, 1 = Yes (default: No)")

        ("iters,i",
         po::value<rocblas_int>(&argus.iters)->default_value(10),
         "Iterations to run inside timing loop")

        ("device",
         po::value<rocblas_int>(&device_id)->default_value(0),
         "Set default device to be used for subsequent program runs");
  // clang-format on

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 0;
  }

  if (precision != 'h' && precision != 's' && precision != 'd' &&
      precision != 'c' && precision != 'z') {
    std::cerr << "Invalid value for --precision" << std::endl;
    return -1;
  }

  // Device Query
  rocblas_int device_count = query_device_property();

  if (device_count <= device_id) {
    printf("Error: invalid device ID. There may not be such device ID. Will "
           "exit \n");
    return -1;
  } else {
    set_device(device_id);
  }
  /* ============================================================================================
   */
 // if (argus.M < 0 || argus.N < 0 || argus.K < 0) {
 //   printf("Invalide matrix dimension\n");
 // }

  //argus.start = range[0];
  //argus.step = range[1];
  //argus.end = range[2];

  if (function == "potf2") {
    if (precision == 's')
      testing_potf2_potrf<float,float,0>(argus);
    else if (precision == 'd')
      testing_potf2_potrf<double,double,0>(argus);
    else if (precision == 'c')
      testing_potf2_potrf<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_potf2_potrf<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "potf2_batched") {
    if (precision == 's')
      testing_potf2_potrf_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_potf2_potrf_batched<double,double,0>(argus);
    else if (precision == 'c')
      testing_potf2_potrf_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_potf2_potrf_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "potf2_strided_batched") {
    if (precision == 's')
      testing_potf2_potrf_strided_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_potf2_potrf_strided_batched<double,double,0>(argus);
    else if (precision == 'c')
      testing_potf2_potrf_strided_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_potf2_potrf_strided_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "potrf") {
    if (precision == 's')
      testing_potf2_potrf<float,float,1>(argus);
    else if (precision == 'd')
      testing_potf2_potrf<double,double,1>(argus);
    else if (precision == 'c')
      testing_potf2_potrf<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_potf2_potrf<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "potrf_batched") {
    if (precision == 's')
      testing_potf2_potrf_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_potf2_potrf_batched<double,double,1>(argus);
    else if (precision == 'c')
      testing_potf2_potrf_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_potf2_potrf_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "potrf_strided_batched") {
    if (precision == 's')
      testing_potf2_potrf_strided_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_potf2_potrf_strided_batched<double,double,1>(argus);
    else if (precision == 'c')
      testing_potf2_potrf_strided_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_potf2_potrf_strided_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "lacgv") {
    if (precision == 'c')
      testing_lacgv<rocblas_float_complex>(argus);
    else if (precision == 'z')
      testing_lacgv<rocblas_double_complex>(argus);
  }
  else if (function == "laswp") {
    if (precision == 's')
      testing_laswp<float>(argus);
    else if (precision == 'd')
      testing_laswp<double>(argus);
    else if (precision == 'c')
      testing_laswp<rocblas_float_complex>(argus);
    else if (precision == 'z')
      testing_laswp<rocblas_double_complex>(argus);
  }
  else if (function == "getf2") {
    if (precision == 's')
      testing_getf2_getrf<float,float,0>(argus);
    else if (precision == 'd')
      testing_getf2_getrf<double,double,0>(argus);
    if (precision == 'c')
      testing_getf2_getrf<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_getf2_getrf<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "getf2_batched") {
    if (precision == 's')
      testing_getf2_getrf_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_getf2_getrf_batched<double,double,0>(argus);
    if (precision == 'c')
      testing_getf2_getrf_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_getf2_getrf_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "getf2_strided_batched") {
    if (precision == 's')
      testing_getf2_getrf_strided_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_getf2_getrf_strided_batched<double,double,0>(argus);
    if (precision == 'c')
      testing_getf2_getrf_strided_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_getf2_getrf_strided_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "getrf") {
    if (precision == 's')
      testing_getf2_getrf<float,float,1>(argus);
    else if (precision == 'd')
      testing_getf2_getrf<double,double,1>(argus);
    if (precision == 'c')
      testing_getf2_getrf<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_getf2_getrf<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "getrf_batched") {
    if (precision == 's')
      testing_getf2_getrf_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_getf2_getrf_batched<double,double,1>(argus);
    if (precision == 'c')
      testing_getf2_getrf_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_getf2_getrf_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "getrf_strided_batched") {
    if (precision == 's')
      testing_getf2_getrf_strided_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_getf2_getrf_strided_batched<double,double,1>(argus);
    if (precision == 'c')
      testing_getf2_getrf_strided_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_getf2_getrf_strided_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "geqr2") {
    if (precision == 's')
      testing_geqr2_geqrf<float,float,0>(argus);
    else if (precision == 'd')
      testing_geqr2_geqrf<double,double,0>(argus);
    if (precision == 'c')
      testing_geqr2_geqrf<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_geqr2_geqrf<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "geqr2_batched") {
    if (precision == 's')
      testing_geqr2_geqrf_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_geqr2_geqrf_batched<double,double,0>(argus);
    if (precision == 'c')
      testing_geqr2_geqrf_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_geqr2_geqrf_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "geqr2_strided_batched") {
    if (precision == 's')
      testing_geqr2_geqrf_strided_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_geqr2_geqrf_strided_batched<double,double,0>(argus);
    if (precision == 'c')
      testing_geqr2_geqrf_strided_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_geqr2_geqrf_strided_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "geqrf") {
    if (precision == 's')
      testing_geqr2_geqrf<float,float,1>(argus);
    else if (precision == 'd')
      testing_geqr2_geqrf<double,double,1>(argus);
    if (precision == 'c')
      testing_geqr2_geqrf<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_geqr2_geqrf<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "geqrf_batched") {
    if (precision == 's')
      testing_geqr2_geqrf_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_geqr2_geqrf_batched<double,double,1>(argus);
    if (precision == 'c')
      testing_geqr2_geqrf_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_geqr2_geqrf_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "geqrf_strided_batched") {
    if (precision == 's')
      testing_geqr2_geqrf_strided_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_geqr2_geqrf_strided_batched<double,double,1>(argus);
    if (precision == 'c')
      testing_geqr2_geqrf_strided_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_geqr2_geqrf_strided_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "gelq2") {
    if (precision == 's')
      testing_gelq2_gelqf<float,float,0>(argus);
    else if (precision == 'd')
      testing_gelq2_gelqf<double,double,0>(argus);
    if (precision == 'c')
      testing_gelq2_gelqf<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_gelq2_gelqf<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "gelq2_batched") {
    if (precision == 's')
      testing_gelq2_gelqf_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_gelq2_gelqf_batched<double,double,0>(argus);
    if (precision == 'c')
      testing_gelq2_gelqf_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_gelq2_gelqf_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "gelq2_strided_batched") {
    if (precision == 's')
      testing_gelq2_gelqf_strided_batched<float,float,0>(argus);
    else if (precision == 'd')
      testing_gelq2_gelqf_strided_batched<double,double,0>(argus);
    if (precision == 'c')
      testing_gelq2_gelqf_strided_batched<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_gelq2_gelqf_strided_batched<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "gelqf") {
    if (precision == 's')
      testing_gelq2_gelqf<float,float,1>(argus);
    else if (precision == 'd')
      testing_gelq2_gelqf<double,double,1>(argus);
    if (precision == 'c')
      testing_gelq2_gelqf<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_gelq2_gelqf<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "gelqf_batched") {
    if (precision == 's')
      testing_gelq2_gelqf_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_gelq2_gelqf_batched<double,double,1>(argus);
    if (precision == 'c')
      testing_gelq2_gelqf_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_gelq2_gelqf_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "gelqf_strided_batched") {
    if (precision == 's')
      testing_gelq2_gelqf_strided_batched<float,float,1>(argus);
    else if (precision == 'd')
      testing_gelq2_gelqf_strided_batched<double,double,1>(argus);
    if (precision == 'c')
      testing_gelq2_gelqf_strided_batched<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_gelq2_gelqf_strided_batched<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "getrs") {
    if (precision == 's')
      testing_getrs<float,float>(argus);
    else if (precision == 'd')
      testing_getrs<double,double>(argus);
    if (precision == 'c')
      testing_getrs<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_getrs<rocblas_double_complex,double>(argus);
  }
  else if (function == "getrs_batched") {
    if (precision == 's')
      testing_getrs_batched<float,float>(argus);
    else if (precision == 'd')
      testing_getrs_batched<double,double>(argus);
    if (precision == 'c')
      testing_getrs_batched<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_getrs_batched<rocblas_double_complex,double>(argus);
  }
  else if (function == "getrs_strided_batched") {
    if (precision == 's')
      testing_getrs_strided_batched<float,float>(argus);
    else if (precision == 'd')
      testing_getrs_strided_batched<double,double>(argus);
    if (precision == 'c')
      testing_getrs_strided_batched<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_getrs_strided_batched<rocblas_double_complex,double>(argus);
  }
  else if (function == "larfg") {
    if (precision == 's')
      testing_larfg<float,float>(argus);
    else if (precision == 'd')
      testing_larfg<double,double>(argus);
    if (precision == 'c')
      testing_larfg<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_larfg<rocblas_double_complex,double>(argus);
  }
  else if (function == "larf") {
    if (precision == 's')
      testing_larf<float,float>(argus);
    else if (precision == 'd')
      testing_larf<double,double>(argus);
    if (precision == 'c')
      testing_larf<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_larf<rocblas_double_complex,double>(argus);
  }
  else if (function == "larft") {
    if (precision == 's')
      testing_larft<float,float>(argus);
    else if (precision == 'd')
      testing_larft<double,double>(argus);
    if (precision == 'c')
      testing_larft<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_larft<rocblas_double_complex,double>(argus);
  }
  else if (function == "larfb") {
    if (precision == 's')
      testing_larfb<float,float>(argus);
    else if (precision == 'd')
      testing_larfb<double,double>(argus);
    if (precision == 'c')
      testing_larfb<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_larfb<rocblas_double_complex,double>(argus);
  }
  else if (function == "org2r") {
    if (precision == 's')
      testing_orgxr_ungxr<float,float,0>(argus);
    else if (precision == 'd')
      testing_orgxr_ungxr<double,double,0>(argus);
  }
  else if (function == "ung2r") {
    if (precision == 'c')
      testing_orgxr_ungxr<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_orgxr_ungxr<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "orgqr") {
    if (precision == 's')
      testing_orgxr_ungxr<float,float,1>(argus);
    else if (precision == 'd')
      testing_orgxr_ungxr<double,double,1>(argus);
  }
  else if (function == "ungqr") {
    if (precision == 'c')
      testing_orgxr_ungxr<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_orgxr_ungxr<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "orm2r") {
    if (precision == 's')
      testing_ormxr_unmxr<float,float,0>(argus);
    else if (precision == 'd')
      testing_ormxr_unmxr<double,double,0>(argus);
  }
  else if (function == "unm2r") {
    if (precision == 'c')
      testing_ormxr_unmxr<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_ormxr_unmxr<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "ormqr") {
    if (precision == 's')
      testing_ormxr_unmxr<float,float,1>(argus);
    else if (precision == 'd')
      testing_ormxr_unmxr<double,double,1>(argus);
  }
  else if (function == "unmqr") {
    if (precision == 'c')
      testing_ormxr_unmxr<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_ormxr_unmxr<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "orml2") {
    if (precision == 's')
      testing_ormlx_unmlx<float,float,0>(argus);
    else if (precision == 'd')
      testing_ormlx_unmlx<double,double,0>(argus);
  }
  else if (function == "unml2") {
    if (precision == 'c')
      testing_ormlx_unmlx<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_ormlx_unmlx<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "ormlq") {
    if (precision == 's')
      testing_ormlx_unmlx<float,float,1>(argus);
    else if (precision == 'd')
      testing_ormlx_unmlx<double,double,1>(argus);
  }
  else if (function == "unmlq") {
    if (precision == 'c')
      testing_ormlx_unmlx<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_ormlx_unmlx<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "orgl2") {
    if (precision == 's')
      testing_orglx_unglx<float,float,0>(argus);
    else if (precision == 'd')
      testing_orglx_unglx<double,double,0>(argus);
  }
  else if (function == "ungl2") {
    if (precision == 'c')
      testing_orglx_unglx<rocblas_float_complex,float,0>(argus);
    else if (precision == 'z')
      testing_orglx_unglx<rocblas_double_complex,double,0>(argus);
  }
  else if (function == "orglq") {
    if (precision == 's')
      testing_orglx_unglx<float,float,1>(argus);
    else if (precision == 'd')
      testing_orglx_unglx<double,double,1>(argus);
  }
  else if (function == "unglq") {
    if (precision == 'c')
      testing_orglx_unglx<rocblas_float_complex,float,1>(argus);
    else if (precision == 'z')
      testing_orglx_unglx<rocblas_double_complex,double,1>(argus);
  }
  else if (function == "orgbr") {
    if (precision == 's')
      testing_orgbr_ungbr<float,float>(argus);
    else if (precision == 'd')
      testing_orgbr_ungbr<double,double>(argus);
  }
  else if (function == "ungbr") {
    if (precision == 'c')
      testing_orgbr_ungbr<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_orgbr_ungbr<rocblas_double_complex,double>(argus);
  }
  else if (function == "ormbr") {
    if (precision == 's')
      testing_ormbr_unmbr<float,float>(argus);
    else if (precision == 'd')
      testing_ormbr_unmbr<double,double>(argus);
  }
  else if (function == "unmbr") {
    if (precision == 'c')
      testing_ormbr_unmbr<rocblas_float_complex,float>(argus);
    else if (precision == 'z')
      testing_ormbr_unmbr<rocblas_double_complex,double>(argus);
  }
  else {
    printf("Invalid value for --function \n");
    return -1;
  }

  return 0;
}


/*! \file tdxscene.cpp
    \brief TDXSceneクラスの実装
    Copyright ©  2015 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/

#include "DXUT.h"
#include "DXUTmisc.h"
#include "myrandom/myrand.h"
#include "resource.h"
#include "TDXScene.h"
#include <mutex>                                                // for std::mutex
#include <boost/assert.hpp>                                     // for BOOST_ASSERT
#include <boost/cast.hpp>                                       // for boost::numeric_cast
#include <boost/math/special_functions/spherical_harmonic.hpp>  // for boost::math::spherical_harmonic
#include <boost/range/algorithm.hpp>                            // for boost::fill
#include <tbb/parallel_for.h>                                   // for tbb::parallel_for
#include <tbb/partitioner.h>                                    // for tbb::auto_partitioner

namespace tdxscene {
	float const TDXScene::MAGNIFICATION = 1.2f;

	TDXScene::TDXScene(std::shared_ptr<getdata::GetData> const & pgd) :
		Complete([this]{ return complete_.load(); }, nullptr),
		Pth([this]{ return std::cref(pth_); }, nullptr),
		Pgd(nullptr, [this](std::shared_ptr<getdata::GetData> const & val) {
			rmax_ = GetRmax(val);
			SetCamera();
			return pgd_ = val;
		}),
		PInputLayout([this]{ return std::cref(pInputLayout_); }, nullptr),
		Redraw(nullptr, [this](bool redraw){ return redraw_ = redraw; }),
		Thread_end(nullptr, [this](bool thread_end){
			thread_end_.store(thread_end);
			return thread_end; }),
		Vertexsize([this]{ return vertexsize_.load(); }, [this](std::vector<SimpleVertex2>::size_type size) {
				vertexsize_.store(size);
				return size; }),
		projectionVariable_(nullptr),
		pgd_(pgd),
		rmax_(GetRmax(pgd)),
		technique_(nullptr),
		vertices_(VERTEXSIZE_FIRST),
		viewVariable_(nullptr),
		worldVariable_(nullptr)
	{
	}

	HRESULT TDXScene::Init(ID3D10Device* pd3dDevice)
	{
		// Read the D3DX effect_ file
		auto dwShaderFlags = D3D10_SHADER_ENABLE_STRICTNESS;
#if defined( DEBUG ) || defined( _DEBUG )
		// Set the D3D10_SHADER_DEBUG flag to embed debug information in the shaders.
		// Setting this flag improves the shader debugging experience, but still allows
		// the shaders to be optimized and to run exactly the way they will run in
		// the release configuration of this program.
		dwShaderFlags |= D3D10_SHADER_DEBUG;
#endif

		ID3D10Effect * effect_tmp;

		auto hr = D3DX10CreateEffectFromFile(
			L"SchracVisualize.fx",
			nullptr,
			nullptr,
			"fx_4_0",
			dwShaderFlags,
			0,
			pd3dDevice,
			nullptr,
			nullptr,
			&effect_tmp,
			nullptr,
			nullptr);

		effect_.reset(effect_tmp);

		if (FAILED(hr))
		{
			::MessageBox(nullptr,
				L"The FX file cannot be located.  Please run this executable from the directory that contains the FX file.",
				L"Error",
				MB_OK);
			if (!utility::v_return(hr)) {
				return S_FALSE;
			}
		}

		technique_ = effect_->GetTechniqueByName("Render2");
		worldVariable_ = effect_->GetVariableByName("World")->AsMatrix();
		viewVariable_ = effect_->GetVariableByName("View")->AsMatrix();
		projectionVariable_ = effect_->GetVariableByName("Projection")->AsMatrix();

		// Define the input layout
		D3D10_INPUT_ELEMENT_DESC layout[] =
		{
			{ "POSITION", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, 0, D3D10_INPUT_PER_VERTEX_DATA, 0 },
			{ "COLOR", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, 12, D3D10_INPUT_PER_VERTEX_DATA, 0 },
		};

		auto const numElements = sizeof(layout) / sizeof(layout[0]);

		// Create the input layout
		D3D10_PASS_DESC PassDesc;
		technique_->GetPassByIndex(0)->GetDesc(&PassDesc);
		ID3D10InputLayout * pVertexLayout;

		if (!utility::v_return(
			pd3dDevice->CreateInputLayout(
			layout,
			numElements,
			PassDesc.pIAInputSignature,
			PassDesc.IAInputSignatureSize,
			&pVertexLayout))) {
			return S_FALSE;
		}

		pInputLayout_.reset(pVertexLayout, utility::Safe_Release<ID3D10InputLayout>());

		// Set the input layout
		pd3dDevice->IASetInputLayout(pInputLayout_.get());

		bd_.Usage = D3D10_USAGE_DEFAULT;
		bd_.BindFlags = D3D10_BIND_VERTEX_BUFFER;
		bd_.CPUAccessFlags = 0;
		bd_.MiscFlags = 0;

		// Initialize the world matrices
		D3DXMatrixIdentity(&world_);

		// Initialize the view matrix
		SetCamera();

		return S_OK;
	}

	LRESULT TDXScene::MsgPrc(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
	{
		return camera_.HandleMessages(hWnd, uMsg, wParam, lParam);
	}

	HRESULT TDXScene::OnFrameMove(double fTime, float fElapsedTime, void* pUserContext)
	{
		// Update the camera_'s position based on user input
		camera_.FrameMove(fElapsedTime);

		return S_OK;
	}

	HRESULT TDXScene::OnRender(ID3D10Device* pd3dDevice, double fTime, float fElapsedTime, void* pUserContext)
	{
		//
		// Clear the back buffer
		//
		auto const ClearColor = { 0.0f, 0.0f, 0.0f, 0.0f }; // red, green, blue, alpha
		auto pRTV = DXUTGetD3D10RenderTargetView();
		pd3dDevice->ClearRenderTargetView(pRTV, ClearColor.begin());

		//
		// Clear the depth stencil
		//
		auto pDSV = DXUTGetD3D10DepthStencilView();
		pd3dDevice->ClearDepthStencilView(pDSV, D3D10_CLEAR_DEPTH, 1.0, 0);

		//
		// Update variables that change once per frame
		//
		auto matcamera = camera_.GetProjMatrix();
		projectionVariable_->SetMatrix(reinterpret_cast<float *>(const_cast<D3DXMATRIX *>(matcamera)));
		auto matview = camera_.GetViewMatrix();
		viewVariable_->SetMatrix(reinterpret_cast<float *>(const_cast<D3DXMATRIX *>(matview)));

		worldVariable_->SetMatrix(reinterpret_cast<float *>(&world_));

		//
		// Render the cube
		//
		D3D10_TECHNIQUE_DESC techDesc;
		technique_->GetDesc(&techDesc);
		for (auto p = 0U; p < techDesc.Passes; ++p)
		{
			technique_->GetPassByIndex(p)->Apply(0);
			pd3dDevice->Draw(vertexsize_, 0);
		}

		return S_OK;
	}

	HRESULT TDXScene::OnResize(ID3D10Device* pd3dDevice, IDXGISwapChain* pSwapChain,
		const DXGI_SURFACE_DESC* pBackBufferSurfaceDesc, void* pUserContext)
	{
		// Setup the projection parameters again
		auto const fAspect = static_cast<float>(pBackBufferSurfaceDesc->Width) / static_cast<float>(pBackBufferSurfaceDesc->Height);

		camera_.SetProjParams(D3DX_PI / 4, fAspect, 0.1f, 100.0f);
		camera_.SetWindow(pBackBufferSurfaceDesc->Width, pBackBufferSurfaceDesc->Height);
		camera_.SetButtonMasks(MOUSE_MIDDLE_BUTTON, MOUSE_WHEEL, MOUSE_LEFT_BUTTON);

		return S_OK;
	}

	HRESULT TDXScene::RedrawFunc(std::int32_t m, ID3D10Device * pd3dDevice, TDXScene::Re_Im_type reim)
	{
		if (redraw_) {
			if (vertices_.size() != vertexsize_) {
				vertices_.resize(vertexsize_);
			}

			pth_.reset(new std::thread([this, m, reim]{ ClearFillSimpleVertex2(m, reim); }), [this](std::thread * pth)
			{
				if (pth->joinable()) {
					thread_end_.store(true);
					pth->join();
				}

				utility::Safe_Delete<std::thread> sd;
				sd(pth);
			});
			redraw_ = false;
		}

		bd_.ByteWidth = sizeof(SimpleVertex2) * vertexsize_;

		static D3D10_SUBRESOURCE_DATA InitData;
		InitData.pSysMem = vertices_.data();

		ID3D10Buffer * vertexBuffertmp;
		if (!utility::v_return(pd3dDevice->CreateBuffer(&bd_, &InitData, &vertexBuffertmp))) {
			return S_FALSE;
		}

		// Set vertex buffer
		static auto const stride = static_cast<UINT>(sizeof(SimpleVertex2));
		static auto const offset = 0U;
		pd3dDevice->IASetVertexBuffers(0, 1, &vertexBuffertmp, &stride, &offset);
		pVertexBuffer_.reset(vertexBuffertmp);

		return S_OK;
	}

	void TDXScene::ClearFillSimpleVertex2(std::int32_t m, TDXScene::Re_Im_type reim)
	{
		complete_.store(false);

		SimpleVertex2 sv2;
		sv2.Col = { 0.0f, 0.0f, 0.0f, 0.0f };
		sv2.Pos = { 0.0f, 0.0f, 0.0f };
		boost::fill(vertices_, sv2);

		tbb::parallel_for(
			0,
			boost::numeric_cast<std::int32_t>(vertexsize_.load()),
			1,
			[this, m, reim](std::int32_t i) { FillSimpleVertex2(m, reim, vertices_[i]); });

		complete_.store(true);
	}

	void TDXScene::FillSimpleVertex2(std::int32_t m, TDXScene::Re_Im_type reim, SimpleVertex2 & ver)
	{
		if (thread_end_) {
			return;
		}

		auto pp = 0.0, p = 0.0;
		auto sign = 0;
		double x, y, z;

		myrandom::MyRand mr(-rmax_, rmax_);
		myrandom::MyRand mr2(pgd_->Funcmin, pgd_->Funcmax);

		do {
			if (thread_end_) {
				return;
			}

			x = mr.myrand();
			y = mr.myrand();
			z = mr.myrand();

			auto const r = std::sqrt(x * x + y * y + z * z);
			if (r < pgd_->R_meshmin()) {
				continue;
			}

			switch (pgd_->Rho_wf_type_) {
			case getdata::GetData::Rho_Wf_type::RHO:
			{
				auto const phi = std::acos(x / std::sqrt(x * x + y * y));
                double v;
                if (m >= 0) {
                    v = boost::math::spherical_harmonic_r(pgd_->L, m, std::acos(z / r), phi);
                }
                else {
                    v = boost::math::spherical_harmonic_i(pgd_->L, m, std::acos(z / r), phi);
                }

                pp = ((*pgd_)(r) * v * v);
				p = mr2.myrand();
			}
			break;

			case getdata::GetData::Rho_Wf_type::WF:
			{
				auto const phi = std::acos(x / std::sqrt(x * x + y * y));
				double ylm = 0.0;
				switch (reim) {
				case TDXScene::Re_Im_type::REAL:
					ylm = boost::math::spherical_harmonic_r(pgd_->L, m, std::acos(z / r), phi);
					break;

				case TDXScene::Re_Im_type::IMAGINARY:
					ylm = boost::math::spherical_harmonic_i(pgd_->L, m, std::acos(z / r), phi);
					break;

				default:
					BOOST_ASSERT(!"何かがおかしい!");
					break;
				}

				pp = (*pgd_)(r) * ylm;
				p = mr2.myrand();
			}
			break;

			default:
				BOOST_ASSERT(!"何かがおかしい!");
				break;
			}

			switch (pgd_->Rho_wf_type_) {
			case getdata::GetData::Rho_Wf_type::RHO:
				sign = 1;
				break;

			case getdata::GetData::Rho_Wf_type::WF:
				sign = (pp > 0.0) - (pp < 0.0);
				break;

			default:
				BOOST_ASSERT(!"何かがおかしい!");
				break;
			}

			if (!m && pgd_->Rho_wf_type_ == getdata::GetData::Rho_Wf_type::WF && reim == TDXScene::Re_Im_type::IMAGINARY) {
				break;
			}

		} while (std::fabs(pp) < std::fabs(p));

		ver.Pos.x = static_cast<float>(x);
		ver.Pos.y = static_cast<float>(y);
		ver.Pos.z = static_cast<float>(z);

		ver.Col.r = sign > 0 ? 0.8f : 0.0f;
		ver.Col.b = 0.8f;
		ver.Col.g = sign < 0 ? 0.8f : 0.0f;
		ver.Col.a = 1.0f;
	}

	void TDXScene::SetCamera()
	{
		// Initialize the view matrix
		auto const pos = static_cast<float>(rmax_)* TDXScene::MAGNIFICATION;
		D3DXVECTOR3 Eye(0.0f, pos, -pos);
		D3DXVECTOR3 At(0.0f, 0.0f, 0.0f);
		camera_.SetViewParams(&Eye, &At);
	}

	double GetRmax(std::shared_ptr<getdata::GetData> const & pgd)
	{
		auto const n = static_cast<double>(pgd->N);
		return (2.3622 * n + 3.3340) * n + 1.3228;
	}
}


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Surface grid reading
// Read data from /IO/*.pts file which contain coordinates of points defining the corner of each panel
//
// I/O:
// - path: path to *.pts file
// - sGrid: temporary dynamic array to store panel vertices
// - bPan: body panel (structure)

#include <iostream>
#include <fstream>
#include <sstream>
#include <Eigen/Dense>
#include "read_sgrid.h"

#define NDIM 3

using namespace std;
using namespace Eigen;

void read_sgrid(string path, MatrixX3d &sGrid, Network &bPan){

    ifstream infile(path);
    string line;

    int i = 2;
    int a = 0;
    string section;
    bool secFLAG = 1;

    if(!infile.is_open())
    {
        cout << "File not found: " << path << endl;
        exit(EXIT_FAILURE);
    }
    else
        cout << "Found grid file: " << path << endl;

    getline(infile, line);
    cout << "Reading file... '" << line << "'" << endl;

    while (getline(infile, line))
    {

        if (secFLAG) {
            stringstream ss(line);
            ss >> section;
            secFLAG = 0;
        }

        else {

            if (section == "$size") {
                stringstream ss(line);
                ss >> bPan.nC >> bPan.nS;
                sGrid.resize(bPan.nC * bPan.nS, NDIM);
                secFLAG = 1;
            }

            else if (section == "$points") {
                stringstream ss(line);
                ss >> sGrid(a,0) >> sGrid(a,1) >> sGrid(a,2);
                a++;
            }

            else {
                cout << "Invalid section name: " << section << " at line " << i-1 << endl;
                exit(EXIT_FAILURE);
            }

        }
        i++;
    }

    // Set number of panels
    bPan.nC_ = bPan.nC - 1;
    bPan.nS_ = bPan.nS - 1;
    bPan.nP = bPan.nC_ * bPan.nS_;

    cout << "Done reading surface sGrid file!" << endl;
    cout << "Number of chordwise points: " << bPan.nC << endl;
    cout << "Number of spanwise points: " << bPan.nS << endl;
    cout << "Number of panels: " << bPan.nP << endl;
    #ifdef VERBOSE
        cout << "Surface points: " << sGrid.rows() << 'X' << sGrid.cols() << endl;
        for (int i = 0; i < bPan.nC*bPan.nS; ++i)
            cout << i << ' ' << sGrid(i,0) << ' ' << sGrid(i,1) << ' ' << sGrid(i,2) << endl;
    #endif
    cout << endl;
}


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include "db.h"
#include <iostream>
using namespace odb;

BOOST_AUTO_TEST_SUITE( test_suite )

BOOST_AUTO_TEST_CASE( test_oct )
{
    Oct* oct = new Oct;
    oct->init(Point(0,0),Point(400,400),40);
    BOOST_ASSERT(oct->getCenterHigh()==Point(400,400));
    BOOST_ASSERT(oct->getCenterLow()==Point(0,0));
    BOOST_ASSERT(oct->getWidth()==40);
    BOOST_ASSERT(oct->xMin()==-20);
    BOOST_ASSERT(oct->xMax()==420);
    BOOST_ASSERT(oct->yMin()==-20);
    BOOST_ASSERT(oct->yMax()==420);
    BOOST_ASSERT(oct->dx()==440);
    BOOST_ASSERT(oct->dy()==440);

    BOOST_ASSERT(oct->getDir()==Oct::OCT_DIR::RIGHT);
    oct->init(Point(0,0),Point(-400,400),40);
    BOOST_ASSERT(oct->getDir()==Oct::OCT_DIR::LEFT);
    oct->init(Point(0,0),Point(-400,-400),40);
    BOOST_ASSERT(oct->getDir()==Oct::OCT_DIR::RIGHT);
    oct->init(Point(0,0),Point(400,-400),40);
    BOOST_ASSERT(oct->getDir()==Oct::OCT_DIR::LEFT);
}
BOOST_AUTO_TEST_CASE( test_geom_shape )
{
    Oct oct(Point(0,0),Point(400,400),40);
    GeomShape* shape = &oct;
    BOOST_ASSERT(shape->xMin()==-20);
    BOOST_ASSERT(shape->xMax()==420);
    BOOST_ASSERT(shape->yMin()==-20);
    BOOST_ASSERT(shape->yMax()==420);
    BOOST_ASSERT(shape->dx()==440);
    BOOST_ASSERT(shape->dy()==440);
    //OCT POINTS
    std::vector<Point> points = shape->getPoints();
    BOOST_ASSERT(points.size()==9);
    BOOST_ASSERT(points[0]==Point(-9,-20));
    BOOST_ASSERT(points[1]==Point(9,-20));
    BOOST_ASSERT(points[2]==Point(420,391));
    BOOST_ASSERT(points[3]==Point(420,409));
    BOOST_ASSERT(points[4]==Point(409,420));
    BOOST_ASSERT(points[5]==Point(391,420));
    BOOST_ASSERT(points[6]==Point(-20,9));
    BOOST_ASSERT(points[7]==Point(-20,-9));
    BOOST_ASSERT(points[8]==Point(-9,-20));

    //RECT
    Rect rect(Point(0,0),Point(400,400));
    shape = ▭
    BOOST_ASSERT(shape->xMin()==0);
    BOOST_ASSERT(shape->xMax()==400);
    BOOST_ASSERT(shape->yMin()==0);
    BOOST_ASSERT(shape->yMax()==400);
    BOOST_ASSERT(shape->dx()==400);
    BOOST_ASSERT(shape->dy()==400);
    //RECT POINTS
    points = shape->getPoints();
    BOOST_ASSERT(points.size()==5);
    BOOST_ASSERT(points[0]==Point(0,0));
    BOOST_ASSERT(points[1]==Point(400,0));
    BOOST_ASSERT(points[2]==Point(400,400));
    BOOST_ASSERT(points[3]==Point(0,400));
    BOOST_ASSERT(points[4]==Point(0,0));

}
BOOST_AUTO_TEST_CASE( test_rect_merge )
{
    Rect rect(Point(0,0),Point(100,50));
    Oct oct(Point(100,50),Point(200,200),80);
    rect.merge((GeomShape*)&oct);
    BOOST_ASSERT(rect.xMin()==0);
    BOOST_ASSERT(rect.xMax()==240);
    BOOST_ASSERT(rect.yMin()==0);
    BOOST_ASSERT(rect.yMax()==240);
    BOOST_ASSERT(rect.dx()==240);
    BOOST_ASSERT(rect.dy()==240);
}
BOOST_AUTO_TEST_SUITE_END()


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2015, Rice University
*  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 Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Ryan Luna */

#define BOOST_TEST_MODULE "AdjacencyList"
#include <boost/test/unit_test.hpp>
#include <iostream>

#include "ompl/datastructures/AdjacencyList.h"

#define BOOST_OMPL_EXPECT_NEAR(a, b, diff) BOOST_CHECK_SMALL((a) - (b), diff)

// Simple graph construction.  No removal of elements
BOOST_AUTO_TEST_CASE(SimpleConstruction)
{
    ompl::AdjacencyList graph;
    BOOST_CHECK_EQUAL(graph.numVertices(), 0);
    BOOST_CHECK_EQUAL(graph.numEdges(), 0);

    // Creating 1000 vertices
    int numVertices = 1000;
    for (int i = 0; i < numVertices; ++i)
    {
        BOOST_CHECK(!graph.vertexExists(i));
        BOOST_CHECK_EQUAL(graph.addVertex(), i);
        BOOST_CHECK(graph.vertexExists(i));
        BOOST_CHECK_EQUAL(graph.numNeighbors(i), 0);
    }

    // We should have #states vertices and 0 edges
    BOOST_CHECK_EQUAL(graph.numVertices(), numVertices);
    BOOST_CHECK_EQUAL(graph.numEdges(), 0 );
    BOOST_CHECK_EQUAL(graph.numConnectedComponents(), numVertices);

    // Adding edges - making a ring topology
    for (int i = 0; i < numVertices; ++i)
    {
        int source = i;
        int target = (i+1) % numVertices;

        BOOST_CHECK(!graph.edgeExists(source, target));
        BOOST_CHECK(graph.addEdge(source, target, std::max(source, target)));
        BOOST_CHECK(graph.edgeExists(source, target));
        BOOST_CHECK(graph.edgeExists(target, source)); // undirected edges

        BOOST_CHECK_EQUAL(graph.numNeighbors(source), (source == 0 ? 1 : 2));

        double weight = graph.getEdgeWeight(source, target);
        BOOST_OMPL_EXPECT_NEAR(weight, std::max(source, target), 1e-6);

        weight = graph.getEdgeWeight(target, source);
        BOOST_OMPL_EXPECT_NEAR(weight, std::max(source, target), 1e-6);

        if (i != numVertices-1)
        {
            BOOST_CHECK_EQUAL(graph.numConnectedComponents(), numVertices - i - 1);
            for(int j = 0; j < target; ++j)
                BOOST_CHECK(graph.inSameComponent(i, j));
            for(int j = target+1; j < numVertices; ++j)
                BOOST_CHECK(!graph.inSameComponent(i, j));
        }
        else
            BOOST_CHECK_EQUAL(graph.numConnectedComponents(), 1);
    }

    // We should have #states vertices and 0 edges
    BOOST_CHECK_EQUAL(graph.numVertices(), numVertices);
    BOOST_CHECK_EQUAL(graph.numEdges(), numVertices);

    // Make sure the neighbors are who we think they are
    for (int i = 0; i < numVertices; ++i)
    {
        BOOST_CHECK_EQUAL(graph.numNeighbors(i), 2);

        std::vector<int> nbrs;
        graph.getNeighbors(i, nbrs);

        // make sure all neighbors are distinct
        for(size_t j = 0; j < nbrs.size(); ++j)
            for(size_t k = j+1; k < nbrs.size(); ++k)
                BOOST_REQUIRE(nbrs[j] != nbrs[k]);

        // Make sure the neighbors are correct
        int nbr1 = (i+1) % numVertices;
        int nbr2 = (i == 0 ? numVertices - 1 : i-1);
        BOOST_CHECK(nbrs[0] == nbr1 || nbrs[0] == nbr2);
        BOOST_CHECK(nbrs[1] == nbr1 || nbrs[1] == nbr2);
    }

    // Check edge weights
    for (int i = 0; i < numVertices; ++i)
    {
        BOOST_CHECK_EQUAL(graph.numNeighbors(i), 2);

        std::vector<std::pair<int, double> > nbrWeights;
        graph.getNeighbors(i, nbrWeights);
        BOOST_CHECK(nbrWeights.size() == 2);

        // make sure all neighbors are distinct
        for(size_t j = 0; j < nbrWeights.size(); ++j)
            for(size_t k = j+1; k < nbrWeights.size(); ++k)
                BOOST_REQUIRE(nbrWeights[j].first != nbrWeights[k].first);

        // Make sure the neighbors are correct
        int nbr1 = (i+1) % numVertices;
        int nbr2 = (i == 0 ? numVertices - 1 : i-1);
        BOOST_CHECK(nbrWeights[0].first == nbr1 || nbrWeights[0].first == nbr2);
        BOOST_CHECK(nbrWeights[1].first == nbr1 || nbrWeights[1].first == nbr2);

        // make sure weights are correct
        for(size_t j = 0; j < nbrWeights.size(); ++j)
        {
            double weight = graph.getEdgeWeight(i, nbrWeights[j].first);
            double expected = std::max(nbrWeights[j].first, i);
            BOOST_OMPL_EXPECT_NEAR(weight, expected, 1e-6);
        }
    }

    graph.clear();
    BOOST_CHECK_EQUAL(graph.numVertices(), 0);
    BOOST_CHECK_EQUAL(graph.numEdges(), 0);
    BOOST_CHECK_EQUAL(graph.numConnectedComponents(), 0);
}

BOOST_AUTO_TEST_CASE(AddModifyRemove)
{
    ompl::AdjacencyList graph;

    // Creating 1000 vertices
    int numVertices = 1000;
    for (int i = 0; i < numVertices; ++i)
        BOOST_CHECK_EQUAL(graph.addVertex(), i);

    // We should have #states vertices and 0 edges
    BOOST_CHECK_EQUAL(graph.numVertices(), numVertices);
    BOOST_CHECK_EQUAL(graph.numEdges(), 0 );

    // Adding edges - making a ring topolocy
    for (int i = 0; i < numVertices; ++i)
    {
        int source = i;
        int target = (i+1) % numVertices;
        BOOST_CHECK(graph.addEdge(source, target, std::max(source, target)));  // adding edge with weight i+1
    }

    // Add edges to 0 from all nodes
    for (int i = 0; i < numVertices; ++i)
    {
        int source = i;
        int target = 0;

        // Don't try to add an edge that exists
        bool exists = graph.edgeExists(source, target);
        if (exists)
        {
            // Make sure we expect this edge
            BOOST_CHECK(target == (i > 0 ? i - 1 : numVertices -1) ||
                        target == (i < numVertices - 1 ? i + 1 : 0));
        }
        else
        {
            if (source != target)
                BOOST_CHECK(graph.addEdge(source, target, i));  // adding edge with weight i
            else BOOST_CHECK(!graph.addEdge(source, target, i));  // NOT adding edge with weight i (self transitions not allowed)
            BOOST_CHECK(!graph.addEdge(source, target, i));  // NOT adding edge again with weight i
        }
    }

    // Check size of graph
    BOOST_CHECK_EQUAL(graph.numVertices(), numVertices);
    BOOST_CHECK_EQUAL(graph.numEdges(), numVertices + numVertices - 3);

    // Make sure the neighbors are who we think they are
    for (int i = 0; i < numVertices; ++i)
    {
        std::vector<int> nbrs;
        graph.getNeighbors(i, nbrs);
        BOOST_CHECK_EQUAL(nbrs.size(), graph.numNeighbors(i));

        // make sure all neighbors are distinct
        for(size_t j = 0; j < nbrs.size(); ++j)
            for(size_t k = j+1; k < nbrs.size(); ++k)
                BOOST_REQUIRE(nbrs[j] != nbrs[k]);

        // Make sure the neighbors are correct
        if (i == 0)
        {
            BOOST_CHECK_EQUAL(graph.numNeighbors(i), numVertices-1);
            for(size_t j = 0; j < nbrs.size(); ++j)
                BOOST_CHECK(nbrs[j] != 0);
        }
        else if (i == 1 || i == numVertices - 1)
        {
            BOOST_CHECK_EQUAL(graph.numNeighbors(i), 2);

            int nbr1 = (i == 1 ? 0 : i-1);
            int nbr2 = (i == 1 ? i+1 : 0);
            for(size_t j = 0; j < nbrs.size(); ++j)
                BOOST_CHECK(nbrs[j] == nbr1 || nbrs[j] == nbr2);
        }
        else
        {
            BOOST_CHECK_EQUAL(graph.numNeighbors(i), 3);

            int nbr1 = i+1;
            int nbr2 = i-1;
            int nbr3 = 0;
            for(size_t j = 0; j < nbrs.size(); ++j)
                BOOST_CHECK(nbrs[j] == nbr1 || nbrs[j] == nbr2 || nbrs[j] == nbr3);
        }
    }

    BOOST_CHECK_EQUAL(graph.numEdges(), numVertices + numVertices - 3);

    // Change edge weights
    for(int i = 0; i < numVertices; ++i)
    {
        for(int j = i+1; j < numVertices; ++j)
        {
            if (graph.edgeExists(i, j))
            {
                double expWeight = std::max(i, j);
                double weight = graph.getEdgeWeight(i, j);
                BOOST_OMPL_EXPECT_NEAR(weight, expWeight, 1e-6);

                double newWeight = std::min(i, j);
                graph.setEdgeWeight(i, j, newWeight);
                weight = graph.getEdgeWeight(i, j);
                BOOST_OMPL_EXPECT_NEAR(weight, newWeight, 1e-6);

            }
        }
    }

    int numEdges = graph.numEdges();

    // remove edges
    for(int i = 1; i < numVertices-1; ++i)
    {
        if (i == 1 || i == numVertices -1)
            BOOST_REQUIRE_EQUAL(graph.numNeighbors(i), 2);
        else
            BOOST_REQUIRE_EQUAL(graph.numNeighbors(i), 3);

        BOOST_REQUIRE(graph.edgeExists(0, i));
        BOOST_REQUIRE(graph.edgeExists(i, 0));

        BOOST_CHECK(graph.removeEdge(0, i));
        BOOST_REQUIRE(!graph.edgeExists(0, i));
        BOOST_REQUIRE(!graph.edgeExists(i, 0));
        BOOST_CHECK_EQUAL(graph.numEdges(), --numEdges);

        if (i == 1 || i == numVertices -1)
            BOOST_REQUIRE_EQUAL(graph.numNeighbors(i), 1);
        else
            BOOST_REQUIRE_EQUAL(graph.numNeighbors(i), 2);

        std::vector<int> nbrs;
        std::vector<std::pair<int, double> > nbrWeights;
        graph.getNeighbors(i, nbrs);
        graph.getNeighbors(i, nbrWeights);
        BOOST_CHECK_EQUAL(nbrs.size(), graph.numNeighbors(i));
        BOOST_CHECK_EQUAL(nbrWeights.size(), graph.numNeighbors(i));

        // make sure all neighbors are distinct
        for(size_t j = 0; j < nbrs.size(); ++j)
            for(size_t k = j+1; k < nbrs.size(); ++k)
                BOOST_REQUIRE(nbrs[j] != nbrs[k]);

        // make sure all neighbors are distinct
        for(size_t j = 0; j < nbrWeights.size(); ++j)
            for(size_t k = j+1; k < nbrWeights.size(); ++k)
                BOOST_REQUIRE(nbrWeights[j].first != nbrWeights[k].first);

        int nbr1 = i+1;
        int nbr2 = i-1;
        for(size_t j = 0; j < nbrs.size(); ++j)
            BOOST_CHECK(nbrs[j] == nbr1 || nbrs[j] == nbr2);
        for(size_t j = 0; j < nbrWeights.size(); ++j)
        {
            BOOST_CHECK(nbrWeights[j].first == nbr1 || nbrWeights[j].first == nbr2);
            BOOST_OMPL_EXPECT_NEAR(nbrWeights[j].second, std::min(i, nbrWeights[j].first), 1e-6);
        }
    }
}

BOOST_AUTO_TEST_CASE(Dijkstra)
{
    ompl::AdjacencyList graph;

    // Creating 1000 vertices
    int numVertices = 1000;
    for (int i = 0; i < numVertices; ++i)
        BOOST_CHECK_EQUAL(graph.addVertex(), i);

    // We should have #states vertices and 0 edges
    BOOST_CHECK_EQUAL(graph.numVertices(), numVertices);
    BOOST_CHECK_EQUAL(graph.numEdges(), 0 );

    // Adding edges - making a ring topolocy
    for (int i = 0; i < numVertices; ++i)
    {
        int source = i;
        int target = (i+1) % numVertices;
        BOOST_CHECK(graph.addEdge(source, target, 1));  // adding edge with weight 1
    }

    BOOST_CHECK_EQUAL(graph.numVertices(), numVertices);
    BOOST_CHECK_EQUAL(graph.numEdges(), numVertices);

    // Short hop around the ring
    std::vector<int> path;
    BOOST_CHECK(graph.dijkstra(0, numVertices-1, path));
    BOOST_REQUIRE(path.size() == 2);
    BOOST_CHECK(path[0] == 0);
    BOOST_CHECK(path[1] == numVertices - 1);

    // Remove the hop in the ring.  Path should go through all vertices in order
    BOOST_CHECK(graph.removeEdge(0, numVertices-1));
    BOOST_REQUIRE_EQUAL(graph.numEdges(), numVertices-1);
    BOOST_CHECK(graph.dijkstra(0, numVertices-1, path));
    BOOST_REQUIRE(path.size() == (unsigned int)numVertices);
    for(int i = 0; i < (int)path.size(); ++i)
        BOOST_CHECK_EQUAL(path[i], i);

    // Add the hop in the ring, but make it crazy high edge weight.  Path should go through all vertices in order
    BOOST_REQUIRE(graph.addEdge(0, numVertices-1, numVertices));
    BOOST_REQUIRE_EQUAL(graph.numEdges(), numVertices);
    BOOST_CHECK(graph.dijkstra(0, numVertices-1, path));
    BOOST_REQUIRE(path.size() == (unsigned int)numVertices);
    for(int i = 0; i < (int)path.size(); ++i)
        BOOST_CHECK_EQUAL(path[i], i);

    if (numVertices > 3)
    {
        BOOST_CHECK(graph.setEdgeWeight(0, numVertices - 1, 1.0));

        // Remove all neighbors of 3
        std::vector<int> nbrs;
        graph.getNeighbors(3, nbrs);
        for(size_t i = 0; i < nbrs.size(); ++i)
        {
            BOOST_CHECK(graph.edgeExists(3, nbrs[i]));
            BOOST_CHECK(graph.removeEdge(3, nbrs[i]));
            BOOST_CHECK(!graph.edgeExists(3, nbrs[i]));
            BOOST_CHECK(!graph.edgeExists(nbrs[i], 3));
        }
        BOOST_CHECK_EQUAL(graph.numNeighbors(3), 0);

        // There should be no path to vertex 3
        BOOST_CHECK(!graph.dijkstra(0, 3, path));
    }
}