/*
 * test_Indicator.cpp
 *
 *  Created on: 2013-4-11
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/indicator/Indicator.h>
#include <hikyuu/indicator/crt/PRICELIST.h>
#include <hikyuu/indicator/crt/KDATA.h>
#include <hikyuu/StockManager.h>

using namespace hku;

/**
 * @defgroup test_indicator_Indicator test_indicator_Indicator
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_add ) {
    /** @arg 正常相加*/
    PriceList d1, d2;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator result = data1 + data2;

    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == i + i + 1);
    }

    /** @arg 两个待加的ind的size不同，其中一个为0 */
    Indicator data3;
    result = data1 + data3;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个待加的ind的size不同，其中一个为0 */
    PriceList d3;
    for (size_t i = 0; i < 20; ++i) {
        d3.push_back(i);
    }
    data3 = PRICELIST(d3);
    result = data1 + data3;
    BOOST_CHECK(data1.size() == 10);
    BOOST_CHECK(data3.size() == 20);
    BOOST_CHECK(result.empty() == false);
    BOOST_CHECK(result.size() == 20);
    BOOST_CHECK(result.discard() == 10);
    for (size_t i = 0; i < result.discard(); ++i) {
        BOOST_CHECK(result[i] == Null<price_t>());
    }
    for (size_t i = result.discard(); i < 20; ++i) {
        BOOST_CHECK(result[i] == i + i - 10);
    }

    /** @arg 两个待加的ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = k + data1;
    BOOST_CHECK(result.size() == k.size());
    BOOST_CHECK(result.getResultNumber() == 1);
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == (k[i] + data1[i]));
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_reduce ) {
    /** @arg 正常相减*/
    PriceList d1, d2;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator result = data1 - data2;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == data1[i] - data2[i]);
    }

    /** @arg 两个待减的ind的size不同 */
    Indicator data3;
    result = data1 - data3;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个待减的ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = k - data1;
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == (k[i] - data1[i]));
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_multi ) {
    /** @arg 正常相乘*/
    PriceList d1, d2;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator result = data1 * data2;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == data1[i] * data2[i]);
    }

    /** @arg 两个待乘的ind的size不同 */
    Indicator data3;
    result = data1 * data3;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个待乘的ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = k * data1;
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == (k[i] * data1[i]));
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_division ) {
    /** @arg 正常相除*/
    PriceList d1, d2;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator result = data2 / data1;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        if (data1[i] == 0.0) {
            BOOST_CHECK(result[i] == Null<price_t>());
        } else {
            BOOST_CHECK(result[i] == data2[i] / data1[i]);
        }
    }

    /** @arg 两个待除的ind的size不同 */
    Indicator data3;
    result = data1 / data3;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个待除的ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = k / data1;
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        if (data1[i] == 0.0) {
            BOOST_CHECK(result[i] == Null<price_t>());
        } else {
            BOOST_CHECK(result[i] == (k[i] / data1[i]));
        }
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_eq ) {
    /** @arg 正常相等*/
    PriceList d1, d2;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator result = (data2 == data1);
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == true);
    }

    /** @arg 两个ind的size不同 */
    Indicator data3;
    result = (data1 == data3);
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = (k == data1);
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == false);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_ne ) {
    /** @arg 正常不相等 */
    PriceList d1, d2;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator result = (data2 != data1);
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == false);
    }

    /** @arg 两个ind的size不同  */
    Indicator data3;
    result = (data1 != data3);
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = (k != data1);
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == true);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_gt ) {
    PriceList d1, d2, d3;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i);
        d3.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator data3 = PRICELIST(d3);

    /** @arg ind1 > ind2*/
    Indicator result = (data3 > data1);
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg ind1 < ind2 */
    result = (data1 > data3);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg ind1 == ind2 */
    result = (data1 > data2);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg 两个ind的size不同 */
    Indicator data4;
    result = data1 > data4;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = (k > data1);
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_ge ) {
    PriceList d1, d2, d3;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i);
        d3.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator data3 = PRICELIST(d3);

    /** @arg ind1 > ind2*/
    Indicator result = (data3 >= data1);
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg ind1 < ind2 */
    result = (data1 >= data3);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg ind1 == ind2 */
    result = (data1 >= data2);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg 两个ind的size不同 */
    Indicator data4;
    result = data1 >= data4;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = (k >= data1);
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_lt ) {
    PriceList d1, d2, d3;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i);
        d3.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator data3 = PRICELIST(d3);

    /** @arg ind1 > ind2*/
    Indicator result = (data3 < data1);
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg ind1 < ind2 */
    result = (data1 < data3);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg ind1 == ind2 */
    result = (data1 < data2);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg 两个ind的size不同 */
    Indicator data4;
    result = data1 < data4;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = (k < data1);
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_operator_le ) {
    PriceList d1, d2, d3;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(i);
        d2.push_back(i);
        d3.push_back(i+1);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator data3 = PRICELIST(d3);

    /** @arg ind1 > ind2*/
    Indicator result = (data3 <= data1);
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg ind1 < ind2 */
    result = (data1 <= data3);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg ind1 == ind2 */
    result = (data1 <= data2);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg 两个ind的size不同 */
    Indicator data4;
    result = data1 <= data4;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);

    /** @arg 两个ind的size相同，但result_number不同 */
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query(0, 10);
    KData kdata = stock.getKData(query);
    Indicator k = KDATA(kdata);
    BOOST_CHECK(k.size() == data1.size());
    result = (k <= data1);
    BOOST_CHECK(result.size() == k.size());
    for (size_t i = 0; i < result.size(); ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_getResult_getResultAsPriceList ) {
    StockManager& sm = StockManager::instance();
    Stock stock = sm.getStock("sh600000");
    KQuery query;
    KData kdata;
    Indicator ikdata, result1;
    PriceList result2;

    /** @arg 源数据为空 */
    ikdata = KDATA(kdata);
    result1 = ikdata.getResult(0);
    result2 = ikdata.getResultAsPriceList(0);
    BOOST_CHECK(result1.size() == 0);
    BOOST_CHECK(result2.size() == 0);

    /** @arg result_num参数非法 */
    query = KQuery(0, 10);
    kdata = stock.getKData(query);
    ikdata = KDATA(kdata);
    BOOST_CHECK(ikdata.size() == 10);
    result1 = ikdata.getResult(6);
    result2 = ikdata.getResultAsPriceList(6);
    BOOST_CHECK(result1.size() == 0);
    BOOST_CHECK(result2.size() == 0);

    /** @arg 正常获取 */
    result1 = ikdata.getResult(0);
    result2 = ikdata.getResultAsPriceList(1);
    BOOST_CHECK(result1.size() == 10);
    BOOST_CHECK(result2.size() == 10);
    BOOST_CHECK(result1[0] == 29.5);
    BOOST_CHECK(std::fabs(result1[1] - 27.58) < 0.0001);
    BOOST_CHECK(result1[9] == 26.45);

    BOOST_CHECK(result2[0] == 29.8);
    BOOST_CHECK(result2[1] == 28.38);
    BOOST_CHECK(result2[9] == 26.55);
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_LOGIC_AND ) {
    PriceList d1, d2, d3;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(0);
        d2.push_back(1);
        d3.push_back(i);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator data3 = PRICELIST(d3);

    /** @arg ind1为全0， ind2为全1 */
    Indicator result = data1 & data2;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg ind为全0， val为1 */
    /*result = IND_AND(data1, 1.0);
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }*/

    /** @arg ind1为全0， ind2为从0开始的整数 */
    result = data1 & data3;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 0.0);
    }

    /** @arg ind1为全1， ind2为从0开始的整数 */
    result = data2 & data3;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result[0] == 0.0);
    for (size_t i = 1; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg 两个ind的size不同 */
    Indicator data4;
    result = data1 & data4;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_LOGIC_OR ) {
    PriceList d1, d2, d3;
    for (size_t i = 0; i < 10; ++i) {
        d1.push_back(0);
        d2.push_back(1);
        d3.push_back(i);
    }

    Indicator data1 = PRICELIST(d1);
    Indicator data2 = PRICELIST(d2);
    Indicator data3 = PRICELIST(d3);

    /** @arg ind1为全0， ind2为全1 */
    Indicator result = data1 | data2;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg ind1为全0， ind2为从0开始的整数 */
    result = data1 | data3;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result[0] == 0.0);
    for (size_t i = 1; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg ind1为全1， ind2为从0开始的整数 */
    result = data2 | data3;
    BOOST_CHECK(result.size() == 10);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result.discard() == 0);
    for (size_t i = 0; i < 10; ++i) {
        BOOST_CHECK(result[i] == 1.0);
    }

    /** @arg 两个ind的size不同 */
    Indicator data4;
    result = data1 | data4;
    BOOST_CHECK(result.empty());
    BOOST_CHECK(result.size() == 0);
}

/** @} */


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                        (C) 2008 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <boost/python.hpp>

#include "mccomponents/kernels/sample/AbstractSQ.h"
#include "mccomponents/kernels/sample/SQAdaptor.h"

extern "C" {
#include "sans/sphere.h"
}

namespace wrap_models{

  struct Sphere {

    Sphere( double scale, double radius, double contrast, double background)
    {
      p.scale = scale;
      p.radius = radius;
      p.contrast = contrast;
      p.background = background;
    }

    double operator() (double q) const
    {
      return sphere_analytical_1D( const_cast<SphereParameters *>(&p), q );
    }

    SphereParameters p;

  };

  void wrap_sphere()
  {
    using namespace boost::python;
    typedef Sphere w_t;
    class_<w_t>
      (
       "SANSModel_Sphere",
       init< double, double, double, double >()
	)
      ;

    using namespace mccomponents::sample;
    typedef SQAdaptor<w_t> SQ;
    class_<SQ, bases<AbstractSQ> >
      (
       "SANSModel_Sphere_SQAdaptor",
       init< const w_t & >()
       [with_custodian_and_ward<1,2>()]
       )
      ;
  }
}

// version
// $Id$

// End of file


/* ROS-CHOMP.
 *
 * Copyright (C) 2015 Jafar Qutteineh. All rights reserved.
 * License (3-Cluase BSD): https://github.com/j3sq/ROS-CHOMP/blob/master/LICENSE
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 *
 * **
 * \file chomp-shell.cpp
 *
 * An interactive GUI for chomp. Click and drag obstacless to move them around. Add more obstacles by right clicking.
 *
 */

#include "gfx.hpp"
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <stdlib.h>
#include <sys/time.h>
#include <err.h>
#include "chomp.hpp"

typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;

using namespace std;

//////////////////////////////////////////////////
// trajectory etc

static size_t const cdim(2);    // dimension of config space
static size_t const nq(20);     // number of q stacked into xi
static size_t const obs_dim(3); // x,y,R
Vector xi;                      // the trajectory (q_1, q_2, ...q_n)
Vector qs;                      // the start config a.k.a. q_0
Vector qe;                      // the end config a.k.a. q_(n+1)
Matrix obs;                     // Matrix containninging all obstacles, each column is (x,y,R) of the obstacle

void add_obs(double px, double py, double radius)
{
	//conservativeResize is used. It's a costy operation, but hopefully will not
	// be done too often.
	obs.conservativeResize(obs_dim, obs.cols() + 1);
	obs.block(0, obs.cols() - 1, obs_dim, 1) << px, py, radius;
}
static enum { PAUSE, STEP, RUN } state;

static int grabbed(-1);
static Vector grab_offset(3);

//////////////////////////////////////////////////
// robot (one per waypoint)

class Robot
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Robot ()
	: position_(Vector::Zero(2))
{
}

void update(Vector const & position)
{
	if (position.size() != 2)
		errx(EXIT_FAILURE, "Robot::update(): position has %zu DOF (but needs 2)",
		     (size_t)position.size());
	position_ = position;
}

void draw() const
{
	// translucent disk for base
	gfx::set_pen(1.0, 0.7, 0.7, 0.7, 0.5);
	gfx::fill_arc(position_[0], position_[1], radius_, 0.0, 2.0 * M_PI);

	// thick circle outline for base
	gfx::set_pen(3.0, 0.2, 0.2, 0.2, 1.0);
	gfx::draw_arc(position_[0], position_[1], radius_, 0.0, 2.0 * M_PI);
}

static double const radius_;

Vector position_;
};

double const Robot::radius_(0.5);

Robot rstart;
Robot rend;
vector <Robot> robots;

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

static void update_robots()
{
	rstart.update(qs);
	rend.update(qe);
	if (nq != robots.size())
		robots.resize(nq);
	for (size_t ii(0); ii < nq; ++ii)
		robots[ii].update(xi.block(ii * cdim, 0, cdim, 1));
}

static void cb_idle()
{
	if (state == PAUSE) return;  // nothinh new happened

	chomp::generatePath(qs, qe, xi, obs);
	update_robots();
}

static void cb_draw()
{
	//////////////////////////////////////////////////
	// set bounds

	Vector bmin(qs);
	Vector bmax(qs);

	for (size_t ii(0); ii < 2; ++ii) {
		if (qe[ii] < bmin[ii])
			bmin[ii] = qe[ii];
		if (qe[ii] > bmax[ii])
			bmax[ii] = qe[ii];
		for (size_t jj(0); jj < nq; ++jj) {
			if (xi[ii + cdim * jj] < bmin[ii])
				bmin[ii] = xi[ii + cdim * jj];
			if (xi[ii + cdim * jj] > bmax[ii])
				bmax[ii] = xi[ii + cdim * jj];
		}
	}

	gfx::set_view(bmin[0] - 2.0, bmin[1] - 2.0, bmax[0] + 2.0, bmax[1] + 2.0);

	//////////////////////////////////////////////////
	// robots

	rstart.draw();
	for (size_t ii(0); ii < robots.size(); ++ii)
		robots[ii].draw();
	rend.draw();

	//////////////////////////////////////////////////
	// trj

	gfx::set_pen(1.0, 0.2, 0.2, 0.2, 1.0);
	gfx::draw_line(qs[0], qs[1], xi[0], xi[1]);
	for (size_t ii(1); ii < nq; ++ii)
		gfx::draw_line(xi[(ii - 1) * cdim], xi[(ii - 1) * cdim + 1], xi[ii * cdim], xi[ii * cdim + 1]);
	gfx::draw_line(xi[(nq - 1) * cdim], xi[(nq - 1) * cdim + 1], qe[0], qe[1]);

	gfx::set_pen(5.0, 0.8, 0.2, 0.2, 1.0);
	gfx::draw_point(qs[0], qs[1]);
	gfx::set_pen(5.0, 0.5, 0.5, 0.5, 1.0);
	for (size_t ii(0); ii < nq; ++ii)
		gfx::draw_point(xi[ii * cdim], xi[ii * cdim + 1]);
	gfx::set_pen(5.0, 0.2, 0.8, 0.2, 1.0);
	gfx::draw_point(qe[0], qe[1]);

	//////////////////////////////////////////////////
	// handles
	for (int ii = 0; ii < obs.cols(); ++ii) {
		gfx::set_pen(1.0, 0.0, 0.0, 1.0, 0.2);
		gfx::fill_arc(obs(0, ii), obs(1, ii), obs(2, ii), 0.0, 2.0 * M_PI);
	}
}

static void cb_mouse(double px, double py, int flags)
{
	if ((flags & gfx::MOUSE_RELEASE) && (flags & gfx::MOUSE_B3)) {
		//add new obstacle at that location
		add_obs(px, py, 2.0);
	} else if (flags & gfx::MOUSE_PRESS) {
		for (int ii = 0; ii < obs.cols(); ++ii) {
			Vector offset(obs.block(0, ii, 2, 1));
			offset[0] -= px;
			offset[1] -= py;
			if (offset.norm() <= obs(2, ii)) {
				grab_offset = offset;
				grabbed = ii;
				state = RUN;
				break;
			}
		}
	} else if (flags & gfx::MOUSE_DRAG) {
		if (-1 != grabbed) {
			obs(0, grabbed) = px + grab_offset(0);
			obs(1, grabbed) = py + grab_offset(1);
		}
	} else if (flags & gfx::MOUSE_RELEASE) {
		grabbed = -1;
		state = PAUSE;
	}
}

int main()
{
	qs.resize(cdim);
	qs << -5.0, -5.0;
	qe.resize(cdim);
	qe << 7.0, 7.0;
	add_obs(3.0, 0.0, 2.0);
	add_obs(0.0, 3.0, 2.0);

	chomp::generatePath(qs, qe, xi, obs);
	update_robots();
	state = PAUSE;
	gfx::main("chomp", cb_idle, cb_draw, cb_mouse);
}


#ifndef _FUSION_MOVE_HPP_
#define _FUSION_MOVE_HPP_

#include <iostream>
#include <sstream>
#include <boost/foreach.hpp>
#include "higher-order-energy.hpp"
#include "clique.hpp"

#define NO_QPBO
#ifndef NO_QPBO
#include "QPBO.h"
#endif

/*
 * fusion-move.hpp
 *
 * Copyright 2012 Alexander Fix
 * See LICENSE.txt for license information
 *
 * Computes a fusion move between the current and proposed image.
 *
 * A fusion move takes two images (current and proposed) and tries to perform
 * the optimal move where each pixel is allowed to either stay at its current
 * value, or switch to its label in the proposed image. This is a
 * generalization of alpha-expansion, where in alpha-expansion each pixel is
 * allowed to either stay the same, or change to a fixed value alpha. That is,
 * alpha expansion is a fusion move where the proposed image is just the flat
 * image with value alpha at all pixels.
 *
 * Template Parameters:
 *  RandomAccessIterator    Needs to support operator[] giving results of type
 *                              Label, so that iter[i] gives the label of
 *                              pixel i
 *  Energy                  The value type of the energy function
 *  Label                   The label space of the function
 *  D                       The maximum number of variables in any clique
 *  QuadraticRep            The type used for optimizing quadratic boolean
 *                              functions
 *
 * Parameters:
 *  size            The number of variables (pixels) in the image
 *  current         The current image
 *  proposed        The proposed image to fuse with
 *  out             Result will be written here
 *  cliqueSystem    The cliques defining the energy
 *  qr              An empty QuadraticRep. The results of the higher-order
 *                      reduction will be written to this variable, and then
 *                      solved.
 *
 * All of current, proposed and out must be of size 'size'
 * cliqueSystem should refer to variables only in the range 0..(size-1)
 */
template <typename RandomAccessIterator,
         typename Energy,
         typename Label,
         int D,
         typename QuadraticRep>
void FusionMove(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        RandomAccessIterator out,
        const CliqueSystem<Energy, Label, D>& cliqueSystem,
        QuadraticRep& qr);

#ifndef NO_QPBO
/*
 * Computes a fusion move with default quadratic representation
 *
 * Same as the above function, but with the QuadraticRep parameter set
 * to QPBO<Energy> as a default
 */
template <typename RandomAccessIterator,
         typename Energy,
         typename Label,
         int D>
void FusionMove(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        RandomAccessIterator out,
        const CliqueSystem<Energy, Label, D>& cliqueSystem);
#endif

/*
 * Set up the fusion move energy as a HigherOrderEnergy
 *
 * Template Parameters:
 *  Same as FusionMove
 *
 * Parameters:
 *  size, current, proposed, cliqueSystem same as FusionMove
 *  hoe     Output parameter. Result of setting up the fusion move as a
 *              higher-order pseudo-boolean function is put here
 */
template <typename RandomAccessIterator,
         typename Energy,
         typename Label,
         int D>
void SetupFusionEnergy(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        const CliqueSystem<Energy, Label, D>& cliqueSystem,
        HigherOrderEnergy<Energy, D>& hoe);

/*
 * Given a labeling (according to the results of qr.GetLabel) fuse the images
 * current and proposed.
 *
 * Labels of 0 and -1 (unlabeled) are assigned to their value in current
 * Labels of 1 are assigned to their value in proposed in the out image
 */
template <typename RandomAccessIterator, typename QuadraticRep>
void GetFusedImage(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        RandomAccessIterator out,
        QuadraticRep& qr);

/*
 * Implementation
 */

template <typename RandomAccessIterator,
    typename Energy,
    typename Label,
    int D,
    typename QuadraticRep>
void FusionMove(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        RandomAccessIterator out,
        const CliqueSystem<Energy, Label, D>& cliqueSystem,
        QuadraticRep& qr)
{
    HigherOrderEnergy<Energy, D> hoe;
    SetupFusionEnergy(size, current, proposed, cliqueSystem, hoe);
    hoe.ToQuadratic(qr);
    qr.Solve();
    GetFusedImage(size, current, proposed, out, qr);
}

#ifndef NO_QPBO
template <typename RandomAccessIterator,
    typename Energy,
    typename Label,
    int D>
void FusionMove(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        RandomAccessIterator out,
        const CliqueSystem<Energy, Label, D>& cliqueSystem)
{
    QPBO<Energy> qr(size, 0);
    FusionMove(size, current, proposed, out, cliqueSystem, qr);
}
#endif

template <typename RandomAccessIterator,
    typename Energy,
    typename Label,
    int D>
void SetupFusionEnergy(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        const CliqueSystem<Energy, Label, D>& cliqueSystem,
        HigherOrderEnergy<Energy, D>& hoe)
{
    hoe.AddVars(size);
    typedef typename CliqueSystem<Energy, Label, D>::CliquePointer
        CliquePointer;
    BOOST_FOREACH(const CliquePointer& cp, cliqueSystem.GetCliques()) {
        const CliqueEnergy<Energy, Label, D>& c = *cp;
        unsigned int size = c._size;

        if (size == 0) {
            continue;
        } else if (size == 1) {
            Energy e0 = c(&current[c._neighborhood[0]]);
            Energy e1 = c(&proposed[c._neighborhood[0]]);
            hoe.AddUnaryTerm(c._neighborhood[0], e1 - e0);
        } else {
            unsigned int numAssignments = 1 << size;
            Energy coeffs[numAssignments];
            for (unsigned int subset = 1; subset < numAssignments; ++subset) {
                coeffs[subset] = 0;
            }
            // For each boolean assignment, get the clique energy at the
            // corresponding labeling
            Label cliqueLabels[size];
            for (unsigned int assignment = 0;
                    assignment < numAssignments;
                    ++assignment)
            {
                for (unsigned int i = 0; i < size; ++i) {
                    if (assignment & (1 << i)) {
                        cliqueLabels[i] = proposed[c._neighborhood[i]];
                    } else {
                        cliqueLabels[i] = current[c._neighborhood[i]];
                    }
                }
                Energy energy = c(cliqueLabels);
                for (unsigned int subset = 1;
                        subset < numAssignments;
                        ++subset)
                {
                    if (assignment & ~subset) {
                        continue;
                    } else {
                        int parity = 0;
                        for (unsigned int b = 0; b < size; ++b) {
                            parity ^=
                                (((assignment ^ subset) & (1 << b)) != 0);
                        }
                        coeffs[subset] += parity ? -energy : energy;
                    }
                }
            }
            typename HigherOrderEnergy<Energy, D>::VarId vars[D];
            for (unsigned int subset = 1; subset < numAssignments; ++subset) {
                int degree = 0;
                for (unsigned int b = 0; b < size; ++b) {
                    if (subset & (1 << b)) {
                        vars[degree++] = c._neighborhood[b];
                    }
                }
                std::sort(vars, vars+degree);
                hoe.AddTerm(coeffs[subset], degree, vars);
            }
        }
    }
}

template <typename RandomAccessIterator, typename QuadraticRep>
void GetFusedImage(size_t size,
        RandomAccessIterator current,
        RandomAccessIterator proposed,
        RandomAccessIterator out,
        QuadraticRep& qr)
{
    for (size_t i = 0; i < size; ++i) {
        int label = qr.GetLabel(i);
        if (label == 1) {
            out[i] = proposed[i];
        } else if (label == 0) {
            out[i] = current[i];
        } else {
            out[i] = current[i];
        }
    }
}
#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/optional/optional.hpp>
#include <cmath>
#include <random>

#include "Domain/CoordinateMaps/SpecialMobius.hpp"
#include "ErrorHandling/Error.hpp"
#include "Utilities/StdArrayHelpers.hpp"
#include "tests/Unit/Domain/CoordinateMaps/TestMapHelpers.hpp"

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.SpecialMobius.Suite",
                  "[Domain][Unit]") {
  // Set up random number generator
  std::random_device rd;
  std::mt19937 gen(rd());
  // Note: Empirically we have found that the map is accurate
  // to 12 decimal places for mu = 0.96.
  // test_suite demands more accuracy than 12 decimal places
  // so we narrow the range of mu correspondingly.
  std::uniform_real_distribution<> mu_dis(-0.85, 0.85);

  const double mu = mu_dis(gen);
  CAPTURE_PRECISE(mu);
  const CoordinateMaps::SpecialMobius special_mobius_map(mu);
  test_suite_for_map_on_sphere(special_mobius_map);
}

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.SpecialMobius.Map",
                  "[Domain][Unit]") {
  // Set up random number generator:
  const auto seed = std::random_device{}();
  std::mt19937 gen(seed);
  INFO("seed = " << seed);
  std::uniform_real_distribution<> radius_dis(0, 1);
  std::uniform_real_distribution<> phi_dis(0, 2.0 * M_PI);
  std::uniform_real_distribution<> theta_dis(0, M_PI);
  // Note: Empirically we have found that the map is accurate
  // to 12 decimal places for mu = 0.96.
  std::uniform_real_distribution<> mu_dis(-0.90, 0.90);
  const double theta = theta_dis(gen);
  CAPTURE_PRECISE(theta);
  const double phi = phi_dis(gen);
  CAPTURE_PRECISE(phi);
  const double radius = radius_dis(gen);
  CAPTURE_PRECISE(radius);
  const double x = radius * sin(theta) * cos(phi);
  CAPTURE_PRECISE(x);
  const double y = radius * sin(theta) * sin(phi);
  CAPTURE_PRECISE(y);
  const double z = radius * cos(theta);
  CAPTURE_PRECISE(z);
  const double mu = mu_dis(gen);
  CAPTURE_PRECISE(mu);
  const std::array<double, 3> input_point{{x, y, z}};
  const CoordinateMaps::SpecialMobius special_mobius_map(mu);
  const auto result_point = special_mobius_map(input_point);
  const auto& result_y = result_point[1];
  const auto& result_z = result_point[2];

  // Points inside the unit ball should remain inside the unit ball
  // under the SpecialMobius map:
  CHECK(magnitude(result_point) < 1.0);
  // As a map obtained by rotation about the x-axis, angles in
  // the y-z plane must be preserved:
  CHECK(atan2(y, z) == approx(atan2(result_y, result_z)));
  // The value `mu` should correspond to the x-coordinate of
  // the preimage of the origin under this map. That is, the
  // point with the x-coordinate `mu` (and y=z=0) should be
  // mapped to the origin:
  CHECK_ITERABLE_APPROX(
      (std::array<double, 3>{{0.0, 0.0, 0.0}}),
      (special_mobius_map(std::array<double, 3>{{mu, 0.0, 0.0}})));
  const std::array<double, 3> plus_one{{1.0, 0.0, 0.0}};
  const std::array<double, 3> minus_one{{-1.0, 0.0, 0.0}};
  // The points (1,0,0) and (-1,0,0) are fixed points:
  CHECK_ITERABLE_APPROX((plus_one), (special_mobius_map(plus_one)));
  CHECK_ITERABLE_APPROX((minus_one), (special_mobius_map(minus_one)));

  // Point at which the map is singular.
  // Since |mu|<1, this point also has x outside [-1,1].
  const std::array<double, 3> bad_point{{-1.0 / mu, 0.0, 0.0}};
  CHECK_FALSE(static_cast<bool>(special_mobius_map.inverse(bad_point)));
}

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.SpecialMobius.LargeMu",
                  "[Domain][Unit]") {
  const double mu = 0.95;
  CAPTURE_PRECISE(mu);
  // A point on the unit sphere with x=0, y!=z:
  const std::array<double, 3> input_point{{0.0, 0.6, 0.8}};
  const CoordinateMaps::SpecialMobius special_mobius_map(mu);
  const auto result_point = special_mobius_map(input_point);
  const auto expected_input_point =
      special_mobius_map.inverse(result_point).get();
  const auto& result_y = result_point[1];
  const auto& result_z = result_point[2];
  // The SpecialMobius map should map the unit sphere to itself:
  CHECK(magnitude(result_point) == approx(1.0));
  // As a map obtained by rotation about the x-axis, angles in
  // the y-z plane must be preserved:
  CHECK(atan2(0.6, 0.8) == approx(atan2(result_y, result_z)));
  // The map is singular for mu = 1.0, but we can guarantee
  // that the accuracy does not suffer even for values of mu
  // that are close to 1.0:
  CHECK(abs(expected_input_point[0] - 0.0) < 1.e-13);
  CHECK(abs(expected_input_point[1] - 0.6) < 1.e-13);
  CHECK(abs(expected_input_point[2] - 0.8) < 1.e-13);
}

// [[OutputRegex, The magnitude of mu must be less than 0.96.]]
[[noreturn]] SPECTRE_TEST_CASE(
    "Unit.Domain.CoordinateMaps.SpecialMobius.Assert1", "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_special_mobius = CoordinateMaps::SpecialMobius(-2.3);
  static_cast<void>(failed_special_mobius);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The magnitude of mu must be less than 0.96.]]
[[noreturn]] SPECTRE_TEST_CASE(
    "Unit.Domain.CoordinateMaps.SpecialMobius.Assert2", "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_special_mobius = CoordinateMaps::SpecialMobius(0.98);
  static_cast<void>(failed_special_mobius);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <complex>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <cmath>

#include <boost/timer.hpp>

#include "dune/common/stdstreams.hh"
#include "dune/grid/sgrid.hh"
#include "dune/grid/uggrid.hh"
#include "dune/grid/geometrygrid.hh"
#include "dune/grid/common/gridinfo.hh"
#include "dune/istl/solvers.hh"

#include "fem/assemble.hh"
#include "fem/embedded_errorest.hh"
#include "fem/istlinterface.hh"
#include "fem/functional_aux.hh"
#include "fem/hierarchicspace.hh"
#include "fem/lagrangespace.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/triplet.hh"
#include "linalg/iluprecond.hh"
#include "linalg/additiveschwarz.hh"
#include "linalg/hyprecond.hh"
#include "io/vtk.hh"
#include "io/amira.hh"

#include "utilities/kaskopt.hh"

#include "ht.hh"

 namespace Dune
  {

    namespace Capabilities
    {

      template< class Grid >
      struct hasHierarchicIndexSet
      {
        static const bool v = false;
      };

      template< class Grid >
      struct hasHierarchicIndexSet< const Grid >
      {
        static const bool v = hasHierarchicIndexSet< Grid >::v;
      };

    }

  }

class SquareToCircle
  : public Dune::AnalyticalCoordFunction< double, 2, 2, SquareToCircle >
  {
    typedef SquareToCircle This;
    typedef Dune::AnalyticalCoordFunction< double, 2, 2, This > Base;

  public:
    typedef Base::DomainVector DomainVector;
    typedef Base::RangeVector RangeVector;

    void evaluate ( const DomainVector &x, RangeVector &y ) const
    {
      double enorm = sqrt(x[0]*x[0]+x[1]*x[1]);

      if(enorm > 0.00001)
      {

        double radius = sqrt(2.0)*std::max(std::fabs(x[0]),std::fabs(x[1]));

      double scaling = radius/enorm;

      y[ 0 ] = x[ 0 ]*scaling;
      y[ 1 ] = x[ 1 ]*scaling;
      } else
      {
      y[ 0 ] = x[ 0 ];
      y[ 1 ] = x[ 1 ];
      }

    }
  };

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

	//   two-dimensional space: dim=2
	int const dim=2;
	typedef Dune::UGGrid<dim> Grid;

        typedef Dune::GeometryGrid<Grid,SquareToCircle> GGrid;

	Dune::GridFactory<Grid> factory;

	// vertex coordinates v[0], v[1]
	Dune::FieldVector<double,dim> v;
	v[0]=-1; v[1]=-1; factory.insertVertex(v);
	v[0]=1; v[1]=-1; factory.insertVertex(v);
	v[0]=1; v[1]=1; factory.insertVertex(v);
	v[0]=-1; v[1]=1; factory.insertVertex(v);
	v[0]=0; v[1]=0; factory.insertVertex(v);
	// triangle defined by 3 vertex indices
	std::vector<unsigned int> vid(3);
	Dune::GeometryType gt(Dune::GeometryType::simplex,2);
	vid[0]=0; vid[1]=1; vid[2]=4; factory.insertElement(gt,vid);
	vid[0]=1; vid[1]=2; vid[2]=4; factory.insertElement(gt,vid);
	vid[0]=2; vid[1]=3; vid[2]=4; factory.insertElement(gt,vid);
	vid[0]=3; vid[1]=0; vid[2]=4; factory.insertElement(gt,vid);
	std::auto_ptr<Grid> grid( factory.createGrid() ) ;

	// the coarse grid will be refined three times
	// some information on the refined mesh
	std::cout << "Grid: " << grid->size(0) << " triangles, " << std::endl;
	std::cout << "      " << grid->size(1) << " edges, " << std::endl;
	std::cout << "      " << grid->size(2) << " points" << std::endl;
	// a gridmanager is constructed
	// as connector between geometric and algebraic information

        SquareToCircle def;

        std::auto_ptr<GGrid> ggrid(new GGrid(*grid,def));

	ggrid->globalRefine(6);

	GridManager<GGrid> gridManager(ggrid);

//StartSnippet2
	// construction of finite element space for the scalar solution T
	typedef GGrid::LeafGridView GridView;
	typedef FEFunctionSpace<ContinuousLagrangeMapper<double,GridView> > H1Space;
	H1Space temperatureSpace(gridManager,gridManager.grid().leafView(),
							 1);
	typedef boost::fusion::vector<H1Space const*> Spaces;
	Spaces spaces(&temperatureSpace);
	// VariableDescription<int spaceId, int components, int Id>
	// spaceId: number of associated FEFunctionSpace
	// components: number of components in this variable
	// Id: number of this variable
	typedef boost::fusion::vector<VariableDescription<0,1,0> >
			VariableDescriptions;
	std::string varNames[1] = { "T" };
	typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
	VariableSet variableSet(spaces,varNames);
//StopSnippet2

//StartSnippet3
	typedef HeatFunctional<double,VariableSet> Functional;
	Functional F;
	typedef VariationalFunctionalAssembler<LinearizationAt<Functional> > GOP;
	typedef GOP::TestVariableRepresentation<>::type Rhs;
	GOP gop(gridManager.signals,spaces);
	VariableSet::VariableSet x(variableSet);
//StopSnippet3

//StartSnippet4

	typedef GOP::AnsatzVariableRepresentation<>::type Sol;
	Sol solution(GOP::AnsatzVariableRepresentation<>::init(gop));
	solution = 0;
	gop.assemble(linearization(F,x));

	Rhs rhs(GOP::TestVariableRepresentation<>::rhs(gop));
	AssembledGalerkinOperator<GOP,0,1,0,1> A(gop, false);
	AssembledGalerkinOperator<GOP,0,1,0,1>::matrix_type tri(A.getmat());
//	A.getmat().print();

//StartSnippet5
    boost::timer directTimer;
    directInverseOperator(A,DirectType::UMFPACK,MatrixProperties::GENERAL).applyscaleadd(-1.0,rhs,solution);
    std::cout << "direct solve: " << directTimer.elapsed() << "s\n";
    x.data = solution.data;
//StopSnippet5

//StartSnippet6
	// output of solution in VTK format for visualization,
	// the data are written as ascii stream into file temperature.vtu,
	// possible is also binary
 	IoOptions options;
 	options.outputType = IoOptions::ascii;

    typedef GGrid::LeafGridView LeafGridView;
    LeafGridView leafView = gridManager.grid().leafView();
    writeVTKFile(leafView,variableSet,x,"temperature",options);

  }


// Copyright (c) 2016 The Bitcoin Core developers
// Copyright (c) 2016-2019 The MagnaChain Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "misc/amount.h"
#include "policy/feerate.h"
#include "test/test_magnachain.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(amount_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(MoneyRangeTest)
{
    BOOST_CHECK_EQUAL(MoneyRange(MCAmount(-1)), false);
    BOOST_CHECK_EQUAL(MoneyRange(MAX_MONEY + MCAmount(1)), false);
    BOOST_CHECK_EQUAL(MoneyRange(MCAmount(1)), true);
}

BOOST_AUTO_TEST_CASE(GetFeeTest)
{
    MCFeeRate feeRate, altFeeRate;

    feeRate = MCFeeRate(0);
    // Must always return 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e5), 0);

    feeRate = MCFeeRate(1000);
    // Must always just return the arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), 1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 121);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 999);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), 1e3);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), 9e3);

    feeRate = MCFeeRate(-1000);
    // Must always just return -1 * arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), -1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), -121);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), -999);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), -1e3);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), -9e3);

    feeRate = MCFeeRate(123);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), 1); // Special case: returns 1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), 1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 14);
    BOOST_CHECK_EQUAL(feeRate.GetFee(122), 15);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 122);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), 123);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), 1107);

    feeRate = MCFeeRate(-123);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), -1); // Special case: returns -1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), -1);

    // check alternate constructor
    feeRate = MCFeeRate(1000);
    altFeeRate = MCFeeRate(feeRate);
    BOOST_CHECK_EQUAL(feeRate.GetFee(100), altFeeRate.GetFee(100));

    // Check full constructor
    // default value
    BOOST_CHECK(MCFeeRate(MCAmount(-1), 1000) == MCFeeRate(-1));
    BOOST_CHECK(MCFeeRate(MCAmount(0), 1000) == MCFeeRate(0));
    BOOST_CHECK(MCFeeRate(MCAmount(1), 1000) == MCFeeRate(1));
    // lost precision (can only resolve satoshis per kB)
    BOOST_CHECK(MCFeeRate(MCAmount(1), 1001) == MCFeeRate(0));
    BOOST_CHECK(MCFeeRate(MCAmount(2), 1001) == MCFeeRate(1));
    // some more integer checks
    BOOST_CHECK(MCFeeRate(MCAmount(26), 789) == MCFeeRate(32));
    BOOST_CHECK(MCFeeRate(MCAmount(27), 789) == MCFeeRate(34));
    // Maximum size in bytes, should not crash
    MCFeeRate(MAX_MONEY, std::numeric_limits<size_t>::max() >> 1).GetFeePerK();
}

BOOST_AUTO_TEST_CASE(BinaryOperatorTest)
{
    MCFeeRate a, b;
    a = MCFeeRate(1);
    b = MCFeeRate(2);
    BOOST_CHECK(a < b);
    BOOST_CHECK(b > a);
    BOOST_CHECK(a == a);
    BOOST_CHECK(a <= b);
    BOOST_CHECK(a <= a);
    BOOST_CHECK(b >= a);
    BOOST_CHECK(b >= b);
    // a should be 0.00000002 BTC/kB now
    a += a;
    BOOST_CHECK(a == b);
}

BOOST_AUTO_TEST_CASE(ToStringTest)
{
    MCFeeRate feeRate;
    feeRate = MCFeeRate(1);
    BOOST_CHECK_EQUAL(feeRate.ToString(), "0.00000001 MGC/kB");
}

BOOST_AUTO_TEST_SUITE_END()


/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include "cq/CQSpectrogram.h"

#include "dsp/Window.h"

#include <cmath>
#include <vector>
#include <iostream>

using std::vector;
using std::cerr;
using std::endl;

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(TestCQTime)

// Principle: Run a Dirac impulse through the CQ transform and check
// that its output has all the peak bins aligned correctly in time.

// Set up fs/2 = 50, frequency range 10 -> 40 i.e. 2 octaves, fixed
// duration of 2 seconds
static const double sampleRate = 100;
static const double cqmin = 10;
static const double cqmax = 40;
static const double bpo = 4;
static const int duration = sampleRate * 2;

// Threshold below which to ignore a column completely
static const double threshold = 0.08;

void
testCQTime(double t)
{
    vector<CQSpectrogram::Interpolation> interpolationTypes;
    interpolationTypes.push_back(CQSpectrogram::InterpolateZeros);
    interpolationTypes.push_back(CQSpectrogram::InterpolateHold);
    interpolationTypes.push_back(CQSpectrogram::InterpolateLinear);

    for (int k = 0; k < int(interpolationTypes.size()); ++k) {

        CQSpectrogram::Interpolation interp = interpolationTypes[k];

        CQParameters params(sampleRate, cqmin, cqmax, bpo);
        CQSpectrogram cq(params, interp);

        BOOST_CHECK_EQUAL(cq.getBinsPerOctave(), bpo);
        BOOST_CHECK_EQUAL(cq.getOctaves(), 2);

        vector<double> input(duration, 0.0);
        int ix = int(floor(t * sampleRate));
        if (ix >= duration) ix = duration-1;
        input[ix] = 1.0;

        CQSpectrogram::RealBlock output = cq.process(input);
        CQSpectrogram::RealBlock rest = cq.getRemainingOutput();
        output.insert(output.end(), rest.begin(), rest.end());

        BOOST_CHECK_EQUAL(output[0].size(),
                          cq.getBinsPerOctave() * cq.getOctaves());

        vector<int> peaks;
        double eps = 1e-8;

        for (int j = 0; j < int(output[0].size()); ++j) {

            int maxidx = -1;
            double max = 0.0;
            for (int i = 0; i < int(output.size()); ++i) {
                double value = output[i][j];
                if (i == 0 || value + eps > max) {
                    max = value;
                    maxidx = i;
                }
            }

            peaks.push_back(maxidx);
        }

        for (int j = 1; j < int(peaks.size()); ++j) {
            int oct = j / bpo;
            int spacing = (1 << oct);
            int actual = peaks[j]/spacing;
            int expected = int(round(double(peaks[0])/spacing));
            if (actual != expected) {
                cerr << "ERROR: In row " << j << " (bin freq "
                     << cq.getBinFrequency(j) << "), interpolation " << interp
                     << ", maximum value for time " << t
                     << "\n       found at index " << peaks[j]
                     << " of " << output.size() << " which does not align with"
                     << " highest frequency\n       bin peak at " << peaks[0]
                     << " given octave spacing of " << spacing
                     << "\n       [latency = " << cq.getLatency()
                     << ", hop = " << cq.getColumnHop() << ", duration = "
                     << duration << ", ix = " << ix << "]" << endl;
                cerr << "row contains: ";
                for (int i = 0; i < int(output.size()); ++i) {
                    if (i == expected * spacing) cerr << "*";
                    if (i == peaks[j]) cerr << "**";
                    cerr << output[i][j] << " ";
                }
                cerr << endl;

                BOOST_CHECK_EQUAL(actual, expected);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(time_zero) { testCQTime(0); }
BOOST_AUTO_TEST_CASE(time_half) { testCQTime(0.5); }
BOOST_AUTO_TEST_CASE(time_one) { testCQTime(1.0); }
BOOST_AUTO_TEST_CASE(time_two) { testCQTime(2.0); }

BOOST_AUTO_TEST_SUITE_END()


// test file for HSO3.hpp and HSO4.hpp

//  (C) Copyright Hubert Holin 2001.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/math/quaternion.hpp>

#include "HSO3.hpp"
#include "HSO4.hpp"

const int    number_of_intervals = 5;

const float    pi = ::std::atan(1.0f)*4;

void    test_SO3();

void    test_SO4();

int    main()

{
    test_SO3();

    test_SO4();

    ::std::cout << "That's all folks!" << ::std::endl;
}

//
//    Test of quaternion and R^3 rotation relationship
//

void    test_SO3_spherical()
{
    ::std::cout << "Testing spherical:" << ::std::endl;
    ::std::cout << ::std::endl;

    const float    rho = 1.0f;

    float        theta;
    float        phi1;
    float        phi2;

    for    (int idxphi2 = 0; idxphi2 <= number_of_intervals; idxphi2++)
    {
        phi2 = (-pi/2)+(idxphi2*pi)/number_of_intervals;

        for    (int idxphi1 = 0; idxphi1 <= number_of_intervals; idxphi1++)
        {
            phi1 = (-pi/2)+(idxphi1*pi)/number_of_intervals;

            for    (int idxtheta = 0; idxtheta <= number_of_intervals; idxtheta++)
            {
                theta = -pi+(idxtheta*(2*pi))/number_of_intervals;

                ::std::cout << "theta = " << theta << " ; ";
                ::std::cout << "phi1 = " << phi1 << " ; ";
                ::std::cout << "phi2 = " << phi2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::spherical(rho, theta, phi1, phi2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_semipolar()
{
    ::std::cout << "Testing semipolar:" << ::std::endl;
    ::std::cout << ::std::endl;

    const float    rho = 1.0f;

    float        alpha;
    float        theta1;
    float        theta2;

    for    (int idxalpha = 0; idxalpha <= number_of_intervals; idxalpha++)
    {
        alpha = (idxalpha*(pi/2))/number_of_intervals;

        for    (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
        {
            theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;

            for    (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
            {
                theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;

                ::std::cout << "alpha = " << alpha << " ; ";
                ::std::cout << "theta1 = " << theta1 << " ; ";
                ::std::cout << "theta2 = " << theta2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::semipolar(rho, alpha, theta1, theta2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_multipolar()
{
    ::std::cout << "Testing multipolar:" << ::std::endl;
    ::std::cout << ::std::endl;

    float    rho1;
    float    rho2;

    float    theta1;
    float    theta2;

    for    (int idxrho = 0; idxrho <= number_of_intervals; idxrho++)
    {
        rho1 = (idxrho*1.0f)/number_of_intervals;
        rho2 = ::std::sqrt(1.0f-rho1*rho1);

        for    (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
        {
            theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;

            for    (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
            {
                theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;

                ::std::cout << "rho1 = " << rho1 << " ; ";
                ::std::cout << "theta1 = " << theta1 << " ; ";
                ::std::cout << "theta2 = " << theta2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::multipolar(rho1, theta1, rho2, theta2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_cylindrospherical()
{
    ::std::cout << "Testing cylindrospherical:" << ::std::endl;
    ::std::cout << ::std::endl;

    float    t;

    float    radius;
    float    longitude;
    float    latitude;

    for    (int idxt = 0; idxt <= number_of_intervals; idxt++)
    {
        t = -1.0f+(idxt*2.0f)/number_of_intervals;
        radius = ::std::sqrt(1.0f-t*t);

        for    (int idxlatitude = 0; idxlatitude <= number_of_intervals; idxlatitude++)
        {
            latitude = (-pi/2)+(idxlatitude*pi)/number_of_intervals;

            for    (int idxlongitude = 0; idxlongitude <= number_of_intervals; idxlongitude++)
            {
                longitude = -pi+(idxlongitude*(2*pi))/number_of_intervals;

                ::std::cout << "t = " << t << " ; ";
                ::std::cout << "longitude = " << longitude;
                ::std::cout << "latitude = " << latitude;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::cylindrospherical(t, radius, longitude, latitude);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_cylindrical()
{
    ::std::cout << "Testing cylindrical:" << ::std::endl;
    ::std::cout << ::std::endl;

    float    r;
    float    angle;

    float    h1;
    float    h2;

    for    (int idxh2 = 0; idxh2 <= number_of_intervals; idxh2++)
    {
        h2 = -1.0f+(idxh2*2.0f)/number_of_intervals;

        for    (int idxh1 = 0; idxh1 <= number_of_intervals; idxh1++)
        {
            h1 = ::std::sqrt(1.0f-h2*h2)*(-1.0f+(idxh2*2.0f)/number_of_intervals);
            r = ::std::sqrt(1.0f-h1*h1-h2*h2);

            for    (int idxangle = 0; idxangle <= number_of_intervals; idxangle++)
            {
                angle = -pi+(idxangle*(2*pi))/number_of_intervals;

                ::std::cout << "angle = " << angle << " ; ";
                ::std::cout << "h1 = " << h1;
                ::std::cout << "h2 = " << h2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::cylindrical(r, angle, h1, h2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3()
{
    ::std::cout << "Testing SO3:" << ::std::endl;
    ::std::cout << ::std::endl;

    test_SO3_spherical();

    test_SO3_semipolar();

    test_SO3_multipolar();

    test_SO3_cylindrospherical();

    test_SO3_cylindrical();
}

//
//    Test of quaternion and R^4 rotation relationship
//

void    test_SO4_spherical()
{
    ::std::cout << "Testing spherical:" << ::std::endl;
    ::std::cout << ::std::endl;

    const float    rho1 = 1.0f;
    const float    rho2 = 1.0f;

    float        theta1;
    float        phi11;
    float        phi21;

    float        theta2;
    float        phi12;
    float        phi22;

    for    (int idxphi21 = 0; idxphi21 <= number_of_intervals; idxphi21++)
    {
        phi21 = (-pi/2)+(idxphi21*pi)/number_of_intervals;

        for    (int idxphi22 = 0; idxphi22 <= number_of_intervals; idxphi22++)
        {
            phi22 = (-pi/2)+(idxphi22*pi)/number_of_intervals;

            for    (int idxphi11 = 0; idxphi11 <= number_of_intervals; idxphi11++)
            {
                phi11 = (-pi/2)+(idxphi11*pi)/number_of_intervals;

                for    (int idxphi12 = 0; idxphi12 <= number_of_intervals; idxphi12++)
                {
                    phi12 = (-pi/2)+(idxphi12*pi)/number_of_intervals;

                    for    (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
                    {
                        theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;

                        for    (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
                        {
                            theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;

                            ::std::cout << "theta1 = " << theta1 << " ; ";
                            ::std::cout << "phi11 = " << phi11 << " ; ";
                            ::std::cout << "phi21 = " << phi21;
                            ::std::cout << "theta2 = " << theta2 << " ; ";
                            ::std::cout << "phi12 = " << phi12 << " ; ";
                            ::std::cout << "phi22 = " << phi22;
                            ::std::cout << ::std::endl;

                            ::boost::math::quaternion<float>    p1 = ::boost::math::spherical(rho1, theta1, phi11, phi21);

                            ::std::cout << "p1 = " << p1 << ::std::endl;

                            ::boost::math::quaternion<float>    q1 = ::boost::math::spherical(rho2, theta2, phi12, phi22);

                            ::std::cout << "q1 = " << q1 << ::std::endl;

                            ::std::pair< ::boost::math::quaternion<float> , ::boost::math::quaternion<float> >    pq1 =
                                ::std::make_pair(p1,q1);

                            R4_matrix<float>                    rot = quaternions_to_R4_rotation(pq1);

                            ::std::cout << "rot = ";
                            ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << "\t" << rot.a14 << ::std::endl;
                            ::std::cout << "\t";
                            ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << "\t" << rot.a24 << ::std::endl;
                            ::std::cout << "\t";
                            ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << "\t" << rot.a34 << ::std::endl;
                            ::std::cout << "\t";
                            ::std::cout << "\t" << rot.a41 << "\t" << rot.a42 << "\t" << rot.a43 << "\t" << rot.a44 << ::std::endl;

                            ::std::pair< ::boost::math::quaternion<float> , ::boost::math::quaternion<float> >    pq2 =
                                R4_rotation_to_quaternions(rot, &pq1);

                            ::std::cout << "p1 = " << pq2.first << ::std::endl;
                            ::std::cout << "p2 = " << pq2.second << ::std::endl;

                            ::std::cout << "round trip discrepancy: " << ::std::sqrt(::boost::math::norm(pq1.first-pq2.first)+::boost::math::norm(pq1.second-pq2.second)) << ::std::endl;

                            ::std::cout << ::std::endl;
                        }
                    }
                }
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO4()
{
    ::std::cout << "Testing SO4:" << ::std::endl;
    ::std::cout << ::std::endl;

    test_SO4_spherical();
}


#include "mtao/opengl/Window.h"
#include <iostream>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#include <Eigen/Geometry>
#pragma GCC diagnostic pop
#include "imgui.h"
#include <memory>
#include <algorithm>
#include "mtao/geometry/mesh/boundary_facets.h"
#include "mtao/geometry/mesh/read_obj.hpp"
#include "mtao/geometry/bounding_box.hpp"
#include "mtao/opengl/drawables.h"
#include <mtao/types.h>
#include <Corrade/Containers/Array.h>
#include <Corrade/Containers/ArrayView.h>

#include <Magnum/Shaders/VertexColor.h>
#include <Magnum/GL/Renderer.h>
#include <Corrade/Utility/Arguments.h>
#include <mtao/opengl/objects/grid.h>
#include <mtao/geometry/grid/grid.h>

#include <glm/gtc/matrix_transform.hpp>
#include <memory>
#include <algorithm>

#include <mtao/eigen_utils.h>
#include<Eigen/IterativeLinearSolvers>
#include <Magnum/EigenIntegration/Integration.h>
#include "curve.hpp"
#include <mtao/geometry/grid/grid_data.hpp>
#include <mtao/geometry/winding_number.hpp>
#include <mtao/opengl/shaders/vector_field.hpp>
using namespace Magnum::Math::Literals;

using namespace mtao::opengl;

class MeshViewer: public mtao::opengl::Window2 {
    public:

        Curve::InterpMode interp_mode = Curve::InterpMode::MeanValue;
        using Vec = mtao::VectorX<GLfloat>;
        using Vec2 = mtao::Vec2f;
        Vec data;
        Vec data_original;
        Vec dx;
        Vec dy;
        Vec signs;
        Eigen::AlignedBox<float,2> bbox;

        int active_weight_index = 0;

        std::array<int,2> N{{300,300}};
        int& NI=N[0];
        int& NJ=N[1];
        Vector2 cursor;
        Curve curve;
        mtao::geometry::grid::Grid2f grid;
        mtao::geometry::grid::GridData2f grid_data;

        Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> solver;

        MeshViewer(const Arguments& args): Window2(args) {
            bbox.min().setConstant(-1);
            bbox.max().setConstant(1);

            curve.points.emplace_back(mtao::Vec2d(.8,.8));
            curve.points.emplace_back(mtao::Vec2d(.7,-.2));
            curve.points.emplace_back(mtao::Vec2d(-.8,-.2));
            curve.points.emplace_back(mtao::Vec2d(-.7,.8));

            edge_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{grid_mesh,_flat_shader, drawables()};
            edge_drawable->activate_triangles({});
            edge_drawable->activate_edges();
            face_drawable = new mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>{grid_mesh,_vcolor_shader, gridcolor_drawgroup};
            grid_mesh.setParent(&root());
            curve_mesh.setParent(&root());
            curve_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{curve_mesh,_flat_shader, drawables()};
            curve_drawable->deactivate();
            curve_mesh.setVertexBuffer(mtao::Vec2f::Zero().eval());
            curve_drawable->data().color = 0xff0000_rgbf;
            curve_drawable->activate_points();
            //
            //curve_point_drawable = new mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>{curve_points,_vcolor_shader, drawables()};
            //curve_points.setVertexBuffer(mtao::Vec2f::Zero().eval());
            //curve_points.setParent(&root());
            //mtao::geometry::grid::Grid2f g(std::array<int,2>{{NI,NJ}});
            grid = mtao::geometry::grid::Grid2f::from_bbox
                (bbox, std::array<int,2>{{NI,NJ}});

            //grid_data.resize(std::array<int,2>{{NI,NJ}});

            //auto V = g.vertices();
            //mtao::RowVectorX<float> C = V.colwise()
            grid_mesh.setColorBuffer(
                    mtao::eigen::vstack(
                        grid.vertices().array().pow(.5)
                        //,mtao::RowVectorX<float>::Zero(g.size())
                        , 1 - grid.vertices().colwise().sum().array() / 2
                        ,mtao::RowVectorX<float>::Ones(grid.size())
                        ));
            grid_mesh.set(grid);
            //curve_point_drawable->deactivate();

            //vfield_mesh.setParent(&scene());
            //_vf_viewer = new mtao::opengl::Drawable<mtao::opengl::VectorFieldShader<2>>{vfield_mesh,_vf_shader, drawables()};
            update();

            //_vf_viewer->deactivate();
            //_vf_viewer->activate_points();
        }
        void update() {
            auto& points = curve.points;
            grid = mtao::geometry::grid::Grid2f::from_bbox
                (bbox, std::array<int,2>{{NI,NJ}});
            grid_mesh.set(grid);
            active_weight_index = std::clamp<int>(active_weight_index, 0,points.size()-1);

            //grid_data.resize(std::array<int,2>{{NI,NJ}});

            if(points.size() > 1) {
                curve_mesh.setEdgeBuffer(curve.V().cast<float>().eval(),curve.E().cast<unsigned int>());
                curve_drawable->activate_edges();
                curve_drawable->activate_points({});

                auto V = grid.vertices();
                auto evaluator = curve.evaluator(interp_mode);
                mtao::VecXd coeffs = mtao::VecXd::Unit(points.size(), active_weight_index);
                std::cout << "Coeffs: " << coeffs.transpose() << std::endl;
                //mtao::VecXf D = evaluator(Dat, V.cast<double>()).cast<float>();
                mtao::VecXf D = evaluator.from_coefficients(coeffs, V.cast<double>()).cast<float>();
                D = D.cwiseAbs();
                //
                mtao::ColVecs4f C(4,grid.size());
                {

                    C.row(3).array() = 1;
                    C.array() = 1;
                    C.row(0) -= D.transpose();
                    C.row(1) -= D.transpose();
                    grid_mesh.setColorBuffer(C);
                }

                //{
                //    vfield_mesh.setVertexBuffer(V);
                //    vfield_mesh.setVFieldBuffer(G);
                //}

            } else {
                curve_mesh.setVertexBuffer(curve.V().cast<float>().eval());
            }

        }
        float scale = 1.0;
        void gui() override {
            if(ImGui::InputInt2("N", &NI))  {
                update();
            }
            {
            const char* interp_types[] = {
                "Wachpress",
                "MeanValue",
                "GaussianRBF",
                "SplineGaussianRBF",
                "DesbrunSplineRBF",
                //"RadialBasis",
            };
            int current = int(interp_mode);
            if(ImGui::ListBox("Interp modes",&current, interp_types, 5,2)){
                interp_mode = Curve::InterpMode(current);
                update();
            }
            }

            if(ImGui::InputInt("active_index", &active_weight_index))  {
                update();
            }
            if(ImGui::SliderFloat2("min", bbox.min().data(),-2,2))  {
                bbox.min() = (bbox.min().array() < bbox.max().array()).select(bbox.min(),bbox.max());
                update();
            }
            if(ImGui::SliderFloat2("max", bbox.max().data(),-2,2))  {
                bbox.max() = (bbox.min().array() > bbox.max().array()).select(bbox.min(),bbox.max());
                update();
            }
            if(ImGui::SliderFloat("scale", &scale,0,2))  {
                _vf_shader.setScale(scale);
            }
            if(edge_drawable) {
                edge_drawable->gui();
            }
            if(face_drawable) {
                face_drawable->gui();
            }
            auto evaluator = curve.evaluator(interp_mode);
            mtao::VecXd coeffs = mtao::VecXd::Unit(curve.points.size(), active_weight_index);
            float v = evaluator.from_coefficients(coeffs, mtao::Vec2d(cursor.x(),cursor.y()))(0);
            ImGui::Text("Cursor Position: (%f,%f):%f", cursor.x(), cursor.y(),v);
        }
        void draw() override {
            Magnum::GL::Renderer::disable(Magnum::GL::Renderer::Feature::DepthTest);
            Magnum::GL::Renderer::disable(Magnum::GL::Renderer::Feature::FaceCulling);
            Magnum::GL::Renderer::setPointSize(10.);
            camera().draw(gridcolor_drawgroup);
            Window2::draw();
        }

        void mouseMoveEvent(MouseMoveEvent& event) override{
            Window2::mouseMoveEvent(event);
            cursor = localPosition(event.position());
        }
        void mousePressEvent(MouseEvent& event) override{
            Window2::mousePressEvent(event);
            if(!ImGui::GetIO().WantCaptureMouse) {
                if(event.button() == MouseEvent::Button::Left) {
                    auto& points = curve.points;
                    points.emplace_back(EigenIntegration::cast<mtao::Vec2f>(cursor).cast<double>());
                    update();

                }
            }
        }

    private:
        Magnum::Shaders::Flat2D _flat_shader;
        Magnum::Shaders::VertexColor2D _vcolor_shader;
        mtao::opengl::VectorFieldShader<2> _vf_shader;

        Magnum::SceneGraph::DrawableGroup2D gridcolor_drawgroup;
        mtao::opengl::objects::Grid<2> grid_mesh;
        mtao::opengl::objects::Mesh<2> curve_mesh;
        mtao::opengl::objects::Mesh<2> curve_points;
        mtao::opengl::objects::Mesh<2> vfield_mesh;
        mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* edge_drawable = nullptr;
        mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>* face_drawable = nullptr;
        mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* curve_drawable = nullptr;
        mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>* curve_point_drawable = nullptr;
        mtao::opengl::Drawable<mtao::opengl::VectorFieldShader<2>>* _vf_viewer = nullptr;

};

MAGNUM_APPLICATION_MAIN(MeshViewer)


#include <algorithm>
#include <cmath>
#include <vector>

#include <Eigen/Dense>

#include <isce3/antenna/EdgeMethodCostFunc.h>
#include <isce3/except/Error.h>
#include <isce3/math/RootFind1dNewton.h>
#include <isce3/math/polyfunc.h>

namespace isce3 { namespace antenna {

std::tuple<double, double, bool, int> rollAngleOffsetFromEdge(
        const poly1d_t& polyfit_echo, const poly1d_t& polyfit_ant,
        const isce3::core::Linspace<double>& look_ang,
        std::optional<poly1d_t> polyfit_weight)
{
    // check the input arguments
    if (polyfit_echo.order != 3 || polyfit_ant.order != 3)
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Requires 3rd-order poly-fit object for both "
                "Echo and Antenna!");
    constexpr double a_tol {1e-5};
    if (std::abs(polyfit_echo.mean - polyfit_ant.mean) > a_tol ||
            std::abs(polyfit_echo.norm - polyfit_ant.norm) > a_tol)
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Requires same (mean, std) for Echo and Antenna Poly1d obj!");

    if (!(polyfit_echo.norm > 0.0))
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Requires positive std of Echo and Antenna Poly1d obj!");

    if (polyfit_weight) {
        if (polyfit_weight->order < 0)
            throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                    "The order of polyfit for weights must be "
                    "at least 0 (constant weights)!");
        if (!(polyfit_weight->norm > 0.0))
            throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                    "Requires positive std of weight Poly1d obj!");
    }

    // create a copy polyfit objects "echo" and "ant" with zero mean and unit
    // std
    auto pf_echo_cp = polyfit_echo;
    pf_echo_cp.mean = 0.0;
    pf_echo_cp.norm = 1.0;
    auto pf_ant_cp = polyfit_ant;
    pf_ant_cp.mean = 0.0;
    pf_ant_cp.norm = 1.0;

    // declare and initialize a look angle vector
    Eigen::ArrayXd lka_vec(look_ang.size());
    for (int idx = 0; idx < look_ang.size(); ++idx)
        lka_vec(idx) = look_ang[idx];

    // create a weighting vector from look vector and weighting Poly1d
    Eigen::ArrayXd wgt_vec;
    if (polyfit_weight) {
        Eigen::Map<Eigen::ArrayXd> wgt_coef(
                polyfit_weight->coeffs.data(), polyfit_weight->coeffs.size());
        wgt_vec = isce3::math::polyval(
                wgt_coef, lka_vec, polyfit_weight->mean, polyfit_weight->norm);
        // normalize power in dB
        wgt_vec -= wgt_vec.maxCoeff();
        // convert from dB to linear power scale
        wgt_vec = Eigen::pow(10, 0.1 * wgt_vec);
    }
    // centralized and scaled the look vector based on mean/std of the echo
    // Poly1d to be used for both antenna and echo in the cost function.
    lka_vec -= polyfit_echo.mean;
    const auto std_inv = 1.0 / polyfit_echo.norm;
    lka_vec *= std_inv;

    // form some derivatives used in the cost function
    auto pf_echo_der = pf_echo_cp.derivative();
    auto pf_ant_der = pf_ant_cp.derivative();
    auto pf_ant_der2 = pf_ant_der.derivative();
    // create a memmap of the coeff for the first and second derivatives
    Eigen::Map<Eigen::ArrayXd> coef_ant_der(
            pf_ant_der.coeffs.data(), pf_ant_der.coeffs.size());
    Eigen::Map<Eigen::ArrayXd> coef_ant_der2(
            pf_ant_der2.coeffs.data(), pf_ant_der2.coeffs.size());
    Eigen::Map<Eigen::ArrayXd> coef_echo_der(
            pf_echo_der.coeffs.data(), pf_echo_der.coeffs.size());
    // form some arrays over scaled look angles for diff of first derivatives
    // and for second derivative
    auto ant_echo_der_dif_vec =
            isce3::math::polyval(coef_ant_der - coef_echo_der, lka_vec);
    auto ant_der2_vec = isce3::math::polyval(coef_ant_der2, lka_vec);

    // build cost function in the form of Poly1d object (3th order polynimal!)
    auto cf_pf = isce3::core::Poly1d(3, 0.0, 1.0);
    // fill up the coeff for the derivative of the WMSE cost function:
    // cost(ofs) = pf_wgt*(pf_echo_der(el) - pf_ant_der(el + ofs))**2
    // See section 1.1 of the cited reference.
    if (polyfit_weight) {
        auto tmp1 = wgt_vec * ant_echo_der_dif_vec;
        auto tmp2 = wgt_vec * ant_der2_vec;
        cf_pf.coeffs[0] = (tmp1 * ant_der2_vec).sum();
        cf_pf.coeffs[1] = (tmp2 * ant_der2_vec).sum() +
                          6 * pf_ant_cp.coeffs[3] * tmp1.sum();
        cf_pf.coeffs[2] = 9 * pf_ant_cp.coeffs[3] * tmp2.sum();
        cf_pf.coeffs[3] =
                18 * pf_ant_cp.coeffs[3] * pf_ant_cp.coeffs[3] * wgt_vec.sum();
    } else // no weighting
    {
        cf_pf.coeffs[0] = (ant_echo_der_dif_vec * ant_der2_vec).sum();
        cf_pf.coeffs[1] = ant_der2_vec.square().sum() +
                          6 * pf_ant_cp.coeffs[3] * ant_echo_der_dif_vec.sum();
        cf_pf.coeffs[2] = 9 * pf_ant_cp.coeffs[3] * ant_der2_vec.sum();
        cf_pf.coeffs[3] = 18 * pf_ant_cp.coeffs[3] * pf_ant_cp.coeffs[3] *
                          look_ang.size();
    }
    // form Root finding object
    auto rf_obj =
            isce3::math::RootFind1dNewton(1e-4, 20, look_ang.spacing() / 10.);
    // solve for the root/roll offset via Newton
    auto [roll, f_val, flag, n_iter] = rf_obj.root(cf_pf);
    // scale back the roll angle by std of original poly1d object
    roll *= polyfit_echo.norm;

    return {roll, f_val, flag, n_iter};
}

std::tuple<double, double, bool, int> rollAngleOffsetFromEdge(
        const poly1d_t& polyfit_echo, const poly1d_t& polyfit_ant,
        double look_ang_near, double look_ang_far, double look_ang_prec,
        std::optional<poly1d_t> polyfit_weight)
{
    if (!(look_ang_near > 0.0 && look_ang_far > 0.0 && look_ang_prec > 0.0))
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "All look angles values must be positive numbers!");
    if (look_ang_near >= (look_ang_far - look_ang_prec))
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Near-range look angle shall be smaller than "
                "far one by at least one prec!");

    const auto ang_size = static_cast<int>(
            std::round((look_ang_far - look_ang_near) / look_ang_prec) + 1);
    auto look_ang = isce3::core::Linspace<double>::from_interval(
            look_ang_near, look_ang_far, ang_size);

    return rollAngleOffsetFromEdge(
            polyfit_echo, polyfit_ant, look_ang, polyfit_weight);
}

}} // namespace isce3::antenna


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <int Scale> struct scaling
{    template <typename T> T operator*(T x) const { return Scale * x; }  };

template <> struct scaling<-1>
{    template <typename T> T operator*(T x) const { return -x; }   };

template <> struct scaling<0>
{    template <typename T> T operator*(T x) const { return T(0); } };

template <> struct scaling<1>
{    template <typename T> T operator*(T x) const { return x; }    };

typedef mtl::dense_vector<int,   mtl::parameters<mtl::col_major, mtl::fixed::dimension<2>, true> > fveci2_type;
typedef mtl::dense_vector<float, mtl::parameters<mtl::col_major, mtl::fixed::dimension<2>, true> > fvecf2_type;

template <typename S, typename V> struct scaled_product2;

template <int S0, int S1>
struct scaling_vector2 : public fveci2_type
{
    typedef scaling<S0>             s0;
    typedef scaling<S1>             s1;
    typedef scaling_vector2<S0, S1> self;

    template <typename Vector>
    scaled_product2<self, Vector> operator*(const Vector& v)
    {
	return scaled_product2<self, Vector>(v);
    }
};

template <typename S, typename V>
struct scaled_product2 : public fvecf2_type
{
    explicit scaled_product2(const V& v) : v(v) {}

    typedef typename mtl::Collection<V>::value_type value_type;
    typedef typename mtl::Collection<V>::size_type  size_type;

    value_type operator[](size_type i)
    {
	return i == 0 ? typename S::s0() * v[0] : typename S::s1() * v[1];
    }

    // template <int I> value_type at() { return S::at<I>::type() * v[I]; }

    const V& v;
};

struct integrator
{
    /* const */ static fvecf2_type s0;
};

float s0a[2]= {1.0f, -1.0f};
fvecf2_type integrator::s0(s0a); // (*this)[0]= };

int main(int argc, char** argv)
{
    scaling<-1> c;
    scaling<1>  d;

    std::cout << "c * 3.0 + d * 4.5f = " << c * 3.0 + d * 4.5f << '\n';

    fvecf2_type v, w, z;
    v= 3.0, 4.5;
    w= 2.0, 7.5;
    std::cout << "v = " << v << ", w = " << w << '\n';
#if 0
    scaling_vector2<-1, 1> s0;
    scaling_vector2< 1, 0> s1;

    z= s0 * v; // + s1 * w;
    std::cout << "z = " << z << '\n';
#endif
    return 0;
}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 * Created on November 10, 2020, 10:34 AM
 */

#include <gtest/gtest.h>

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

#include "MaterialLib/MPL/Medium.h"
#include "MaterialLib/MPL/Properties/CreateStrainDependentPermeability.h"
#include "MaterialLib/MPL/Properties/StrainDependentPermeability.h"
#include "MaterialLib/MPL/Utils/FormEigenTensor.h"
#include "ParameterLib/ConstantParameter.h"
#include "TestMPL.h"
#include "Tests/TestTools.h"

TEST(MaterialPropertyLib, StrainDependentPermeability)
{
    ParameterLib::ConstantParameter<double> const k0("k0", 1.e-20);
    double const b1 = 4000.0;
    double const b2 = 300.0;
    double const b3 = 50.0;
    double const min_permeability = 1.e-22;
    double const max_permeability = 1.e-10;

    auto const k_model = MPL::StrainDependentPermeability<3>(
        "k_strain", k0, b1, b2, b3, min_permeability, max_permeability,
        nullptr);

    ParameterLib::SpatialPosition const pos;
    double const t = std::numeric_limits<double>::quiet_NaN();
    double const dt = std::numeric_limits<double>::quiet_NaN();
    MPL::VariableArray vars;

    /// Extensive volumetric strain, which increases permeability.
    {
        double const e_vol = 1.e-4;
        double const e_vol_pls = 1.e-5;

        vars[static_cast<int>(MPL::Variable::volumetric_strain)] = e_vol;
        vars[static_cast<int>(MPL::Variable::equivalent_plastic_strain)] =
            e_vol_pls;
        auto const k = MPL::formEigenTensor<3>(k_model.value(vars, pos, t, dt));

        double const k_expected = 1.0528627949453586e-20;

        ASSERT_LE(std::fabs(k_expected - k(0, 0)) / k_expected, 1e-10)
            << "for expected increased permeability " << k_expected
            << " and for computed increased permeability " << k(0, 0);
    }
    /// Compact volumetric strain, which decreases permeability.
    {
        double const e_vol = -1.e-4;
        double const e_vol_pls = 1.e-5;

        vars[static_cast<int>(MPL::Variable::volumetric_strain)] = e_vol;
        vars[static_cast<int>(MPL::Variable::equivalent_plastic_strain)] =
            e_vol_pls;
        auto const k = MPL::formEigenTensor<3>(k_model.value(vars, pos, t, dt));

        double const k_expected = 9.7134113133089206e-21;

        ASSERT_LE(std::fabs(k_expected - k(0, 0)) / k_expected, 1e-10)
            << "for expected decreased permeability " << k_expected
            << " and for computed decreased permeability " << k(0, 0);
    }
}