/*
Copyright 2019 Association for the promotion of open - source insurance software and for the establishment of open interface standards in the insurance industry

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http ://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include <memory>
#include <boost/program_options.hpp>
#include <boost/test/included/unit_test_framework.hpp>
#include <boost/test/unit_test.hpp>
#include <log4cplus/configurator.h>
#include <log4cplus/logger.h>
#include <calculusprime/cache/DefaultCacheFactory.h>
#include <calculusprime/cache/DefaultFunctionResultCache.h>
#include <calculusprime/cache/DefaultParseTreeCache.h>
#include <calculusprime/internal/defs.h>
#include <calculusprime/internal/parsing/Function.h>
#include <calculusprime/internal/parsing/Parser.h>
#include <calculusprime/internal/parsing/ParsingException.h>
#include <calculusprime/internal/parsing/ValueHolder.h>
#include <calculusprime/internal/ParsingContext.h>
#include <calculusprime/internal/RatingEngine.h>
#include <calculusprime/RatingEngineException.h>
#include <calculusprime/Value.h>

using boost::unit_test_framework::test_suite;
using namespace CalculusPrime;

static const std::string g_dummyVtarKey("ee3812d0-8cf1-11e9-b475-0800200c9a66");
static const std::string g_dummy("dummy");

void testCalculation();
void testInstanceCalculation();
void testCalculationPerformance();

namespace {
std::string getUniqueName()
{
    static std::size_t counter = 0;
    return g_dummy + std::to_string(counter++);
}

void testParser(const std::shared_ptr<ParsingContext>& parsingContext_, const std::string& formula_, const Value& expected_)
{
    Parser parser(parsingContext_);
    Value value = parser.parse(getUniqueName(), formula_);
    BOOST_CHECK_EQUAL(expected_, value);
}

void assignParamToParsingContext(const std::shared_ptr<ParsingContext>& parsingContext_, const std::string& variableName_, const Value::Variant_t& variant_)
{
    std::shared_ptr<ValueHolder::ValueMap_t> valueMap(std::make_shared<ValueHolder::ValueMap_t>());
    (*valueMap)[std::string()] = Value(variant_);
    parsingContext_->assignParam(variableName_, std::make_shared<ValueHolder>(valueMap));
}

std::unique_ptr<Parser> getSimpleParser()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(DefaultCacheFactory::getFunctionResultCache(g_dummyVtarKey));
    std::shared_ptr<IParseTreeCache> parseTreeCache(DefaultCacheFactory::getParseTreeCache(g_dummyVtarKey));
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    return std::make_unique<Parser>(parsingContext);
}

std::unique_ptr<Parser> getParser(const std::string& variableName_, const Value::Variant_t& variant_)
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(DefaultCacheFactory::getFunctionResultCache(g_dummyVtarKey));
    std::shared_ptr<IParseTreeCache> parseTreeCache(DefaultCacheFactory::getParseTreeCache(g_dummyVtarKey));
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));
    assignParamToParsingContext(parsingContext, variableName_, variant_);

    return std::make_unique<Parser>(parsingContext);
}

void testParser(const std::string& formula_, const Value& expected_)
{
    Value value = getSimpleParser()->parse(getUniqueName(), formula_);
    BOOST_CHECK_EQUAL(expected_, value);
}

void testParser(const std::string& variableName_, const Value::Variant_t& variant_, const std::string& formula_, const Value& expected_)
{
    Value value = getParser(variableName_, variant_)->parse(getUniqueName(), formula_);
    BOOST_CHECK_EQUAL(expected_, value);
}

void testParserExpectRatingengineErrorType(const std::string& formula_, const RatingEngineError& expectedErrorType_)
{
    try {
        testParser(formula_, Value("dummy-expected exception!"));
        BOOST_ERROR("this code shouldn't have been reached");
    }
    catch (const RatingEngineException& ex) {
        BOOST_CHECK_EQUAL(ex.getErrorType(), expectedErrorType_);
    }
}

template <class ExceptionType>
void testParserExpectException(const std::string& formula_)
{
    BOOST_CHECK_THROW(testParser(formula_, Value("dummy-expected exception!")), ExceptionType);
}

void testInvalidDates(const std::string& functionName_)
{
    testParserExpectException<ParsingException>("return " + functionName_ + "('asdf')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-01')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('aaaa-01-01')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-bb-cc')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-01-00')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-01-99')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-13-01')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-00-01')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('0000-01-01')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('201-01-01')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-1-01')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016-01-1')");
    testParserExpectException<ParsingException>("return " + functionName_ + "('2016 01 01')");
    // 2015-02-29 is not a leap year
    testParserExpectException<ParsingException>("return " + functionName_ + "('2015-02-29')");
}
}

void testValue()
{
    Value v1;
    BOOST_CHECK(v1.isVoid());

    std::string s;
    Value v2(s);
    BOOST_CHECK(!v2.isVoid());
    BOOST_CHECK(!v2.isBool());
    BOOST_CHECK(v2.isString());
    BOOST_CHECK(!v2.isDouble());

    bool b = true;
    Value v3(b);
    BOOST_CHECK(!v3.isVoid());
    BOOST_CHECK(v3.isBool());
    BOOST_CHECK(!v3.isString());
    BOOST_CHECK(!v3.isDouble());

    double d = 123.45;
    Value v4(d);
    BOOST_CHECK(!v4.isVoid());
    BOOST_CHECK(!v4.isBool());
    BOOST_CHECK(!v4.isString());
    BOOST_CHECK(v4.isDouble());
}

/**
* test if then else expression
*/
void testIfThenElseExpression()
{
    const std::string formula = "if (value1=1.0) then return 'A' else if (value1=2.0) then return 'B' else return 'C' end";

    testParser("value1", 1.0, formula, Value("A"));
    testParser("value1", 2.0, formula, Value("B"));
    testParser("value1", 3.0, formula, Value("C"));
}

/**
* test add expression
*/
void testAddExpression()
{
    testParser("return 2+2", Value(4.0));
    testParser("return 2+1", Value(3.0));
    testParser("return 2+0", Value(2.0));
    testParser("return 0+0", Value(0.0));
    testParser("return -0+0", Value(0.0));
    testParser("return -2+1", Value(-1.0));
    testParser("return 4+0.5", Value(4.5));
}

/**
* test subtract expression
*/
void testSubtractExpression()
{
    testParser("return 2-2", Value(0.0));
    testParser("return 2-1", Value(1.0));
    testParser("return 2-0", Value(2.0));
    testParser("return 4-0.5", Value(3.5));
    testParser("return 0-0", Value(0.0));
    testParser("return -0-0", Value(0.0));
    testParser("return -2-1", Value(-3.0));
}

/**
* test multiply expression
*/
void testMultiplyExpression()
{
    testParser("return 2*2", Value(4.0));
    testParser("return 2*1", Value(2.0));
    testParser("return 2*0", Value(0.0));
    testParser("return 2*-1", Value(-2.0));
    testParser("return 4*0.5", Value(2.0));
    testParser("return -2*2", Value(-4.0));
    testParser("return -2*-2", Value(4.0));
}

/**
* test divide expression
*/
void testDivideExpression()
{
    testParser("return 2/2", Value(1.0));
    testParser("return 2/1", Value(2.0));
    testParser("return 2/-1", Value(-2.0));
    testParser("return 4/0.5", Value(8.0));
    testParser("return -2/2", Value(-1.0));
    testParser("return -2/-2", Value(1.0));

    testParserExpectRatingengineErrorType("return 2/0", RatingEngineError::DIVISION_BY_ZERO);
}

/**
* test power expression
*/
void testPowerExpression()
{
    testParser("return 2^2", Value(4.0));
    testParser("return 2^1", Value(2.0));
    testParser("return 2^0", Value(1.0));
    testParser("return 2^-1", Value(0.5));
    testParser("return 4^0.5", Value(2.0));
}

/**
* test modulo expression
*/
void testModuloExpression()
{
    testParser("return 10 mod 7", Value(3.0));
    testParser("return 20 mod 7", Value(6.0));
    testParser("return 10 mod 10", Value(0.0));

    testParserExpectRatingengineErrorType("return 10 mod 0", RatingEngineError::DIVISION_BY_ZERO);
}

/**
* test round function
*/
void testRoundFunction()
{
    testParser("return rnd(3.456, 0)", Value(3.0));
    testParser("return rnd(3.456, 1)", Value(3.5));
    testParser("return rnd(3.456, 2)", Value(3.46));
    testParser("return rnd(333.456, -1)", Value(330.0));
    testParser("return rnd(333.456, -2)", Value(300.0));
}

/**
* test min function
*/
void testMinFunction()
{
    testParser("return min(1.0, 2.0)", Value(1.0));
    testParser("return min(1.0, 1.0)", Value(1.0));
    testParser("return min(2.0, 1.0)", Value(1.0));
    testParser("return min(1.0, -2.0)", Value(-2.0));
    testParser("return min(-2.0, 1.0)", Value(-2.0));
    testParser("return min(-2.0, -1.0)", Value(-2.0));
    testParser("return min(-1.0, -2.0)", Value(-2.0));
}

/**
* test error function
*/
void testErrorFunction()
{
    try {
        testParser("if (1 = 1) then error(123) else return 0.0 end", Value(123.0));
        BOOST_ERROR("this code shouldn't have been reached");
    }
    catch (RatingEngineException ex) {
        BOOST_CHECK_EQUAL(ex.getErrorType(), RatingEngineError::CALCULATION_CANCELLED);
        BOOST_CHECK_EQUAL("123", ex.getAddOn());
    }
}

/**
* test max function
*/
void testMaxFunction()
{
    testParser("return max(1.0, 2.0)", Value(2.0));
    testParser("return max(1.0, 1.0)", Value(1.0));
    testParser("return max(2.0, 1.0)", Value(2.0));
    testParser("return max(1.0, -2.0)", Value(1.0));
    testParser("return max(-2.0, 1.0)", Value(1.0));
    testParser("return max(-2.0, -1.0)", Value(-1.0));
    testParser("return max(-1.0, -2.0)", Value(-1.0));
}

/**
* test day function
*/
void testDayFunction()
{
    testParser("return day('2016-01-20')", Value(20.0));
    testParser("return day('0001-01-01')", Value(1.0));
    testParser("return day('2999-12-31')", Value(31.0));
    // leap year
    testParser("return day('2016-02-29')", Value(29.0));

    testInvalidDates("day");
}

/**
* test date functions in Austria for year 1980 (summer time started on 5.4. 23:59:59 with a leap to 6.4. 01:00. The time 00:00 did not exist)
*/
void testDateFunctionsAustriaYear1980()
{
    testParser("return day('1980-04-06')", Value(6));
    testParser("return month('1980-04-06')", Value(4));
    testParser("return year('1980-04-06')", Value(1980));
}

/**
* test month function
*/
void testMonthFunction()
{
    testParser("return month('2016-01-20')", Value(1.0));
    testParser("return month('0001-01-01')", Value(1.0));
    testParser("return month('2999-12-31')", Value(12.0));
    // leap year
    testParser("return month('2016-02-29')", Value(2.0));

    testInvalidDates("month");
}

/**
* test year function
*/
void testYearFunction()
{
    testParser("return year('2016-01-20')", Value(2016.0));
    testParser("return year('0001-01-01')", Value(1.0));
    testParser("return year('2999-12-31')", Value(2999.0));
    testParser("return year('9999-12-31')", Value(9999.0));
    // leap year
    testParser("return year('2016-02-29')", Value(2016.0));

    testInvalidDates("year");
}

/**
* test substr function
*/
void testSubstrFunction()
{
    testParser("return substr('abcdefg',1,1)", Value("a"));
    testParser("return substr('abcdefg',1,7)", Value("abcdefg"));
    testParser("return substr('abcdefg',1,99)", Value("abcdefg"));
    testParser("return substr('abcdefg',2,6)", Value("bcdefg"));
    testParser("return substr('abcdefg',2,2)", Value("bc"));
    testParserExpectException<ParsingException>("return substr('abcdefg',2,-1)");
    testParserExpectException<ParsingException>("return substr('abcdefg',1,0)");
    testParserExpectException<ParsingException>("return substr('abcdefg',0,6)");
    testParserExpectException<ParsingException>("return substr('abcdefg',-1,6)");
    testParserExpectException<ParsingException>("return substr('abcdefg',99,6)");
}

/**
* test line comments
*/
void testLineComments()
{
    testParser("return //comment \n substr('abcdefg',1,1)", Value("a"));
    testParser("return substr('abcdefg', //comment\n 1,1)", Value("a"));
    testParser("return //asdf \n substr('abcdefg',//\n 1,1)", Value("a"));
    testParser("return substr('ab//cdefg',1,9)", Value("ab//cdefg"));
    testParser("return substr('ab//cdefg',1,9)\n", Value("ab//cdefg"));
    testParser("return // /*comment \n substr('abcdefg',1,1)", Value("a"));
    testParser("return substr('abcdefg',1,1) \n//asdf", Value("a"));
    testParser("return 1//*2", Value(1.0));
}

/**
* test comments
*/
void testComments()
{
    testParser("return /* comment */ substr('abcdefg',1,1)", Value("a"));
    testParser("return substr('abcdefg', /*comment*/1,1)", Value("a"));
    testParser("return /* asdf \n comment */ substr('abcdefg', /*\n*/ 1,1)", Value("a"));
    testParser("return substr('ab/*cd*/g',1,9)", Value("ab/*cd*/g"));
    testParser("return /* //comment */ substr('abcdefg',1,1)", Value("a"));
}

/**
* test number to string conversion
*/
void testNumberToStringConversion()
{
    testParser("return 'A'+50.0", Value("A50"));
    testParser("return 'A'+50.5", Value("A50.5"));
    testParser("return 'A'+0", Value("A0"));
    testParser("return 'A'+0.0", Value("A0"));
    testParser("return 'A'+50.120", Value("A50.12"));
}

/**
* tests function defintion and calls
*/
void testSimpleFunction()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(std::make_shared<DefaultFunctionResultCache>());
    std::shared_ptr<IParseTreeCache> parseTreeCache(std::make_shared<DefaultParseTreeCache>());
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    assignParamToParsingContext(parsingContext, "faktor", 0.8);

    // argument names non case sensitive (input / Input)
    parsingContext->addFunction(std::make_shared<Function>("F_Rabatt(input)", "return Input*faktor"));

    testParser(parsingContext, "return F_Rabatt(1000)", Value(800.0));
    testParser(parsingContext, "return F_Rabatt(faktor)", Value(0.64));
    // function name non case sensitive
    testParser(parsingContext, "return f_rabatt(faktor)", Value(0.64));
}

/**
* tests handling of multiple functions with different signatures
*/
void testFunctionsWithSameNameAndDifferentSignatures()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(std::make_shared<DefaultFunctionResultCache>());
    std::shared_ptr<IParseTreeCache> parseTreeCache(std::make_shared<DefaultParseTreeCache>());
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    assignParamToParsingContext(parsingContext, "faktor", 0.8);

    parsingContext->addFunction(std::make_shared<Function>("F_Rabatt(input)", "return input*faktor"));
    parsingContext->addFunction(std::make_shared<Function>("F_Rabatt(input, input2)", "return input*input2*faktor"));

    testParser(parsingContext, "return F_Rabatt(1000)", Value(800.0));
    testParser(parsingContext, "return F_Rabatt(2000, 10)", Value(16000.0));
}

/**
* tests that a function can call other functions
*/
void testFunctionCallsOtherFunction()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(std::make_shared<DefaultFunctionResultCache>());
    std::shared_ptr<IParseTreeCache> parseTreeCache(std::make_shared<DefaultParseTreeCache>());
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    assignParamToParsingContext(parsingContext, "faktor", 0.8);

    parsingContext->addFunction(std::make_shared<Function>("F_Rabatt(input)", "return input*faktor"));
    parsingContext->addFunction(std::make_shared<Function>("F_Rabatt(input, input2)", "return F_Rabatt(input)*input2+input"));

    testParser(parsingContext, "return F_Rabatt(2000, 10)", Value(18000.0));
}

/**
* tests scope of function arguments
*/
void testFunctionArgumentScope()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(std::make_shared<DefaultFunctionResultCache>());
    std::shared_ptr<IParseTreeCache> parseTreeCache(std::make_shared<DefaultParseTreeCache>());
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    assignParamToParsingContext(parsingContext, "faktor", 0.8);

    // input2 is not visible here
    parsingContext->addFunction(std::make_shared<Function>("F_Rabatt(input)", "return input2*faktor"));
    parsingContext->addFunction(std::make_shared<Function>("F_Rabatt(input, input2)", "return F_Rabatt(input)*input2+input"));

    BOOST_CHECK_THROW(testParser(parsingContext, "return F_Rabatt(2000, 10)", Value(18000.0)), RatingEngineException);
}

/**
* tests a recursive function call
*/
void testRecursiveFunctionCall()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(std::make_shared<DefaultFunctionResultCache>());
    std::shared_ptr<IParseTreeCache> parseTreeCache(std::make_shared<DefaultParseTreeCache>());
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    parsingContext->addFunction(std::make_shared<Function>("factorial(input)", "if (input = 1) then return 1 else return input*factorial(input-1) end"));

    testParser(parsingContext, "return factorial(5)", Value(120.0));
}

/**
* tests bool expressions
*/
void testBoolExpression()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(std::make_shared<DefaultFunctionResultCache>());
    std::shared_ptr<IParseTreeCache> parseTreeCache(std::make_shared<DefaultParseTreeCache>());
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    parsingContext->addFunction(std::make_shared<Function>("testFunction(input)", "if (input) then return 'yes' else return 'no' end"));
    parsingContext->addFunction(std::make_shared<Function>("testFunction2(input)", "if (input = true) then return 'yes' else return 'no' end"));
    parsingContext->addFunction(std::make_shared<Function>("testFunction3(input)", "if (input = 'yes') then return true else return false end"));

    testParser(parsingContext, "return testFunction(true)", Value("yes"));
    testParser(parsingContext, "return testFunction(false)", Value("no"));

    testParser(parsingContext, "return testFunction2(true)", Value("yes"));
    testParser(parsingContext, "return testFunction2(false)", Value("no"));

    testParser(parsingContext, "return testFunction3('yes')", Value(true));
    testParser(parsingContext, "return testFunction3('no')", Value(false));
}

/**
* tests string expressions
*/
void testStringExpression()
{
    std::shared_ptr<IFunctionResultCache> functionResultCache(std::make_shared<DefaultFunctionResultCache>());
    std::shared_ptr<IParseTreeCache> parseTreeCache(std::make_shared<DefaultParseTreeCache>());
    std::shared_ptr<ParsingContext> parsingContext(std::make_shared<ParsingContext>(std::vector<std::shared_ptr<IFunction>>(), functionResultCache, parseTreeCache, g_dummyVtarKey, log4cplus::Logger::getRoot()));

    parsingContext->addFunction(std::make_shared<Function>("testFunction(input)", "if (input) then return '1' else return '0' end"));

    testParser(parsingContext, "return testFunction(true)", Value("1"));
    testParser(parsingContext, "return testFunction(false)", Value("0"));
}

/**
* test ceil function
*/
void testCeilFunction()
{
    testParser("return ceil(2.3)", Value(3.0));
    testParser("return ceil(3.8)", Value(4.0));
    testParser("return ceil(0.0)", Value(0.0));
    testParser("return ceil(-2.3)", Value(-2.0));
    testParser("return ceil(-3.8)", Value(-3.0));
}

/**
* test floor function
*/
void testFloorFunction()
{
    testParser("return floor(2.3)", Value(2.0));
    testParser("return floor(3.8)", Value(3.0));
    testParser("return floor(0.0)", Value(0.0));
    testParser("return floor(-2.3)", Value(-3.0));
    testParser("return floor(-3.8)", Value(-4.0));
}

/**
* test exp function
*/
void testExpFunction()
{
    testParser("return rnd(exp(5.0),6)", Value(148.413159));
    testParser("return rnd(exp(1.0),6)", Value(2.718282));
    testParser("return rnd(exp(-1.0),6)", Value(0.367879));
    // overflow
    testParserExpectException<ParsingException>("return exp(999999.0)");
}

test_suite* init_unit_test_suite(int argc, char* argv[])
{
    namespace bpo = boost::program_options;

    log4cplus::initialize();

    // Programmparameter
    std::string properties;

    bpo::options_description desc("test_calculusprime options");
    desc.add_options()
    ("properties,p", bpo::value<std::string>(&properties), "properties")
    ;

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

    if (!properties.empty()) {
        log4cplus::PropertyConfigurator::doConfigure(properties);
    }
    else {
        log4cplus::Logger::getRoot().setLogLevel(log4cplus::WARN_LOG_LEVEL);
        log4cplus::Logger::getInstance(LOGGER_CALCULUSPRIME).setLogLevel(log4cplus::WARN_LOG_LEVEL);
    }

    test_suite* test = BOOST_TEST_SUITE("Calculusprime Test Suite");

    test->add(BOOST_TEST_CASE(&testValue));
    test->add(BOOST_TEST_CASE(&testCalculation));
    test->add(BOOST_TEST_CASE(&testInstanceCalculation));
    test->add(BOOST_TEST_CASE(&testCalculationPerformance));
    test->add(BOOST_TEST_CASE(&testIfThenElseExpression));
    test->add(BOOST_TEST_CASE(&testAddExpression));
    test->add(BOOST_TEST_CASE(&testSubtractExpression));
    test->add(BOOST_TEST_CASE(&testMultiplyExpression));
    test->add(BOOST_TEST_CASE(&testDivideExpression));
    test->add(BOOST_TEST_CASE(&testPowerExpression));
    test->add(BOOST_TEST_CASE(&testModuloExpression));
    test->add(BOOST_TEST_CASE(&testRoundFunction));
    test->add(BOOST_TEST_CASE(&testCeilFunction));
    test->add(BOOST_TEST_CASE(&testFloorFunction));
    test->add(BOOST_TEST_CASE(&testExpFunction));
    test->add(BOOST_TEST_CASE(&testMinFunction));
    test->add(BOOST_TEST_CASE(&testErrorFunction));
    test->add(BOOST_TEST_CASE(&testMaxFunction));
    test->add(BOOST_TEST_CASE(&testDayFunction));
    test->add(BOOST_TEST_CASE(&testDateFunctionsAustriaYear1980));
    test->add(BOOST_TEST_CASE(&testMonthFunction));
    test->add(BOOST_TEST_CASE(&testYearFunction));
    test->add(BOOST_TEST_CASE(&testSubstrFunction));
    test->add(BOOST_TEST_CASE(&testLineComments));
    test->add(BOOST_TEST_CASE(&testComments));
    test->add(BOOST_TEST_CASE(&testNumberToStringConversion));
    test->add(BOOST_TEST_CASE(&testSimpleFunction));
    test->add(BOOST_TEST_CASE(&testFunctionsWithSameNameAndDifferentSignatures));
    test->add(BOOST_TEST_CASE(&testFunctionCallsOtherFunction));
    test->add(BOOST_TEST_CASE(&testFunctionArgumentScope));
    test->add(BOOST_TEST_CASE(&testRecursiveFunctionCall));
    test->add(BOOST_TEST_CASE(&testBoolExpression));
    test->add(BOOST_TEST_CASE(&testStringExpression));

    return test;
}


/*
 * (C) British Crown Copyright 2021 Met Office
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include "ufo/filters/ImpactHeightCheck.h"

#include <Eigen/Core>
#include <iomanip>
#include <iostream>
#include <limits>
#include <vector>

#include "ioda/ObsDataVector.h"
#include "ioda/ObsSpace.h"

#include "oops/util/Logger.h"
#include "ufo/filters/QCflags.h"

namespace ufo {

// -----------------------------------------------------------------------------
/// ImpactHeightCheck: Calculate the impact height for model profiles, and reject
/// any observations which are outside the range of model impact heights.  Check
/// for any sharp refractivity gradients, and reject any observations below them.

ImpactHeightCheck::ImpactHeightCheck(
        ioda::ObsSpace & obsdb,
        const Parameters_ & parameters,
        std::shared_ptr<ioda::ObsDataVector<int> > flags,
        std::shared_ptr<ioda::ObsDataVector<float> > obserr)
  : FilterBase(obsdb, parameters, flags, obserr), parameters_(parameters)
{
  oops::Log::trace() << "ImpactHeightCheck constructor" << std::endl;
  allvars_ += Variable("refractivity@ObsDiag");
  allvars_ += Variable("model_heights@ObsDiag");
  allvars_ += Variable("impact_parameter@MetaData");
  allvars_ += Variable("earth_radius_of_curvature@MetaData");
}

// -----------------------------------------------------------------------------

ImpactHeightCheck::~ImpactHeightCheck() {
  oops::Log::trace() << "ImpactHeightCheck destructed" << std::endl;
}

// -----------------------------------------------------------------------------
/// Apply the filter: remove observations which are outside the range of
/// impact heights

void ImpactHeightCheck::applyFilter(const std::vector<bool> & apply,
                                    const Variables & filtervars,
                                    std::vector<std::vector<bool>> & flagged) const {
  oops::Log::trace() << "ImpactHeightCheck post-filter" << std::endl;
  const oops::Variables observed = obsdb_.obsvariables();
  const float missingFloat = util::missingValue(missingFloat);

  // Get the refractivity from the obs diagnostics, including the number of
  // vertical levels on which the refractivity has been calculated (nRefLevels)
  Variable refractivityVariable = Variable("refractivity@ObsDiag");
  oops::Log::debug() << data_.nlevs(refractivityVariable) << std::endl;
  const size_t nRefLevels = data_.nlevs(refractivityVariable);
  std::vector<std::vector<float>> refractivity;

  for (size_t iLevel = 0; iLevel < nRefLevels; ++iLevel) {
    std::vector<float> inputData;
    data_.get(refractivityVariable, static_cast<int>(iLevel), inputData);
    refractivity.push_back(inputData);
  }

  // For the benefits of debugging, output the refractivity for the first
  // observation
  oops::Log::debug() << "Refractivity(first ob) ";
  for (size_t iLevel = 0; iLevel < nRefLevels; ++iLevel) {
    oops::Log::debug() << refractivity[iLevel][0] << " ";
  }
  oops::Log::debug() << std::endl;

  // Get the height of the levels on which the refractivity has been calculated.
  // Must be the same length as the array defining the refractivity.
  Variable modelHeightsVariable = Variable("model_heights@ObsDiag");
  oops::Log::debug() << data_.nlevs(modelHeightsVariable) << std::endl;
  if (data_.nlevs(modelHeightsVariable) != nRefLevels) {
    throw eckit::BadValue("Model heights and refractivity must have the same number of levels",
                          Here());
  }
  std::vector<std::vector<float>> modelHeights;

  // Read the heights of the refractivity levels
  for (size_t iLevel = 0; iLevel < nRefLevels; ++iLevel) {
    std::vector<float> inputData;
    data_.get(modelHeightsVariable, static_cast<int>(iLevel), inputData);
    modelHeights.push_back(inputData);
  }

  // For debugging, output the heights of the refractivity levels for the first
  // observation.
  oops::Log::debug() << "Model heights (first ob) ";
  for (size_t iLevel = 0; iLevel < nRefLevels; ++iLevel) {
    oops::Log::debug() << modelHeights[iLevel][0] << " ";
  }
  oops::Log::debug() << std::endl;

  // Read in the observation impact parameter for each observation
  Variable impactVariable = Variable("impact_parameter@MetaData");
  std::vector<float> impactParameter;
  data_.get(impactVariable, impactParameter);

  // Read in the earth's radius of curvature for each observation
  Variable radiusCurvatureParameter = Variable("earth_radius_of_curvature@MetaData");
  std::vector<float> radiusCurvature;
  data_.get(radiusCurvatureParameter, radiusCurvature);

  // For each variable, perform the filter
  for (size_t iFilterVar = 0; iFilterVar < filtervars.nvars(); ++iFilterVar) {
    const size_t iVar = observed.find(filtervars.variable(iFilterVar).variable());

    // Loop over the observations
    for (size_t iObs = 0; iObs < obsdb_.nlocs(); ++iObs) {
      if (apply[iObs] && (*flags_)[iVar][iObs] == QCflags::pass) {
        // Load the refractivity profile and the associated model heights for this observation,
        // cleansing any missing data
        std::vector<float> refracProfile;
        std::vector<float> heightProfile;
        for (size_t iLevel = 0; iLevel < nRefLevels; ++iLevel)
          if (refractivity[iLevel][iObs] != missingFloat &&
              modelHeights[iLevel][iObs] != missingFloat) {
            refracProfile.push_back(refractivity[iLevel][iObs]);
            heightProfile.push_back(modelHeights[iLevel][iObs]);
          }

        if (refracProfile.size() < 2) {
          oops::Log::error() << "Should have at least two valid points in every profile:" <<
                                std::endl << "size = " << refracProfile.size() << "  " <<
                                "iObs = " << iObs << std::endl;
          flagged[iFilterVar][iObs] = true;
          continue;
        }

        oops::Log::debug() << "Min and max refrac for profile " << iObs <<
                              " is " << refracProfile[0] << " to " << refracProfile.back() <<
                              std::endl;
        oops::Log::debug() << "Impact height " << impactParameter[iObs] - radiusCurvature[iObs] <<
                              std::endl;

        // Output the calculated refractivity gradient for the first observation
        const std::vector<float> & gradient = calcVerticalGradient(refracProfile, heightProfile);
        if (iObs == 0) {
            oops::Log::debug() << "Gradient found to be" << std::endl;
            for (float grad : gradient)
                oops::Log::debug() << grad << "  ";
            oops::Log::debug() << std::endl;
        }

        float sharpGradientImpact = std::numeric_limits<float>::lowest();
        // Search for sharp gradients (super-refraction) starting at the top of the profile
        for (int iLevel = static_cast<int>(nRefLevels)-2; iLevel >= 0; --iLevel) {
          size_t thisLevel = static_cast<size_t>(iLevel);
          if (gradient[thisLevel] != missingFloat &&
              gradient[thisLevel] < parameters_.gradientThreshold.value()) {
            sharpGradientImpact = calcImpactHeight(refracProfile[thisLevel],
                                                   heightProfile[thisLevel],
                                                   radiusCurvature[iObs]);
            oops::Log::info() << "Sharp refractivity gradient of " << gradient[thisLevel] <<
                                 " found at " << thisLevel << "  " << sharpGradientImpact <<
                                 std::endl;
            oops::Log::debug() << thisLevel << "   " << refracProfile[thisLevel] << "   " <<
                                  heightProfile[thisLevel] << "   " <<
                                  radiusCurvature[thisLevel] << std::endl;
            break;
          }
        }

        // Reject observation if it is below the minimum (either surface or sharp gradient)
        const float obsImpactHeight = impactParameter[iObs] - radiusCurvature[iObs];
        oops::Log::debug() << "Checking minimum height " << obsImpactHeight << "   " <<
                              sharpGradientImpact + parameters_.sharpGradientOffset.value() <<
                              "   " << calcImpactHeight(refracProfile[0], heightProfile[0],
                                                        radiusCurvature[iObs]) +
                              parameters_.surfaceOffset.value() << std::endl;
        if (obsImpactHeight < sharpGradientImpact + parameters_.sharpGradientOffset.value() ||
            obsImpactHeight < calcImpactHeight(refracProfile[0], heightProfile[0],
                                               radiusCurvature[iObs]) +
                                               parameters_.surfaceOffset.value())
          flagged[iFilterVar][iObs] = true;

        // Reject observation if it is above the maximum
        oops::Log::debug() << "Checking maximum height " << obsImpactHeight << "   " <<
                              calcImpactHeight(refracProfile.back(), heightProfile.back(),
                                               radiusCurvature[iObs]) << std::endl;
        if (obsImpactHeight > calcImpactHeight(refracProfile.back(), heightProfile.back(),
                                               radiusCurvature[iObs]))
          flagged[iFilterVar][iObs] = true;
      }
    }
  }
}

// -----------------------------------------------------------------------------
/// Calculate the vertical gradient of refractivity, assuming that any missing
/// data has already been removed
std::vector<float> ImpactHeightCheck::calcVerticalGradient(
        const std::vector<float> & refrac,
        const std::vector<float> & height) const {
  std::vector<float> gradient;
  for (size_t iLevel = 0; iLevel < refrac.size()-1; ++iLevel) {
    gradient.push_back((refrac[iLevel+1] - refrac[iLevel]) / (height[iLevel+1] - height[iLevel]));
  }
  return gradient;
}

// -----------------------------------------------------------------------------
/// Calculate the impact parameter from the refractivity, impact height and
/// radius of curvature
float ImpactHeightCheck::calcImpactHeight(float refractivity,
                                          float modelHeight,
                                          float radiusCurv) const {
  return 1.0E-6f * refractivity * (radiusCurv + modelHeight) + modelHeight;
}

// -----------------------------------------------------------------------------
void ImpactHeightCheck::print(std::ostream & os) const {
  os << "ImpactHeightCheck: config = " << parameters_ << std::endl;
}

// -----------------------------------------------------------------------------

}  // namespace ufo


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/polynomials/include/functions/polevl.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/bench/benchmark.hpp>

template<typename T> NT2_EXPERIMENT(polevl_bench)
{
  public:
  polevl_bench(std::size_t s) : NT2_EXPRIMENT_CTOR(1,"cycles/elements"), size(s)
  {
    coeff[0] = T(2); coeff[1] = T(3); coeff[2] = T(4);
  }

  virtual void run() const
  {
    for(int i=0;i<size;++i) out[i] = nt2::polevl(in[i], coeff);
  }

  virtual double compute(nt2::benchmark_result_t const& r) const
  {
    return r.first/double(size);
  }

  virtual void info(std::ostream& os) const { os << size; }

  virtual void reset() const
  {
    in.resize(size);
    out.resize(size);
    nt2::roll(in,-10,10);
  }

  private:
  int       size;
  mutable   std::vector<T>  in,out;
  boost::array<T,3>         coeff;
};

NT2_RUN_EXPERIMENT_TPL( polevl_bench, (float)(double), (1024) );


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file TableFactor.cpp
 * @brief disrete factor
 * @date May 27, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/discrete/TableFactor.h>

#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <utility>

using namespace std;

namespace gtsam {

/* ************************************************************************ */
TableFactor::TableFactor(){};

/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& keys,
                         const Eigen::SparseVector<double>& table)
    : DiscreteFactor(keys.indices()), sparse_table_(table.size()) {
  sparse_table_ = table;
  size_t denom = table.size();
  for (const DiscreteKey& dkey : keys) {
    cardinalities_.insert(dkey);
    denom /= dkey.second;
    denominator_.insert(std::pair<Key, size_t>(dkey.first, denom));
  }
}

/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(
    const std::vector<double>& table) {
  Eigen::SparseVector<double> sparse_table(table.size());
  // counting number of nonzero elements in table and reserving the space
  const size_t nnz = std::count_if(table.begin(), table.end(),
                                   [](size_t i) { return i != 0; });
  sparse_table.reserve(nnz);
  for (size_t i = 0; i < table.size(); i++) {
    if (table[i] != 0) sparse_table.insert(i) = table[i];
  }
  return sparse_table;
}

/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(const std::string& table) {
  // Convert string to doubles
  std::vector<double> ys;
  std::istringstream iss(table);
  std::copy(std::istream_iterator<double>(iss), std::istream_iterator<double>(),
            std::back_inserter(ys));
  return Convert(ys);
}

/* ************************************************************************ */
bool TableFactor::equals(const DiscreteFactor& other, double tol) const {
  if (!dynamic_cast<const TableFactor*>(&other)) {
    return false;
  } else {
    const auto& f(static_cast<const TableFactor&>(other));
    return (sparse_table_ - f.sparse_table_).sum() == 0;
    // return sparse_table_ == f.sparse_table_;
  }
}

/* ************************************************************************ */
void TableFactor::print(const string& s, const KeyFormatter& formatter) const {
  cout << s;
  cout << " f[";
  for (auto&& key : keys())
    cout << boost::format(" (%1%,%2%),") % formatter(key) % cardinality(key);
  cout << " ]" << endl;
}
/* ************************************************************************ */
double TableFactor::operator()(const DiscreteValues& values) const {
  // Find index of value in O(N) where N is number of keys
  size_t idx = findIndex(values);
  return sparse_table_.coeff(idx);
}

/* ************************************************************************ */
DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
  return toDecisionTreeFactor() * f;
}

/* ************************************************************************ */
TableFactor TableFactor::operator*(const TableFactor& f) const {
  DiscreteKeys keys = unionKeys(f);
  size_t cardinality = 1;
  // Create new TableFactor
  for (DiscreteKey key : keys) cardinality *= key.second;
  Eigen::SparseVector<double> new_sparse_table_(cardinality);
  new_sparse_table_.reserve(sparse_table_.nonZeros() *
                            (cardinality / sparse_table_.size()));
  TableFactor multiplied_f(keys, new_sparse_table_);
  for (Eigen::SparseVector<double>::InnerIterator it(f.sparse_table_); it;
       ++it) {
    DiscreteValues assignment_f;
    for (Key key : f.keys_) {
      assignment_f[key] = f.lazy_cp(key, it.index());
    }
    std::vector<DiscreteValues> projections = project(assignment_f);
    for (DiscreteValues projection : projections) {
      size_t m_index = multiplied_f.findIndex(projection);
      multiplied_f.sparse_table_.insert(m_index) =
          it.value() * operator()(projection);
    }
  }
  return multiplied_f;
}

/* ************************************************************************ */
DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
  DiscreteKeys dkeys = discreteKeys();
  std::vector<double> table;
  for (size_t i = 0; i < sparse_table_.size(); i++) {
    table.push_back(sparse_table_.coeff(i));
  }
  DecisionTreeFactor f(dkeys, table);
  return f;
}

/* ************************************************************************ */
TableFactor::shared_ptr TableFactor::sum(size_t nrFrontals) const {
  // Create map to keep track of summed values
  std::map<size_t, double> cs;
  size_t cardinality = cardinalities_.at(nrFrontals);
  for (size_t i = 0; i < cardinality; i++) cs[i] = 0;
  DiscreteKey target_key = discreteKey(nrFrontals);
  for (Eigen::SparseVector<double>::InnerIterator it(sparse_table_); it; ++it) {
    cs[lazy_cp(target_key.first, it.index())] += it.value();
  }
  std::vector<double> result;
  for (std::map<size_t, double>::iterator it = cs.begin(); it != cs.end();
       ++it) {
    result.push_back(it->second);
  }
  return boost::make_shared<TableFactor>(target_key, result);
}

/* ************************************************************************ */
TableFactor TableFactor::eliminate(const Key key) const {
  // Eliminate the input key from the TableFactor while retaining only the
  // maximum value of the assignment.
  // ex) Given assignments (0, 0, 0) = 3, (1, 0, 0) = 10
  // when first key is eliminated the output should be (0, 0) = 10
  // Collect keys that are not the key to be eliminated
  DiscreteKeys dkeys;
  size_t cardinality = 1;
  for (Key k : keys_) {
    if (k != key) {
      dkeys.push_back(discreteKey(k));
      cardinality *= cardinalities_.at(k);
    }
  }
  // Create new TableFactor with remaining keys
  Eigen::SparseVector<double> new_sparse_table_(cardinality);
  if (sparse_table_.nonZeros() < cardinality)
    new_sparse_table_.reserve(sparse_table_.nonZeros());
  TableFactor eliminated_f(dkeys, new_sparse_table_);
  // Populate the new TableFactor
  for (Eigen::SparseVector<double>::InnerIterator it(sparse_table_); it; ++it) {
    DiscreteValues assignments;
    for (DiscreteKey dkey : dkeys) {
      assignments[dkey.first] = lazy_cp(dkey.first, it.index());
    }
    size_t index = eliminated_f.findIndex(assignments);
    // swap values to store the maximum
    if (it.value() > eliminated_f.sparse_table_.coeff(index)) {
      eliminated_f.sparse_table_.coeffRef(index) = it.value();
    }
  }
  return eliminated_f;
}

/* ************************************************************************ */
TableFactor TableFactor::max(size_t nrFrontals) const {
  if (nrFrontals > size())
    throw invalid_argument(
        (boost::format("TableFactor::max: invalid number of frontal "
                       "keys %d, nr.keys=%d") %
         nrFrontals % size())
            .str());
  // Iteratively eliminate nrFrontals number of keys from this TableFactor
  TableFactor result(*this);
  for (size_t i = 0; i < nrFrontals; i++) {
    Key j = keys()[i];
    result = result.eliminate(j);
  }
  return result;
}

/* ************************************************************************ */
TableFactor TableFactor::max(const Ordering& frontalKeys) const {
  // Create map to keep track of summed values
  if (frontalKeys.size() > size())
    throw invalid_argument(
        (boost::format("TableFactor::max: invalid number of frontal "
                       "keys %d, nr.keys=%d") %
         frontalKeys.size() % size())
            .str());
  // Iteratively eliminate a key according to the ordering (frontalKeys) fron
  // this TableFactor
  TableFactor result(*this);
  for (size_t i = 0; i < frontalKeys.size(); i++) {
    Key j = frontalKeys[i];
    result = result.eliminate(j);
  }
  return result;
}

/* ************************************************************************ */
TableFactor TableFactor::fromDecisionTreeFactor(
    const DecisionTreeFactor& f) const {
  auto enumerated = f.enumerate();
  std::vector<double> table;
  for (size_t i = 0; i < enumerated.size(); i++) {
    table.push_back(enumerated[i].second);
  }
  DiscreteKeys tree_dkeys = f.discreteKeys();
  TableFactor table_f(tree_dkeys, table);
  return table_f;
}

/* ************************************************************************ */
size_t TableFactor::lazy_cp(Key target_key, size_t index) const {
  return (index / denominator_.at(target_key)) % cardinalities_.at(target_key);
}

/* ************************************************************************ */
size_t TableFactor::findIndex(const DiscreteValues& assignment) const {
  // a b c d => D * (C * (B * (a) + b) + c) + d
  // index = a
  // for (i in bcd) :
  //     index = index * I + i
  size_t index = 0;
  for (auto&& key : keys_) {
    if (index) {
      index *= cardinalities_.at(key);
    }
    index += assignment.at(key);
  }
  return index;
}

/* ************************************************************************ */
DiscreteValues TableFactor::maxAssignment() const {
  DiscreteValues assignments;
  Eigen::Index nnz = sparse_table_.nonZeros();
  Eigen::Index maxRowIdx;
  Eigen::VectorXd::Map(sparse_table_.valuePtr(), nnz).maxCoeff(&maxRowIdx);
  for (Key key : keys_) {
    assignments[key] = lazy_cp(key, sparse_table_.innerIndexPtr()[maxRowIdx]);
  }
  return assignments;
}

/* ************************************************************************ */
DiscreteKeys TableFactor::unionKeys(const TableFactor& f) const {
  // new cardinalities
  std::map<Key, size_t> cs;
  // make unique key-cardinality map
  for (Key j : keys()) cs[j] = cardinality(j);
  for (Key j : f.keys()) cs[j] = f.cardinality(j);
  // Convert map into keys
  DiscreteKeys keys;
  for (const std::pair<const Key, size_t>& key : cs) {
    keys.emplace_back(key);
  }
  // Sort
  std::sort(keys.begin(), keys.end());
  return keys;
}

/* ************************************************************************ */
std::vector<DiscreteValues> TableFactor::project(
    const DiscreteValues& assignment_f) const {
  // project assignment_f onto union of assignments
  // ex) project (v0, v2) onto the union (v0, v1, v2)
  std::vector<DiscreteValues> projected_assignments;
  for (Eigen::SparseVector<double>::InnerIterator it(sparse_table_); it; ++it) {
    DiscreteValues union_assignments;
    bool flag = true;
    // check if there is an overlapping key
    for (auto a_it = assignment_f.begin(); a_it != assignment_f.end() && flag;
         a_it++) {
      union_assignments.insert(*a_it);
      // if there is an overlapping key check the assigned value for that key
      if (find(a_it->first) != keys_.end()) {
        flag = lazy_cp(a_it->first, it.index()) == a_it->second;
      }
    }
    // If there is no overlapping key, or if the overlapped key's assigned
    // value is same with TableFactor's value project the assignment.
    if (flag) {
      for (Key key : keys_) {
        union_assignments.insert(
            std::pair<Key, size_t>(key, lazy_cp(key, it.index())));
      }
      projected_assignments.push_back(union_assignments);
    }
  }
  return projected_assignments;
}

/* ************************************************************************ */
std::vector<std::pair<DiscreteValues, double>> TableFactor::enumerate() const {
  // Get all possible assignments
  std::vector<std::pair<Key, size_t>> pairs = discreteKeys();
  // Reverse to make cartesian product output a more natural ordering.
  std::vector<std::pair<Key, size_t>> rpairs(pairs.rbegin(), pairs.rend());
  const auto assignments = DiscreteValues::CartesianProduct(rpairs);

  // Construct unordered_map with values
  std::vector<std::pair<DiscreteValues, double>> result;
  for (const auto& assignment : assignments) {
    result.emplace_back(assignment, operator()(assignment));
  }
  return result;
}

/* ************************************************************************ */
DiscreteKeys TableFactor::discreteKeys() const {
  DiscreteKeys result;
  for (Key key : keys_) {
    DiscreteKey dkey(key, cardinality(key));
    if (std::find(result.begin(), result.end(), dkey) == result.end()) {
      result.push_back(dkey);
    }
  }
  return result;
}

// Print out header.
/* ************************************************************************ */
string TableFactor::markdown(const KeyFormatter& keyFormatter,
                             const Names& names) const {
  stringstream ss;

  // Print out header.
  ss << "|";
  for (auto& key : keys()) {
    ss << keyFormatter(key) << "|";
  }
  ss << "value|\n";

  // Print out separator with alignment hints.
  ss << "|";
  for (size_t j = 0; j < size(); j++) ss << ":-:|";
  ss << ":-:|\n";

  // Print out all rows.
  auto rows = enumerate();
  for (const auto& kv : rows) {
    ss << "|";
    auto assignment = kv.first;
    for (auto& key : keys()) {
      size_t index = assignment.at(key);
      ss << DiscreteValues::Translate(names, key, index) << "|";
    }
    ss << kv.second << "|\n";
  }
  return ss.str();
}

/* ************************************************************************ */
string TableFactor::html(const KeyFormatter& keyFormatter,
                         const Names& names) const {
  stringstream ss;

  // Print out preamble.
  ss << "
\n
\n
\n";

  // Print out header row.
  ss << "
";
  for (auto& key : keys()) {
    ss << "" << keyFormatter(key) << "	";
  }
  ss << "value
\n";

  // Finish header and start body.
  ss << "
\n
\n";

  // Print out all rows.
  auto rows = enumerate();
  for (const auto& kv : rows) {
    ss << "
";
    auto assignment = kv.first;
    for (auto& key : keys()) {
      size_t index = assignment.at(key);
      ss << "" << DiscreteValues::Translate(names, key, index) << "	";
    }
    ss << "" << kv.second << "	";  // value
    ss << "
\n";
  }
  ss << "
\n
\n
";
  return ss.str();
}

/* ************************************************************************ */
}  // namespace gtsam


#include "../DynAutoDiff/DynAutoDiff.hpp"

#define BOOST_TEST_MODULE IO_Test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#define TL 1e-10

using namespace Eigen;
using namespace std;
using namespace DynAutoDiff;

BOOST_AUTO_TEST_SUITE(Eigen_Helper_Test)

BOOST_AUTO_TEST_CASE(load_mat_test) {
    auto x1 = rowvec({1.0, 5.0}), s1 = psca(5.0), Sigma = mat({6.0, 7.0, 8.0, 9.0}, 2, 2, true),
         x2 = vec({2.0, 6.0});
    auto y1 = (s1 - x1) * x2 + s1;
    GraphManager gm(y1);
    gm.run();
    gm.save("graph.json");

    auto y2 = gm.load("graph.json");
    gm.run();
    BOOST_CHECK_EQUAL(y1->v(), y2->v());
}
BOOST_AUTO_TEST_SUITE_END()


#include <toynet/examples/diff2/diff2.h>
#include <toynet/stlio.h>
#include <toynet/ublas/io.h>
#include <toynet/ublas/convert.h>
#include <iostream>
#include <sstream>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

using namespace boost::program_options;
using namespace toynet;

void print(const boost::property_tree::ptree& pt)
{
    boost::property_tree::ptree::const_iterator end = pt.end();
    for (boost::property_tree::ptree::const_iterator it = pt.begin(); it != end; ++it) {
        std::cout << it->first << ": " << it->second.get_value<std::string>() << std::endl;
        print(it->second);
    }
}

std::vector<diff2::Tensor1D> parse_json_examples(const std::string& s)
{
    std::vector<diff2::Tensor1D> ret;
    if (!s.empty()) {
        std::stringstream ss(s);
        boost::property_tree::ptree pt;
        boost::property_tree::read_json(ss, pt);
        for (const auto& e1 : pt) {
            diff2::Tensor1D v;
            for (const auto& e2 : e1.second) {
                v.resize(v.size() + 1);
                v[v.size() - 1] = e2.second.get_value<double>();
            }
            ret.push_back(v);
        }
    }
    return ret;
}

struct Options
{
    options_description desc;
    variables_map vm;
    bool help;
    int hidden;
    int width;
    int inputs;
    int outputs;
    int epochs;
    double lr;
    double alpha;
    std::string trainingx;
    std::string trainingy;
    std::string testing;
    bool progress;
    std::string loss;
    std::string optimizer;
    std::string initializer;
    double seed;

    Options(int argc, char* argv[])
        : desc("Allowed options")
        , help(false)
        , hidden(1)
        , width(2)
        , inputs(2)
        , outputs(1)
        , epochs(10)
        , lr(0.01)
        , alpha(0.5)
        , trainingx("[[4.0, 3.0]]")
        , trainingy("[[1.0]]")
        , testing("")
        , progress(true)
        , loss("mse")
        , optimizer("gradient")
        , initializer("fixed")
        , seed(0.0)
    {
        desc.add_options()
            // First parameter describes option name/short name
            // The second is parameter to option
            // The third is description
            ("help,h", bool_switch(&help), "print help message")
            ("hidden,H", value(&hidden), "number of hidden layers")
            ("width,W", value(&width), "width of hidden layers")
            ("inputs,I", value(&inputs), "width of input layer")
            ("outputs,O", value(&outputs), "width of output layer")
            ("epochs,e", value(&epochs), "number of training epochs")
            ("lr", value(&lr), "learning rate")
            ("alpha", value(&alpha), "momentum's alpha")
            ("trainingx", value(&trainingx), "training inputs")
            ("trainingy", value(&trainingy), "training outputs")
            ("testing", value(&testing), "testing X")
            ("progress", value(&progress), "show loss at each epoch")
            ("loss", value(&loss), "loss function")
            ("optimizer", value(&optimizer), "optimizer")
            ("initializer", value(&initializer), "weight initializer")
            ("seed", value(&seed), "rng seed")
            ;
        store(parse_command_line(argc, argv, desc), vm);
        notify(vm);
    }
};

std::unique_ptr<Loss> get_loss(const std::string& name)
{
    if (name == "mse")
        return std::make_unique<MSELoss>();
    if (name == "softmax")
        return std::make_unique<SoftmaxLoss>();
    throw std::runtime_error("unknown loss function: " + name);
}

std::unique_ptr<diff2::Optimizer> get_optimizer(const std::string& name, double lr, double alpha)
{
    if (name == "gradient")
        return std::make_unique<diff2::GradientOptimizer>(lr);
    if (name == "momentum")
        return std::make_unique<diff2::MomentumOptimizer>(lr, alpha);
    throw std::runtime_error("unknown optimizer: " + name);
}

std::unique_ptr<diff2::WeightInitializer> get_initializer(const std::string& name, double seed)
{
    if (name == "fixed")
        return std::make_unique<diff2::FixedWeightInitializer>();
    if (name == "glorot")
        return std::make_unique<diff2::GlorotBengio2010Initializer>(seed);
    throw std::runtime_error("unknown initializer: " + name);
}

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

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

    const std::vector<diff2::Tensor1D> training_X = parse_json_examples(opts.trainingx);
    const std::vector<diff2::Tensor1D> training_Y = parse_json_examples(opts.trainingy);
    const std::vector<diff2::Tensor1D> testing = parse_json_examples(opts.testing);

    // Make sure the dimensions of each training example's X is equal to opts.inputs
    for (const auto&v : training_X) {
        if (v.size() != opts.inputs) {
            std::cerr << "Size mismatch: the network has input size "
                      << opts.inputs << " but a training input has size "
                      << v.size() << ": " << v << std::endl;
            return 1;
        }
    }

    // Make sure the dimensions of each training example's Y is equal to opts.outputs
    for (const auto&v : training_Y) {
        if (v.size() != opts.outputs) {
            std::cerr << "Size mismatch: the network has output size "
                      << opts.inputs << " but a training output has size "
                      << v.size() << ": " << v << std::endl;
            return 1;
        }
    }

    // Make sure the dimensions of each testing example is equal to opts.inputs
    for (const auto&v : testing) {
        if (v.size() != opts.inputs) {
            std::cerr << "Size mismatch: the network has input size "
                      << opts.inputs << " but a testing input has size "
                      << v.size() << ": " << v << std::endl;
            return 1;
        }
    }

    std::unique_ptr<diff2::WeightInitializer> initializer = get_initializer(opts.initializer, opts.seed);
    std::unique_ptr<Loss> loss = get_loss(opts.loss);
    std::unique_ptr<diff2::Optimizer> opt = get_optimizer(opts.optimizer, opts.lr, opts.alpha);
    diff2::Network network(opts.hidden, opts.width, opts.inputs, opts.outputs, initializer.get());
    diff2::Trainer trainer(network, *loss, *opt);
    for (int e = 1;  e <= opts.epochs;  ++e) {
        trainer.train(e, training_X, training_Y);
        if (opts.progress)
            std::cout << e << " " << trainer.workspace.loss << std::endl;
    }
    std::cout << "loss " << trainer.workspace.loss << " weights " << network.W << std::endl;

    for (const auto&v : testing)
        std::cout << v << " -> " << network.predict(v) << std::endl;

    return 0;
}


#ifndef RenderUtilitiesGPU_h
#define RenderUtilitiesGPU_h

#include "cuda_utilities.cuh"
#include "PngWrapper.hpp"
#include "RenderUtilities.hpp"

#include <Eigen/Dense>

class Camera;

void render_scene(uint16_t width, uint16_t height, float3 *vertices, float3 *normals,
                  const Eigen::Vector3f & light_source, uint8_t *image);

#endif // RenderUtilities_h


// slam3d - Frontend for graph-based SLAM
// Copyright (C) 2017 S. Kasperski
//
// 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.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "G2oSolver.hpp"
#include "edge_direction_prior.h"
#include "edge_position_prior.h"

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/core/sparse_optimizer_terminate_action.h>

#include <boost/format.hpp>

using namespace slam3d;

typedef g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> SlamLinearSolver;

struct G2oSolver::Internal
{
	g2o::SparseOptimizer optimizer;
	g2o::HyperGraph::VertexSet newVertices;
	g2o::HyperGraph::EdgeSet newEdges;
};

G2oSolver::G2oSolver(Logger* logger) : Solver(logger), mInt(new Internal)
{
	// Initialize the SparseOptimizer
	std::unique_ptr<SlamLinearSolver> linearSolver(new SlamLinearSolver);
	linearSolver->setBlockOrdering(true);
	std::unique_ptr<g2o::BlockSolver_6_3> blockSolver(new g2o::BlockSolver_6_3(std::move(linearSolver)));
	mInt->optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)));

	// Set the default terminate action
	g2o::SparseOptimizerTerminateAction* terminateAction = new g2o::SparseOptimizerTerminateAction;
	mInt->optimizer.addPostIterationAction(terminateAction);

	mInitialized = false;
}

G2oSolver::~G2oSolver()
{
	clear();
}

void G2oSolver::addVertex(IdType id, const Transform& pose)
{
	boost::unique_lock<boost::mutex> guard(mMutex);

	// Check that given id has not been added before
	if(mInt->optimizer.vertex(id) != NULL)
	{
		throw DuplicateVertex(id);
	}

	// Set current pose and id
	g2o::VertexSE3* poseVertex = new g2o::VertexSE3();
	poseVertex->setEstimate(pose.cast<double>());  //Eigen::Isometry3d
	poseVertex->setId(id);

	// Add the vertex to the optimizer
	mInt->optimizer.addVertex(poseVertex);
	mInt->newVertices.insert(poseVertex);
}

void G2oSolver::addEdgeSE3(IdType source, IdType target, SE3Constraint::Ptr se3)
{
	boost::unique_lock<boost::mutex> guard(mMutex);

	// Create a new edge
	g2o::EdgeSE3* constraint = new g2o::EdgeSE3();

	// Set source and target
	constraint->vertices()[0] = mInt->optimizer.vertex(source);
	constraint->vertices()[1] = mInt->optimizer.vertex(target);
	if(constraint->vertices()[0] == NULL || constraint->vertices()[1] == NULL)
	{
		delete constraint;
		throw BadEdge(source, target);
	}

	// Set the measurement (odometry distance between vertices)
	const TransformWithCovariance& twc = se3->getRelativePose();
	constraint->setMeasurement(twc.transform.cast<double>());            // slam3d::Transform  aka Eigen::Isometry3d
	constraint->setInformation(twc.covariance.inverse().cast<double>()); // slam3d::Covariance<6> aka Eigen::Matrix<double,6,6>

	// Add the constraint to the optimizer
	mInt->optimizer.addEdge(constraint);
	mInt->newEdges.insert(constraint);
}

void G2oSolver::addEdgeGravity(IdType vertex, GravityConstraint::Ptr grav)
{
	boost::unique_lock<boost::mutex> guard(mMutex);
	g2o::EdgeDirectionPrior* prior = new g2o::EdgeDirectionPrior(grav->getDirection(), grav->getReference());
	prior->vertices()[0] = mInt->optimizer.vertex(vertex);
	prior->setInformation(grav->getCovariance().inverse().cast<double>());

	mInt->optimizer.addEdge(prior);
	mInt->newEdges.insert(prior);
}

void G2oSolver::addEdgePosition(IdType vertex, PositionConstraint::Ptr pos)
{
	boost::unique_lock<boost::mutex> guard(mMutex);
	g2o::EdgePositionPrior* prior = new g2o::EdgePositionPrior(pos->getPosition());
	prior->vertices()[0] = mInt->optimizer.vertex(vertex);
	prior->setInformation(pos->getCovariance().inverse().cast<double>());

	mInt->optimizer.addEdge(prior);
	mInt->newEdges.insert(prior);
}

void G2oSolver::setFixed(IdType id)
{
	boost::unique_lock<boost::mutex> guard(mMutex);

	// Fix the vertex in the graph to hold the map in place
	g2o::OptimizableGraph::Vertex* v = mInt->optimizer.vertex(id);
	if(!v)
	{
		mLogger->message(ERROR, (boost::format("Could not fix vertex with ID %1%!") % id).str());
		throw UnknownVertex(id);
	}
	v->setFixed(true);
}

bool G2oSolver::compute(unsigned iterations)
{
	// Clear previous optimization result
	mCorrections.clear();

	// need to do something?
	boost::unique_lock<boost::mutex> guard(mMutex);
	if(mInt->optimizer.activeVertices().size() == 0 && mInt->newVertices.size() < 2)
		return true;

	// Check input
	if(!mInt->optimizer.verifyInformationMatrices(true))
	{
		mLogger->message(ERROR, "Failed to verify information matrices!");
		return false;
	}

	// Reset the stop flag that is set by TerminateAction
	bool* stopFlag = mInt->optimizer.forceStopFlag();
	if(stopFlag)
	{
		*stopFlag = false;
	}

	// Do the graph optimization
	if(mInitialized)
	{
		mLogger->message(DEBUG, "Update Initialization.");
		mInt->optimizer.updateInitialization(mInt->newVertices, mInt->newEdges);
	}else
	{
		mLogger->message(DEBUG, "Do first Initialization.");
		mInitialized = mInt->optimizer.initializeOptimization();
	}
	mInt->newVertices.clear();
	mInt->newEdges.clear();

	int iter = mInt->optimizer.optimize(iterations, false);
	if (iter <= 0)
	{
		mLogger->message(ERROR, "Optimization failed!");
		return false;
	}
	mLogger->message(DEBUG ,(boost::format("Optimization finished after %1% iterations.") % iter).str());

	// Write the result so it can be used by the mapper
	g2o::SparseOptimizer::VertexContainer vertices = mInt->optimizer.activeVertices();
	for (g2o::SparseOptimizer::VertexContainer::const_iterator n = vertices.begin(); n < vertices.end(); n++)
	{
		g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(*n);
		assert(vertex);
		Transform iso = Transform(vertex->estimate());
		mCorrections.push_back(IdPose((*n)->id(), iso));
	}
	return true;
}

IdPoseVector G2oSolver::getCorrections()
{
	return mCorrections;
}

void G2oSolver::clear()
{
	boost::unique_lock<boost::mutex> guard(mMutex);
	mInt->optimizer.clear();
	mInitialized = false;
	mCorrections.clear();
}

void G2oSolver::saveGraph(std::string filename)
{
	boost::unique_lock<boost::mutex> guard(mMutex);
	if(mInt->optimizer.save(filename.c_str()))
	{
		mLogger->message(INFO, (boost::format("Saved current g2o graph in %1%.") % filename).str());
	}else
	{
		mLogger->message(ERROR, (boost::format("Could not save %1%.") % filename).str());
	}
}


#define BOOST_TEST_MODULE "test_3spn2_base_stacking_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/3SPN2/ThreeSPN2BaseStackingInteraction.hpp>
#include <mjolnir/input/read_units.hpp>

#include <random>

using parameter_set_to_test = std::tuple<
    mjolnir::ThreeSPN2BaseStackingPotentialParameter<double>,
    mjolnir::ThreeSPN2CBaseStackingPotentialParameter<double>
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(ThreeSPN2BaseStackingInteraction_numerical_diff,
        ParameterSet, parameter_set_to_test)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_stacking_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using interaction_type = mjolnir::ThreeSPN2BaseStackingInteraction<traits_type>;
    using potential_type   = mjolnir::ThreeSPN2BaseStackingPotential<real_type>;
    using base_stack_kind  = typename potential_type::base_stack_kind;

    {
        using unit_type = mjolnir::unit::constants<real_type>;
        using phys_type = mjolnir::physics::constants<real_type>;
        const std::string energy = "kcal/mol";
        const std::string length = "angstrom";
        phys_type::set_kB(phys_type::kB() * (unit_type::J_to_cal() / 1000.0) *
                          unit_type::avogadro_constant());
        phys_type::set_eps0(phys_type::eps0() * (1000.0 / unit_type::J_to_cal()) /
                            unit_type::avogadro_constant());
        phys_type::set_energy_unit(energy);

        phys_type::set_eps0(phys_type::eps0() / unit_type::m_to_angstrom());

        phys_type::set_m_to_length(unit_type::m_to_angstrom());
        phys_type::set_length_to_m(unit_type::angstrom_to_m());

        phys_type::set_L_to_volume(1e-3 * std::pow(unit_type::m_to_angstrom(), 3));
        phys_type::set_volume_to_L(1e+3 * std::pow(unit_type::angstrom_to_m(), 3));

        phys_type::set_length_unit(length);
    }

    //        SBi
    //     Si --> Bi
    //    /     `-^
    //   Pj theta | rij
    //    \       |
    //     Sj --- Bj
    //
    //  rij:
    //  1. (r < r0)
    //  2. (r0 < r)
    //  theta:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);
    constexpr real_type pi = mjolnir::math::constants<real_type>::pi();

    for(const auto bs_kind : {base_stack_kind::AA, base_stack_kind::AT,
                              base_stack_kind::AG, base_stack_kind::AC,
                              base_stack_kind::TA, base_stack_kind::TT,
                              base_stack_kind::TG, base_stack_kind::TC,
                              base_stack_kind::GA, base_stack_kind::GT,
                              base_stack_kind::GG, base_stack_kind::GC,
                              base_stack_kind::CA, base_stack_kind::CT,
                              base_stack_kind::CG, base_stack_kind::CC})
    {
        potential_type   potential(ParameterSet{});
        interaction_type interaction("none",
                std::vector<std::pair<std::array<std::size_t, 3>, base_stack_kind>>{
                    { {{0, 1, 2}}, bs_kind }
                }, potential_type(potential), {});

        system_type sys(3, boundary_type{});

        sys.mass(0) = 1.0;
        sys.mass(1) = 1.0;
        sys.mass(2) = 1.0;

        sys.rmass(0) = 1.0;
        sys.rmass(1) = 1.0;
        sys.rmass(2) = 1.0;

        sys.position(0) = coord_type(0.0, 0.0, 0.0);
        sys.position(1) = coord_type(0.0, 0.0, 0.0);
        sys.position(2) = coord_type(0.0, 0.0, 0.0);

        sys.velocity(0) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(1) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(2) = coord_type(0.0, 0.0, 0.0);

        sys.force(0)    = coord_type(0.0, 0.0, 0.0);
        sys.force(1)    = coord_type(0.0, 0.0, 0.0);
        sys.force(2)    = coord_type(0.0, 0.0, 0.0);

        sys.name(0)  = "Si";
        sys.name(1)  = "Bi";
        sys.name(2)  = "Bj";
        sys.group(0) = "DNA";
        sys.group(1) = "DNA";
        sys.group(2) = "DNA";

        potential.initialize(sys);
        interaction.initialize(sys);

        const auto theta0    = potential.theta_0(bs_kind);
        const auto pi_over_K = potential.pi_over_K_BS();
        const auto theta0_1  = theta0 + 0.2 * pi_over_K; //         dtheta < pi/2K
        const auto theta0_2  = theta0 + 0.7 * pi_over_K; // pi/2K < dtheta < pi/K
        const auto theta0_3  = theta0 + 1.2 * pi_over_K; // pi/K  < dtheta
        const auto r0_1      = potential.r0(bs_kind) - 0.2;
        const auto r0_2      = potential.r0(bs_kind) + 0.5;

        for(const auto r : {r0_1, r0_2})
        {
        for(const auto theta : {theta0_1, theta0_2, theta0_3})
        {
            BOOST_TEST_MESSAGE("======================================");
            BOOST_TEST_MESSAGE("r = " << r << ", theta = " << theta);

        for(std::size_t i=0; i<100; ++i)
        {
            // generate particle configuration in the following way
            //    y
            // Bj ^
            //  \ | theta0
            // r0\|-.
            // ---o-----o--> x
            //  Bi      Si
            //

            sys.position(0) = coord_type(4.0, 0.0, 0.0); // Si
            sys.position(1) = coord_type(0.0, 0.0, 0.0); // Bi
            sys.position(2) = coord_type(r * std::cos(theta), r * std::sin(theta), 0.0); // Bj
            sys.force(0) = coord_type(0.0, 0.0, 0.0);
            sys.force(1) = coord_type(0.0, 0.0, 0.0);
            sys.force(2) = coord_type(0.0, 0.0, 0.0);

            // ... and then rotate random direction to remove special axis
            //
            // Do this thousand times with different random numbers!
            {
                using matrix33_type = typename traits_type::matrix33_type;

                const auto rot_x = uni(mt) * pi;
                const auto rot_y = uni(mt) * pi;
                const auto rot_z = uni(mt) * pi;

                matrix33_type rotm_x(1.0,             0.0,              0.0,
                                     0.0, std::cos(rot_x), -std::sin(rot_x),
                                     0.0, std::sin(rot_x),  std::cos(rot_x));
                matrix33_type rotm_y( std::cos(rot_y), 0.0,  std::sin(rot_y),
                                                  0.0, 1.0,              0.0,
                                     -std::sin(rot_y), 0.0,  std::cos(rot_y));
                matrix33_type rotm_z(std::cos(rot_z), -std::sin(rot_z), 0.0,
                                     std::sin(rot_z),  std::cos(rot_z), 0.0,
                                                 0.0,              0.0, 1.0);

                const matrix33_type rotm = rotm_x * rotm_y * rotm_z;
                sys.position(0) = rotm * sys.position(0);
                sys.position(1) = rotm * sys.position(1);
                sys.position(2) = rotm * sys.position(2);

                BOOST_TEST(mjolnir::math::length(sys.position(0)) == 4.0,
                           boost::test_tools::tolerance(1e-6));
                BOOST_TEST(mjolnir::math::length(sys.position(1)) == 0.0,
                           boost::test_tools::tolerance(1e-6));
                BOOST_TEST(mjolnir::math::length(sys.position(2)) == r,
                           boost::test_tools::tolerance(1e-6));
            }

            sys.position(0) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));
            sys.position(1) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));
            sys.position(2) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));

            const system_type init = sys;

            BOOST_TEST(mjolnir::math::X(init.force(0)) == 0.0);
            BOOST_TEST(mjolnir::math::Y(init.force(0)) == 0.0);
            BOOST_TEST(mjolnir::math::Z(init.force(0)) == 0.0);

            BOOST_TEST(mjolnir::math::X(init.force(1)) == 0.0);
            BOOST_TEST(mjolnir::math::Y(init.force(1)) == 0.0);
            BOOST_TEST(mjolnir::math::Z(init.force(1)) == 0.0);

            BOOST_TEST(mjolnir::math::X(init.force(2)) == 0.0);
            BOOST_TEST(mjolnir::math::Y(init.force(2)) == 0.0);
            BOOST_TEST(mjolnir::math::Z(init.force(2)) == 0.0);

            constexpr real_type tol = 1e-4;
            constexpr real_type dr  = 1e-5;
            for(std::size_t idx=0; idx<3; ++idx)
            {
                {
                    // ----------------------------------------------------------------
                    // reset positions
                    sys = init;

                    // calc U(x-dx)
                    const auto E0 = interaction.calc_energy(sys);

                    mjolnir::math::X(sys.position(idx)) += dr;

                    // calc F(x)
                    interaction.calc_force(sys);

                    mjolnir::math::X(sys.position(idx)) += dr;

                    // calc U(x+dx)
                    const auto E1 = interaction.calc_energy(sys);

                    // central difference
                    const auto dE = (E1 - E0) * 0.5;

                    BOOST_TEST(-dE / dr ==  mjolnir::math::X(sys.force(idx)),
                               boost::test_tools::tolerance(tol));
                }
                {
                    // ----------------------------------------------------------------
                    // reset positions
                    sys = init;

                    BOOST_TEST(mjolnir::math::X(init.force(0)) == 0.0);
                    BOOST_TEST(mjolnir::math::Y(init.force(0)) == 0.0);
                    BOOST_TEST(mjolnir::math::Z(init.force(0)) == 0.0);

                    BOOST_TEST(mjolnir::math::X(init.force(1)) == 0.0);
                    BOOST_TEST(mjolnir::math::Y(init.force(1)) == 0.0);
                    BOOST_TEST(mjolnir::math::Z(init.force(1)) == 0.0);

                    BOOST_TEST(mjolnir::math::X(init.force(2)) == 0.0);
                    BOOST_TEST(mjolnir::math::Y(init.force(2)) == 0.0);
                    BOOST_TEST(mjolnir::math::Z(init.force(2)) == 0.0);

                    BOOST_TEST(mjolnir::math::X(sys.force(0)) == 0.0);
                    BOOST_TEST(mjolnir::math::Y(sys.force(0)) == 0.0);
                    BOOST_TEST(mjolnir::math::Z(sys.force(0)) == 0.0);

                    BOOST_TEST(mjolnir::math::X(sys.force(1)) == 0.0);
                    BOOST_TEST(mjolnir::math::Y(sys.force(1)) == 0.0);
                    BOOST_TEST(mjolnir::math::Z(sys.force(1)) == 0.0);

                    BOOST_TEST(mjolnir::math::X(sys.force(2)) == 0.0);
                    BOOST_TEST(mjolnir::math::Y(sys.force(2)) == 0.0);
                    BOOST_TEST(mjolnir::math::Z(sys.force(2)) == 0.0);

                    // calc U(x-dx)
                    const auto E0 = interaction.calc_energy(sys);

                    mjolnir::math::Y(sys.position(idx)) += dr;

                    // calc F(x)
                    interaction.calc_force(sys);

                    mjolnir::math::Y(sys.position(idx)) += dr;

                    // calc U(x+dx)
                    const auto E1 = interaction.calc_energy(sys);

                    // central difference
                    const auto dE = (E1 - E0) * 0.5;

                    BOOST_TEST(-dE / dr == mjolnir::math::Y(sys.force(idx)),
                               boost::test_tools::tolerance(tol));
                }
                {
                    // ----------------------------------------------------------------
                    // reset positions
                    sys = init;

                    // calc U(x-dx)
                    const auto E0 = interaction.calc_energy(sys);

                    mjolnir::math::Z(sys.position(idx)) += dr;

                    // calc F(x)
                    interaction.calc_force(sys);

                    mjolnir::math::Z(sys.position(idx)) += dr;

                    // calc U(x+dx)
                    const auto E1 = interaction.calc_energy(sys);

                    // central difference
                    const auto dE = (E1 - E0) * 0.5;

                    BOOST_TEST(-dE / dr == mjolnir::math::Z(sys.force(idx)),
                               boost::test_tools::tolerance(tol));
                }
            }
        } // perturbation
        } // theta
        } // r
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(ThreeSPN2BaseStackingInteraction_calc_force_and_energy,
        ParameterSet, parameter_set_to_test)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_stacking_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using interaction_type = mjolnir::ThreeSPN2BaseStackingInteraction<traits_type>;
    using potential_type   = mjolnir::ThreeSPN2BaseStackingPotential<real_type>;
    using base_stack_kind  = typename potential_type::base_stack_kind;

    {
        using unit_type = mjolnir::unit::constants<real_type>;
        using phys_type = mjolnir::physics::constants<real_type>;
        const std::string energy = "kcal/mol";
        const std::string length = "angstrom";
        phys_type::set_kB(phys_type::kB() * (unit_type::J_to_cal() / 1000.0) *
                          unit_type::avogadro_constant());
        phys_type::set_eps0(phys_type::eps0() * (1000.0 / unit_type::J_to_cal()) /
                            unit_type::avogadro_constant());
        phys_type::set_energy_unit(energy);

        phys_type::set_eps0(phys_type::eps0() / unit_type::m_to_angstrom());

        phys_type::set_m_to_length(unit_type::m_to_angstrom());
        phys_type::set_length_to_m(unit_type::angstrom_to_m());

        phys_type::set_L_to_volume(1e-3 * std::pow(unit_type::m_to_angstrom(), 3));
        phys_type::set_volume_to_L(1e+3 * std::pow(unit_type::angstrom_to_m(), 3));

        phys_type::set_length_unit(length);
    }

    //        SBi
    //     Si --> Bi
    //    /     `-^
    //   Pj theta | rij
    //    \       |
    //     Sj --- Bj
    //
    //  rij:
    //  1. (r < r0)
    //  2. (r0 < r)
    //  theta:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);
    constexpr real_type pi = mjolnir::math::constants<real_type>::pi();

    for(const auto bs_kind : {base_stack_kind::AA, base_stack_kind::AT,
                              base_stack_kind::AG, base_stack_kind::AC,
                              base_stack_kind::TA, base_stack_kind::TT,
                              base_stack_kind::TG, base_stack_kind::TC,
                              base_stack_kind::GA, base_stack_kind::GT,
                              base_stack_kind::GG, base_stack_kind::GC,
                              base_stack_kind::CA, base_stack_kind::CT,
                              base_stack_kind::CG, base_stack_kind::CC})
    {
        potential_type   potential(ParameterSet{});
        interaction_type interaction("none",
                std::vector<std::pair<std::array<std::size_t, 3>, base_stack_kind>>{
                    { {{0, 1, 2}}, bs_kind }
                }, potential_type(potential), {});

        system_type sys(3, boundary_type{});

        sys.mass(0) = 1.0;
        sys.mass(1) = 1.0;
        sys.mass(2) = 1.0;

        sys.rmass(0) = 1.0;
        sys.rmass(1) = 1.0;
        sys.rmass(2) = 1.0;

        sys.position(0) = coord_type(0.0, 0.0, 0.0);
        sys.position(1) = coord_type(0.0, 0.0, 0.0);
        sys.position(2) = coord_type(0.0, 0.0, 0.0);

        sys.velocity(0) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(1) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(2) = coord_type(0.0, 0.0, 0.0);

        sys.force(0)    = coord_type(0.0, 0.0, 0.0);
        sys.force(1)    = coord_type(0.0, 0.0, 0.0);
        sys.force(2)    = coord_type(0.0, 0.0, 0.0);

        sys.name(0)  = "Si";
        sys.name(1)  = "Bi";
        sys.name(2)  = "Bj";
        sys.group(0) = "DNA";
        sys.group(1) = "DNA";
        sys.group(2) = "DNA";

        potential.initialize(sys);
        interaction.initialize(sys);

        const auto theta0    = potential.theta_0(bs_kind);
        const auto pi_over_K = potential.pi_over_K_BS();
        const auto theta0_1  = theta0 + 0.2 * pi_over_K; //         dtheta < pi/2K
        const auto theta0_2  = theta0 + 0.7 * pi_over_K; // pi/2K < dtheta < pi/K
        const auto theta0_3  = theta0 + 1.2 * pi_over_K; // pi/K  < dtheta
        const auto r0_1      = potential.r0(bs_kind) - 0.2;
        const auto r0_2      = potential.r0(bs_kind) + 0.5;

        for(const auto r : {r0_1, r0_2})
        {
        for(const auto theta : {theta0_1, theta0_2, theta0_3})
        {
            BOOST_TEST_MESSAGE("======================================");
            BOOST_TEST_MESSAGE("r = " << r << ", theta = " << theta);

        for(std::size_t i=0; i<100; ++i)
        {
            // generate particle configuration in the following way
            //    y
            // Bj ^
            //  \ | theta0
            // r0\|-.
            // ---o-----o--> x
            //  Bi      Si
            //

            sys.position(0) = coord_type(4.0, 0.0, 0.0); // Si
            sys.position(1) = coord_type(0.0, 0.0, 0.0); // Bi
            sys.position(2) = coord_type(r * std::cos(theta), r * std::sin(theta), 0.0); // Bj
            sys.force(0) = coord_type(0.0, 0.0, 0.0);
            sys.force(1) = coord_type(0.0, 0.0, 0.0);
            sys.force(2) = coord_type(0.0, 0.0, 0.0);

            // ... and then rotate random direction to remove special axis
            //
            // Do this thousand times with different random numbers!
            {
                using matrix33_type = typename traits_type::matrix33_type;

                const auto rot_x = uni(mt) * pi;
                const auto rot_y = uni(mt) * pi;
                const auto rot_z = uni(mt) * pi;

                matrix33_type rotm_x(1.0,             0.0,              0.0,
                                     0.0, std::cos(rot_x), -std::sin(rot_x),
                                     0.0, std::sin(rot_x),  std::cos(rot_x));
                matrix33_type rotm_y( std::cos(rot_y), 0.0,  std::sin(rot_y),
                                                  0.0, 1.0,              0.0,
                                     -std::sin(rot_y), 0.0,  std::cos(rot_y));
                matrix33_type rotm_z(std::cos(rot_z), -std::sin(rot_z), 0.0,
                                     std::sin(rot_z),  std::cos(rot_z), 0.0,
                                                 0.0,              0.0, 1.0);

                const matrix33_type rotm = rotm_x * rotm_y * rotm_z;
                sys.position(0) = rotm * sys.position(0);
                sys.position(1) = rotm * sys.position(1);
                sys.position(2) = rotm * sys.position(2);

                BOOST_TEST(mjolnir::math::length(sys.position(0)) == 4.0,
                           boost::test_tools::tolerance(1e-6));
                BOOST_TEST(mjolnir::math::length(sys.position(1)) == 0.0,
                           boost::test_tools::tolerance(1e-6));
                BOOST_TEST(mjolnir::math::length(sys.position(2)) == r,
                           boost::test_tools::tolerance(1e-6));
            }

            sys.position(0) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));
            sys.position(1) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));
            sys.position(2) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));

            BOOST_TEST(mjolnir::math::X(sys.force(0)) == 0.0);
            BOOST_TEST(mjolnir::math::Y(sys.force(0)) == 0.0);
            BOOST_TEST(mjolnir::math::Z(sys.force(0)) == 0.0);

            BOOST_TEST(mjolnir::math::X(sys.force(1)) == 0.0);
            BOOST_TEST(mjolnir::math::Y(sys.force(1)) == 0.0);
            BOOST_TEST(mjolnir::math::Z(sys.force(1)) == 0.0);

            BOOST_TEST(mjolnir::math::X(sys.force(2)) == 0.0);
            BOOST_TEST(mjolnir::math::Y(sys.force(2)) == 0.0);
            BOOST_TEST(mjolnir::math::Z(sys.force(2)) == 0.0);

            auto ref_sys = sys;
            constexpr real_type tol = 1e-4;

            const auto energy = interaction.calc_force_and_energy(sys);
            const auto ref_energy = interaction.calc_energy(ref_sys);
            interaction.calc_force(ref_sys);
            BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

            for(std::size_t idx=0; idx<3; ++idx)
            {
                BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
                BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
                BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            }
        } // perturbation
        } // theta
        } // r
    }
}


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

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_GET_RADIUS_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_GET_RADIUS_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace strategy_detail
{

template
<
    typename RadiusTypeOrSphere,
    typename Tag = typename tag<RadiusTypeOrSphere>::type
>
struct get_radius
{
    typedef typename geometry::radius_type<RadiusTypeOrSphere>::type type;
    static type apply(RadiusTypeOrSphere const& sphere)
    {
        return geometry::get_radius<0>(sphere);
    }
};

template <typename RadiusTypeOrSphere>
struct get_radius<RadiusTypeOrSphere, void>
{
    typedef RadiusTypeOrSphere type;
    static type apply(RadiusTypeOrSphere const& radius)
    {
        return radius;
    }
};

// For backward compatibility
template <typename Point>
struct get_radius<Point, point_tag>
{
    typedef typename select_most_precise
        <
            typename coordinate_type<Point>::type,
            double
        >::type type;

    template <typename RadiusOrSphere>
    static typename get_radius<RadiusOrSphere>::type
        apply(RadiusOrSphere const& radius_or_sphere)
    {
        return get_radius<RadiusOrSphere>::apply(radius_or_sphere);
    }
};

} // namespace strategy_detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_GET_RADIUS_HPP


#include <boost/hana/fwd/power.hpp>