//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  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)
//
#define BOOST_LOCALE_SOURCE
#include "formatter.hpp"

#include "predefined_formatters.hpp"
#include "time_zone.hpp"
#include "uconv.hpp"

#include <unicode/datefmt.h>
#include <unicode/decimfmt.h>
#include <unicode/numfmt.h>
#include <unicode/rbnf.h>
#include <unicode/smpdtfmt.h>

#include <boost/locale/formatting.hpp>
#include <boost/locale/info.hpp>
#include <iostream>
#include <limits>

#ifdef BOOST_MSVC
#pragma warning(disable : 4244) // lose data
#endif

namespace boost
{
namespace locale
{
namespace impl_icu
{

std::locale::id icu_formatters_cache::id;

namespace
{
struct init
{
    init()
    {
        std::has_facet<icu_formatters_cache>(std::locale::classic());
    }
} instance;
} // namespace

template <typename CharType>
class number_format : public formatter<CharType>
{
  public:
    typedef CharType char_type;
    typedef std::basic_string<CharType> string_type;

    virtual string_type format(double value, size_t& code_points) const
    {
        icu::UnicodeString tmp;
        icu_fmt_->format(value, tmp);
        code_points = tmp.countChar32();
        return cvt_.std(tmp);
    }
    virtual string_type format(int64_t value, size_t& code_points) const
    {
        icu::UnicodeString tmp;
        icu_fmt_->format(static_cast<::int64_t>(value), tmp);
        code_points = tmp.countChar32();
        return cvt_.std(tmp);
    }

    virtual string_type format(int32_t value, size_t& code_points) const
    {
        icu::UnicodeString tmp;
#ifdef __SUNPRO_CC
        icu_fmt_->format(static_cast<int>(value), tmp);
#else
        icu_fmt_->format(::int32_t(value), tmp);
#endif
        code_points = tmp.countChar32();
        return cvt_.std(tmp);
    }

    virtual size_t parse(string_type const& str, double& value) const
    {
        return do_parse(str, value);
    }

    virtual size_t parse(string_type const& str, int64_t& value) const
    {
        return do_parse(str, value);
    }
    virtual size_t parse(string_type const& str, int32_t& value) const
    {
        return do_parse(str, value);
    }

    number_format(icu::NumberFormat* fmt, std::string codepage) :
        cvt_(codepage), icu_fmt_(fmt)
    {
    }

  private:
    bool get_value(double& v, icu::Formattable& fmt) const
    {
        UErrorCode err = U_ZERO_ERROR;
        v = fmt.getDouble(err);
        if (U_FAILURE(err))
            return false;
        return true;
    }

    bool get_value(int64_t& v, icu::Formattable& fmt) const
    {
        UErrorCode err = U_ZERO_ERROR;
        v = fmt.getInt64(err);
        if (U_FAILURE(err))
            return false;
        return true;
    }

    bool get_value(int32_t& v, icu::Formattable& fmt) const
    {
        UErrorCode err = U_ZERO_ERROR;
        v = fmt.getLong(err);
        if (U_FAILURE(err))
            return false;
        return true;
    }

    template <typename ValueType>
    size_t do_parse(string_type const& str, ValueType& v) const
    {
        icu::Formattable val;
        icu::ParsePosition pp;
        icu::UnicodeString tmp = cvt_.icu(str.data(), str.data() + str.size());

        icu_fmt_->parse(tmp, val, pp);

        ValueType tmp_v;

        if (pp.getIndex() == 0 || !get_value(tmp_v, val))
            return 0;
        size_t cut =
            cvt_.cut(tmp, str.data(), str.data() + str.size(), pp.getIndex());
        if (cut == 0)
            return 0;
        v = tmp_v;
        return cut;
    }

    icu_std_converter<CharType> cvt_;
    icu::NumberFormat* icu_fmt_;
};

template <typename CharType>
class date_format : public formatter<CharType>
{
  public:
    typedef CharType char_type;
    typedef std::basic_string<CharType> string_type;

    virtual string_type format(double value, size_t& code_points) const
    {
        return do_format(value, code_points);
    }
    virtual string_type format(int64_t value, size_t& code_points) const
    {
        return do_format(value, code_points);
    }

    virtual string_type format(int32_t value, size_t& code_points) const
    {
        return do_format(value, code_points);
    }

    virtual size_t parse(string_type const& str, double& value) const
    {
        return do_parse(str, value);
    }
    virtual size_t parse(string_type const& str, int64_t& value) const
    {
        return do_parse(str, value);
    }
    virtual size_t parse(string_type const& str, int32_t& value) const
    {
        return do_parse(str, value);
    }

    date_format(icu::DateFormat* fmt, bool transfer_owneship,
                std::string codepage) :
        cvt_(codepage)
    {
        if (transfer_owneship)
        {
            aicu_fmt_.reset(fmt);
            icu_fmt_ = aicu_fmt_.get();
        }
        else
        {
            icu_fmt_ = fmt;
        }
    }

  private:
    template <typename ValueType>
    size_t do_parse(string_type const& str, ValueType& value) const
    {
        icu::ParsePosition pp;
        icu::UnicodeString tmp = cvt_.icu(str.data(), str.data() + str.size());

        UDate udate = icu_fmt_->parse(tmp, pp);
        if (pp.getIndex() == 0)
            return 0;
        double date = udate / 1000.0;
        typedef std::numeric_limits<ValueType> limits_type;
        if (date > limits_type::max() || date < limits_type::min())
            return 0;
        size_t cut =
            cvt_.cut(tmp, str.data(), str.data() + str.size(), pp.getIndex());
        if (cut == 0)
            return 0;
        value = static_cast<ValueType>(date);
        return cut;
    }

    string_type do_format(double value, size_t& codepoints) const
    {
        UDate date = value * 1000.0; /// UDate is time_t in miliseconds
        icu::UnicodeString tmp;
        icu_fmt_->format(date, tmp);
        codepoints = tmp.countChar32();
        return cvt_.std(tmp);
    }

    icu_std_converter<CharType> cvt_;
    hold_ptr<icu::DateFormat> aicu_fmt_;
    icu::DateFormat* icu_fmt_;
};

icu::UnicodeString strftime_to_icu_full(icu::DateFormat* dfin, char const* alt)
{
    hold_ptr<icu::DateFormat> df(dfin);
    icu::SimpleDateFormat* sdf = dynamic_cast<icu::SimpleDateFormat*>(df.get());
    icu::UnicodeString tmp;
    if (sdf)
    {
        sdf->toPattern(tmp);
    }
    else
    {
        tmp = alt;
    }
    return tmp;
}

icu::UnicodeString strftime_to_icu_symbol(char c, icu::Locale const& locale,
                                          icu_formatters_cache const* cache = 0)
{
    switch (c)
    {
        case 'a': // Abbr Weekday
            return "EE";
        case 'A': // Full Weekday
            return "EEEE";
        case 'b': // Abbr Month
            return "MMM";
        case 'B': // Full Month
            return "MMMM";
        case 'c': // DateTile Full
        {
            if (cache)
                return cache->date_time_format_[1][1];
            return strftime_to_icu_full(
                icu::DateFormat::createDateTimeInstance(
                    icu::DateFormat::kFull, icu::DateFormat::kFull, locale),
                "yyyy-MM-dd HH:mm:ss");
        }
        // not supported by ICU ;(
        //  case 'C': // Century -> 1980 -> 19
        //  retur
        case 'd': // Day of Month [01,31]
            return "dd";
        case 'D': // %m/%d/%y
            return "MM/dd/yy";
        case 'e': // Day of Month [1,31]
            return "d";
        case 'h': // == b
            return "MMM";
        case 'H': // 24 clock hour 00,23
            return "HH";
        case 'I': // 12 clock hour 01,12
            return "hh";
        case 'j': // day of year 001,366
            return "D";
        case 'm': // month as [01,12]
            return "MM";
        case 'M': // minute [00,59]
            return "mm";
        case 'n': // \n
            return "\n";
        case 'p': // am-pm
            return "a";
        case 'r': // time with AM/PM %I:%M:%S %p
            return "hh:mm:ss a";
        case 'R': // %H:%M
            return "HH:mm";
        case 'S': // second [00,61]
            return "ss";
        case 't': // \t
            return "\t";
        case 'T': // %H:%M:%S
            return "HH:mm:ss";
            /*          case 'u': // weekday 1,7 1=Monday
                        case 'U': // week number of year [00,53] Sunday first
                        case 'V': // week number of year [01,53] Moday first
                        case 'w': // weekday 0,7 0=Sunday
                        case 'W': // week number of year [00,53] Moday first, */
        case 'x': // Date
        {
            if (cache)
                return cache->date_format_[1];
            return strftime_to_icu_full(icu::DateFormat::createDateInstance(
                                            icu::DateFormat::kMedium, locale),
                                        "yyyy-MM-dd");
        }
        case 'X': // Time
        {
            if (cache)
                return cache->time_format_[1];
            return strftime_to_icu_full(icu::DateFormat::createTimeInstance(
                                            icu::DateFormat::kMedium, locale),
                                        "HH:mm:ss");
        }
        case 'y': // Year [00-99]
            return "yy";
        case 'Y': // Year 1998
            return "yyyy";
        case 'Z': // timezone
            return "vvvv";
        case '%': // %
            return "%";
        default:
            return "";
    }
}

icu::UnicodeString strftime_to_icu(icu::UnicodeString const& ftime,
                                   icu::Locale const& locale)
{
    unsigned len = ftime.length();
    icu::UnicodeString result;
    bool escaped = false;
    for (unsigned i = 0; i < len; i++)
    {
        UChar c = ftime[i];
        if (c == '%')
        {
            i++;
            c = ftime[i];
            if (c == 'E' || c == 'O')
            {
                i++;
                c = ftime[i];
            }
            if (escaped)
            {
                result += "'";
                escaped = false;
            }
            result += strftime_to_icu_symbol(c, locale);
        }
        else if (c == '\'')
        {
            result += "''";
        }
        else
        {
            if (!escaped)
            {
                result += "'";
                escaped = true;
            }
            result += c;
        }
    }
    if (escaped)
        result += "'";
    return result;
}

template <typename CharType>
formatter<CharType>* generate_formatter(std::ios_base& ios,
                                        icu::Locale const& locale,
                                        std::string const& encoding)
{
    using namespace boost::locale::flags;

    hold_ptr<formatter<CharType>> fmt;
    ios_info& info = ios_info::get(ios);
    uint64_t disp = info.display_flags();

    icu_formatters_cache const& cache =
        std::use_facet<icu_formatters_cache>(ios.getloc());

    if (disp == posix)
        return fmt.release();

    UErrorCode err = U_ZERO_ERROR;

    switch (disp)
    {
        case number:
        {
            std::ios_base::fmtflags how =
                (ios.flags() & std::ios_base::floatfield);
            icu::NumberFormat* nf = 0;

            if (how == std::ios_base::scientific)
                nf = cache.number_format(icu_formatters_cache::fmt_sci);
            else
                nf = cache.number_format(icu_formatters_cache::fmt_number);

            nf->setMaximumFractionDigits(ios.precision());
            if (how == std::ios_base::scientific || how == std::ios_base::fixed)
            {
                nf->setMinimumFractionDigits(ios.precision());
            }
            else
            {
                nf->setMinimumFractionDigits(0);
            }
            fmt.reset(new number_format<CharType>(nf, encoding));
        }
        break;
        case currency:
        {
            icu::NumberFormat* nf;

            uint64_t curr = info.currency_flags();

            if (curr == currency_default || curr == currency_national)
                nf = cache.number_format(icu_formatters_cache::fmt_curr_nat);
            else
                nf = cache.number_format(icu_formatters_cache::fmt_curr_iso);

            fmt.reset(new number_format<CharType>(nf, encoding));
        }
        break;
        case percent:
        {
            icu::NumberFormat* nf =
                cache.number_format(icu_formatters_cache::fmt_per);
            nf->setMaximumFractionDigits(ios.precision());
            std::ios_base::fmtflags how =
                (ios.flags() & std::ios_base::floatfield);
            if (how == std::ios_base::scientific || how == std::ios_base::fixed)
            {
                nf->setMinimumFractionDigits(ios.precision());
            }
            else
            {
                nf->setMinimumFractionDigits(0);
            }
            fmt.reset(new number_format<CharType>(nf, encoding));
        }
        break;
        case spellout:
            fmt.reset(new number_format<CharType>(
                cache.number_format(icu_formatters_cache::fmt_spell),
                encoding));
            break;
        case ordinal:
            fmt.reset(new number_format<CharType>(
                cache.number_format(icu_formatters_cache::fmt_ord), encoding));
            break;
        case date:
        case time:
        case datetime:
        case strftime:
        {
            using namespace flags;
            hold_ptr<icu::DateFormat> adf;
            icu::DateFormat* df = 0;
            icu::SimpleDateFormat* sdf = cache.date_formatter();
            // try to use cached first
            if (sdf)
            {
                int tmf = info.time_flags();
                switch (tmf)
                {
                    case time_short:
                        tmf = 0;
                        break;
                    case time_long:
                        tmf = 2;
                        break;
                    case time_full:
                        tmf = 3;
                        break;
                    case time_default:
                    case time_medium:
                    default:
                        tmf = 1;
                }
                int dtf = info.date_flags();
                switch (dtf)
                {
                    case date_short:
                        dtf = 0;
                        break;
                    case date_long:
                        dtf = 2;
                        break;
                    case date_full:
                        dtf = 3;
                        break;
                    case date_default:
                    case date_medium:
                    default:
                        dtf = 1;
                }

                icu::UnicodeString pattern;
                switch (disp)
                {
                    case date:
                        pattern = cache.date_format_[dtf];
                        break;
                    case time:
                        pattern = cache.time_format_[tmf];
                        break;
                    case datetime:
                        pattern = cache.date_time_format_[dtf][tmf];
                        break;
                    case strftime:
                    {
                        if (!cache.date_format_[1].isEmpty() &&
                            !cache.time_format_[1].isEmpty() &&
                            !cache.date_time_format_[1][1].isEmpty())
                        {
                            icu_std_converter<CharType> cvt_(encoding);
                            std::basic_string<CharType> const& f =
                                info.date_time_pattern<CharType>();
                            pattern = strftime_to_icu(
                                cvt_.icu(f.c_str(), f.c_str() + f.size()),
                                locale);
                        }
                    }
                    break;
                }
                if (!pattern.isEmpty())
                {
                    sdf->applyPattern(pattern);
                    df = sdf;
                    sdf = 0;
                }
                sdf = 0;
            }

            if (!df)
            {
                icu::DateFormat::EStyle dstyle = icu::DateFormat::kDefault;
                icu::DateFormat::EStyle tstyle = icu::DateFormat::kDefault;

                switch (info.time_flags())
                {
                    case time_short:
                        tstyle = icu::DateFormat::kShort;
                        break;
                    case time_medium:
                        tstyle = icu::DateFormat::kMedium;
                        break;
                    case time_long:
                        tstyle = icu::DateFormat::kLong;
                        break;
                    case time_full:
                        tstyle = icu::DateFormat::kFull;
                        break;
                }
                switch (info.date_flags())
                {
                    case date_short:
                        dstyle = icu::DateFormat::kShort;
                        break;
                    case date_medium:
                        dstyle = icu::DateFormat::kMedium;
                        break;
                    case date_long:
                        dstyle = icu::DateFormat::kLong;
                        break;
                    case date_full:
                        dstyle = icu::DateFormat::kFull;
                        break;
                }

                if (disp == date)
                    adf.reset(
                        icu::DateFormat::createDateInstance(dstyle, locale));
                else if (disp == time)
                    adf.reset(
                        icu::DateFormat::createTimeInstance(tstyle, locale));
                else if (disp == datetime)
                    adf.reset(icu::DateFormat::createDateTimeInstance(
                        dstyle, tstyle, locale));
                else
                { // strftime
                    icu_std_converter<CharType> cvt_(encoding);
                    std::basic_string<CharType> const& f =
                        info.date_time_pattern<CharType>();
                    icu::UnicodeString fmt = strftime_to_icu(
                        cvt_.icu(f.data(), f.data() + f.size()), locale);
                    adf.reset(new icu::SimpleDateFormat(fmt, locale, err));
                }
                if (U_FAILURE(err))
                    return fmt.release();
                df = adf.get();
            }

            df->adoptTimeZone(get_time_zone(info.time_zone()));

            // Depending if we own formatter or not
            if (adf.get())
                fmt.reset(
                    new date_format<CharType>(adf.release(), true, encoding));
            else
                fmt.reset(new date_format<CharType>(df, false, encoding));
        }
        break;
    }

    return fmt.release();
}

template <>
formatter<char>* formatter<char>::create(std::ios_base& ios,
                                         icu::Locale const& l,
                                         std::string const& e)
{
    return generate_formatter<char>(ios, l, e);
}

template <>
formatter<wchar_t>* formatter<wchar_t>::create(std::ios_base& ios,
                                               icu::Locale const& l,
                                               std::string const& e)
{
    return generate_formatter<wchar_t>(ios, l, e);
}

#ifdef BOOST_LOCALE_ENABLE_CHAR16_T
template <>
formatter<char16_t>* formatter<char16_t>::create(std::ios_base& ios,
                                                 icu::Locale const& l,
                                                 std::string const& e)
{
    return generate_formatter<char16_t>(ios, l, e);
}

#endif

#ifdef BOOST_LOCALE_ENABLE_CHAR32_T
template <>
formatter<char32_t>* formatter<char32_t>::create(std::ios_base& ios,
                                                 icu::Locale const& l,
                                                 std::string const& e)
{
    return generate_formatter<char32_t>(ios, l, e);
}

#endif

} // namespace impl_icu

} // namespace locale
} // namespace boost

// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;
namespace tt = nsm::type_traits;

template<typename T, std::size_t N, typename Env>
void test( Env&, tt::false_type const& )
{}

template<typename T, std::size_t N, typename Env>
void test( Env& runtime, tt::true_type const& )
{
  using p_t = bs::pack<T, N>;
  using g_t = bd::downgrade_t<p_t>;
  using type = typename g_t::value_type;

  T a1[N], a2[N];
  type c[2*N];
  for(size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i*2) : T(3+i);
    a2[i] = (i%2) ? T(7+i) : T(i);
    c[i]   = std::is_integral<T>::value ? bs::saturate<type>(a1[i]) : a1[i];
    c[i+N] = std::is_integral<T>::value ? bs::saturate<type>(a2[i]) : a2[i];
  }

  // Spice up with value that will be narrowed
  a1[0] = bs::Valmax<T>();
  a2[0] = bs::Valmin<T>();
  c[0]  = std::is_integral<T>::value ? bs::saturate<type>(a1[0]) : a1[0];
  c[N]  = std::is_integral<T>::value ? bs::saturate<type>(a2[0]) : a2[0];

  p_t aa1(&a1[0], &a1[0]+N);
  p_t aa2(&a2[0], &a2[0]+N);
  g_t cc (&c[0], &c[0]+2*N);

  STF_EQUAL(bs::saturated_(bs::group)(aa1,aa2), cc);
}

STF_CASE_TPL("Check group on pack", STF_NUMERIC_TYPES)
{;
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N  >(runtime, bd::is_downgradable< bs::pack<T,N  > >{});
  test<T, N/2>(runtime, bd::is_downgradable< bs::pack<T,N/2> >{});
  test<T, N*2>(runtime, bd::is_downgradable< bs::pack<T,N*2> >{});
}


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

#include "crypter.h"
#include "script/script.h"
#include "script/standard.h"
#include "util.h"
#include <openssl/md5.h>
#include <string>
#include <vector>
#include <boost/foreach.hpp>
#include <openssl/aes.h>
#include <openssl/evp.h>
#include <iostream>
#include <fstream>

#include <openssl/pem.h> // For RSA Key Export

unsigned char chKeyBiblePay[256];
unsigned char chIVBiblePay[256];
bool fKeySetBiblePay;
// RSA
const unsigned int RSA_KEYLEN = 2048;
const unsigned int KEY_SERVER_PRI = 0;
const unsigned int KEY_SERVER_PUB = 1;
const unsigned int KEY_CLIENT_PUB = 2;
const unsigned int KEY_AES        = 3;
const unsigned int KEY_AES_IV     = 4;
const unsigned int KEY_CLIENT_PRI = 5;
// END RSA

bool CCrypter::SetKeyFromPassphrase(const SecureString& strKeyData, const std::vector<unsigned char>& chSalt, const unsigned int nRounds, const unsigned int nDerivationMethod)
{
    if (nRounds < 1 || chSalt.size() != WALLET_CRYPTO_SALT_SIZE)
        return false;

    int i = 0;
    if (nDerivationMethod == 0)
        i = EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha512(), &chSalt[0],
                          (unsigned char *)&strKeyData[0], strKeyData.size(), nRounds, chKey, chIV);

    if (i != (int)WALLET_CRYPTO_KEY_SIZE)
    {
        memory_cleanse(chKey, sizeof(chKey));
        memory_cleanse(chIV, sizeof(chIV));
        return false;
    }

    fKeySet = true;
    return true;
}

bool CCrypter::SetKey(const CKeyingMaterial& chNewKey, const std::vector<unsigned char>& chNewIV)
{
    if (chNewKey.size() != WALLET_CRYPTO_KEY_SIZE || chNewIV.size() != WALLET_CRYPTO_KEY_SIZE)
        return false;

    memcpy(&chKey[0], &chNewKey[0], sizeof chKey);
    memcpy(&chIV[0], &chNewIV[0], sizeof chIV);

    fKeySet = true;
    return true;
}

bool CCrypter::Encrypt(const CKeyingMaterial& vchPlaintext, std::vector<unsigned char> &vchCiphertext)
{
    if (!fKeySet)
        return false;

    // max ciphertext len for a n bytes of plaintext is
    // n + AES_BLOCK_SIZE - 1 bytes
    int nLen = vchPlaintext.size();
    int nCLen = nLen + AES_BLOCK_SIZE, nFLen = 0;
    vchCiphertext = std::vector<unsigned char> (nCLen);

    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();

    if (!ctx) return false;

    bool fOk = true;

    EVP_CIPHER_CTX_init(ctx);
    if (fOk) fOk = EVP_EncryptInit_ex(ctx, EVP_aes_256_cbc(), NULL, chKey, chIV) != 0;
    if (fOk) fOk = EVP_EncryptUpdate(ctx, &vchCiphertext[0], &nCLen, &vchPlaintext[0], nLen) != 0;
    if (fOk) fOk = EVP_EncryptFinal_ex(ctx, (&vchCiphertext[0]) + nCLen, &nFLen) != 0;
    EVP_CIPHER_CTX_cleanup(ctx);

    EVP_CIPHER_CTX_free(ctx);

    if (!fOk) return false;

    vchCiphertext.resize(nCLen + nFLen);
    return true;
}

bool CCrypter::Decrypt(const std::vector<unsigned char>& vchCiphertext, CKeyingMaterial& vchPlaintext)
{
    if (!fKeySet)
        return false;

    // plaintext will always be equal to or lesser than length of ciphertext
    int nLen = vchCiphertext.size();
    int nPLen = nLen, nFLen = 0;

    vchPlaintext = CKeyingMaterial(nPLen);

    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();

    if (!ctx) return false;

    bool fOk = true;

    EVP_CIPHER_CTX_init(ctx);
    if (fOk) fOk = EVP_DecryptInit_ex(ctx, EVP_aes_256_cbc(), NULL, chKey, chIV) != 0;
    if (fOk) fOk = EVP_DecryptUpdate(ctx, &vchPlaintext[0], &nPLen, &vchCiphertext[0], nLen) != 0;
    if (fOk) fOk = EVP_DecryptFinal_ex(ctx, (&vchPlaintext[0]) + nPLen, &nFLen) != 0;
    EVP_CIPHER_CTX_cleanup(ctx);

    EVP_CIPHER_CTX_free(ctx);

    if (!fOk) return false;

    vchPlaintext.resize(nPLen + nFLen);
    return true;
}

bool LoadBibleKey(std::string biblekey, std::string salt)
{
	const char* chBibleKey = biblekey.c_str();
	const char* chSalt = salt.c_str();
	OPENSSL_cleanse(chKeyBiblePay, sizeof(chKeyBiblePay));
    OPENSSL_cleanse(chIVBiblePay, sizeof(chIVBiblePay));
    EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha512(),(unsigned char *)chSalt,
		(unsigned char *)chBibleKey,
		strlen(chBibleKey), 1,
		chKeyBiblePay, chIVBiblePay);
    fKeySetBiblePay = true;
    return true;
}

std::vector<unsigned char> StringToVector(std::string sData)
{
        std::vector<unsigned char> v(sData.begin(), sData.end());
		return v;
}

std::string VectorToString(std::vector<unsigned char> v)
{
        std::string s(v.begin(), v.end());
        return s;
}

std::string PubKeyToAddress(const CScript& scriptPubKey)
{
	CTxDestination address1;
    ExtractDestination(scriptPubKey, address1);
    CBitcoinAddress address2(address1);
    return address2.ToString();
}

std::string RetrieveMd5(std::string s1)
{
	try
	{
		const char* chIn = s1.c_str();
		unsigned char digest2[16];
		MD5((unsigned char*)chIn, strlen(chIn), (unsigned char*)&digest2);
		char mdString2[33];
		for(int i = 0; i < 16; i++) sprintf(&mdString2[i*2], "%02x", (unsigned int)digest2[i]);
 		std::string xmd5(mdString2);
		return xmd5;
	}
    catch (std::exception &e)
	{
		return "";
	}
}

void PrintStratisKeyDebugInfo()
{
	std::string sKey1(reinterpret_cast<char*>(chKeyBiblePay));
	std::string sIV1(reinterpret_cast<char*>(chIVBiblePay));
	std::string sRow = "chKeyBiblePay: ";
	for (int i = 0; i < (int)sKey1.length(); i++)
	{
		int ichar = (int)sKey1[i];
		sRow += RoundToString(ichar,0) + ",";
	}
	sRow += "  chIVBiblePay: ";
	for (int i = 0; i < (int)sIV1.length(); i++)
	{
		int ichar = (int)sIV1[i];
		sRow += RoundToString(ichar,0) + ",";
	}
	LogPrintf(" \n StratisKeyDebugInfo: %s \n",sRow.c_str());
}

bool BibleEncrypt(std::vector<unsigned char> vchPlaintext, std::vector<unsigned char> &vchCiphertext)
{
	if (!fKeySetBiblePay) LoadBibleKey("biblepay","eb5a781ea9da2ef36");
    int nLen = vchPlaintext.size();
    int nCLen = nLen + AES_BLOCK_SIZE, nFLen = 0;
    vchCiphertext = std::vector<unsigned char> (nCLen);
    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();
    bool fOk = true;
    EVP_CIPHER_CTX_init(ctx);
	if (fOk) fOk = EVP_EncryptInit_ex(ctx, EVP_aes_256_cbc(), NULL, chKeyBiblePay, chIVBiblePay);
    if (fOk) fOk = EVP_EncryptUpdate(ctx, &vchCiphertext[0], &nCLen, &vchPlaintext[0], nLen);
    if (fOk) fOk = EVP_EncryptFinal_ex(ctx, (&vchCiphertext[0])+nCLen, &nFLen);
    EVP_CIPHER_CTX_free(ctx);
    if (!fOk) return false;
    vchCiphertext.resize(nCLen + nFLen);
    return true;
}

bool BibleDecrypt(const std::vector<unsigned char>& vchCiphertext,std::vector<unsigned char>& vchPlaintext)
{
	LoadBibleKey("biblepay","eb5a781ea9da2ef36");
	int nLen = vchCiphertext.size();
    int nPLen = nLen, nFLen = 0;
    //EVP_CIPHER_CTX ctx;
    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();
    bool fOk = true;
    EVP_CIPHER_CTX_init(ctx);
    if (fOk) fOk = EVP_DecryptInit_ex(ctx, EVP_aes_256_cbc(), NULL, chKeyBiblePay, chIVBiblePay);
    if (fOk) fOk = EVP_DecryptUpdate(ctx, &vchPlaintext[0], &nPLen, &vchCiphertext[0], nLen);
    if (fOk) fOk = EVP_DecryptFinal_ex(ctx, (&vchPlaintext[0])+nPLen, &nFLen);
    EVP_CIPHER_CTX_free(ctx);
    if (!fOk) return false;
    vchPlaintext.resize(nPLen + nFLen);
    return true;
}

static bool EncryptSecret(const CKeyingMaterial& vMasterKey, const CKeyingMaterial &vchPlaintext, const uint256& nIV, std::vector<unsigned char> &vchCiphertext)
{
    CCrypter cKeyCrypter;
    std::vector<unsigned char> chIV(WALLET_CRYPTO_KEY_SIZE);
    memcpy(&chIV[0], &nIV, WALLET_CRYPTO_KEY_SIZE);
    if(!cKeyCrypter.SetKey(vMasterKey, chIV))
        return false;
    return cKeyCrypter.Encrypt(*((const CKeyingMaterial*)&vchPlaintext), vchCiphertext);
}

// General secure AES 256 CBC encryption routine
bool EncryptAES256(const SecureString& sKey, const SecureString& sPlaintext, const std::string& sIV, std::string& sCiphertext)
{
    // max ciphertext len for a n bytes of plaintext is
    // n + AES_BLOCK_SIZE - 1 bytes
    int nLen = sPlaintext.size();
    int nCLen = nLen + AES_BLOCK_SIZE;
    int nFLen = 0;

    // Verify key sizes
    if(sKey.size() != 32 || sIV.size() != AES_BLOCK_SIZE) {
        LogPrintf("crypter EncryptAES256 - Invalid key or block size: Key: %d sIV:%d\n", sKey.size(), sIV.size());
        return false;
    }

    // Prepare output buffer
    sCiphertext.resize(nCLen);

    // Perform the encryption
    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();

    if (!ctx) return false;

    bool fOk = true;

    EVP_CIPHER_CTX_init(ctx);
    if (fOk) fOk = EVP_EncryptInit_ex(ctx, EVP_aes_256_cbc(), NULL, (const unsigned char*) &sKey[0], (const unsigned char*) &sIV[0]);
    if (fOk) fOk = EVP_EncryptUpdate(ctx, (unsigned char*) &sCiphertext[0], &nCLen, (const unsigned char*) &sPlaintext[0], nLen);
    if (fOk) fOk = EVP_EncryptFinal_ex(ctx, (unsigned char*) (&sCiphertext[0])+nCLen, &nFLen);
    EVP_CIPHER_CTX_cleanup(ctx);

    EVP_CIPHER_CTX_free(ctx);

    if (!fOk) return false;

    sCiphertext.resize(nCLen + nFLen);
    return true;
}

static bool DecryptSecret(const CKeyingMaterial& vMasterKey, const std::vector<unsigned char>& vchCiphertext, const uint256& nIV, CKeyingMaterial& vchPlaintext)
{
    CCrypter cKeyCrypter;
    std::vector<unsigned char> chIV(WALLET_CRYPTO_KEY_SIZE);
    memcpy(&chIV[0], &nIV, WALLET_CRYPTO_KEY_SIZE);
    if(!cKeyCrypter.SetKey(vMasterKey, chIV))
        return false;
    return cKeyCrypter.Decrypt(vchCiphertext, *((CKeyingMaterial*)&vchPlaintext));
}

bool DecryptAES256(const SecureString& sKey, const std::string& sCiphertext, const std::string& sIV, SecureString& sPlaintext)
{
    // plaintext will always be equal to or lesser than length of ciphertext
    int nLen = sCiphertext.size();
    int nPLen = nLen, nFLen = 0;

    // Verify key sizes
    if(sKey.size() != 32 || sIV.size() != AES_BLOCK_SIZE) {
        LogPrintf("crypter DecryptAES256 - Invalid key or block size\n");
        return false;
    }

    sPlaintext.resize(nPLen);

    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();

    if (!ctx) return false;

    bool fOk = true;

    EVP_CIPHER_CTX_init(ctx);
    if (fOk) fOk = EVP_DecryptInit_ex(ctx, EVP_aes_256_cbc(), NULL, (const unsigned char*) &sKey[0], (const unsigned char*) &sIV[0]);
    if (fOk) fOk = EVP_DecryptUpdate(ctx, (unsigned char *) &sPlaintext[0], &nPLen, (const unsigned char *) &sCiphertext[0], nLen);
    if (fOk) fOk = EVP_DecryptFinal_ex(ctx, (unsigned char *) (&sPlaintext[0])+nPLen, &nFLen);
    EVP_CIPHER_CTX_cleanup(ctx);

    EVP_CIPHER_CTX_free(ctx);

    if (!fOk) return false;

    sPlaintext.resize(nPLen + nFLen);
    return true;
}

static bool DecryptKey(const CKeyingMaterial& vMasterKey, const std::vector<unsigned char>& vchCryptedSecret, const CPubKey& vchPubKey, CKey& key)
{
    CKeyingMaterial vchSecret;
    if(!DecryptSecret(vMasterKey, vchCryptedSecret, vchPubKey.GetHash(), vchSecret))
        return false;

    if (vchSecret.size() != 32)
        return false;

    key.Set(vchSecret.begin(), vchSecret.end(), vchPubKey.IsCompressed());
    return key.VerifyPubKey(vchPubKey);
}

/* R ANDREWS - BIBLEPAY - 9/13/2018 - ADD SUPPORT FOR RSA */

int RSA_WRITE_KEY_TO_FILE(FILE *file, int key, EVP_PKEY *rKey)
{
	switch(key)
	{
		case KEY_SERVER_PRI:
			 if(!PEM_write_PrivateKey(file, rKey, NULL, NULL, 0, 0, NULL))         return -1;
			 break;
		case KEY_CLIENT_PRI:
			 if(!PEM_write_PrivateKey(file, rKey, NULL, NULL, 0, 0, NULL))         return -1;
			 break;
		case KEY_SERVER_PUB:
			 if(!PEM_write_PUBKEY(file, rKey))                                     return -1;
			 break;
		case KEY_CLIENT_PUB:
			 if(!PEM_write_PUBKEY(file, rKey))                                     return -1;
			 break;
		default:
			return -1;
	}
	return 1;
}

std::vector<char> ReadAllBytes(char const* filename)
{
    std::ifstream ifs(filename, std::ios::binary|std::ios::ate);
    std::ifstream::pos_type pos = ifs.tellg();
    std::vector<char>  result(pos);
    ifs.seekg(0, std::ios::beg);
    ifs.read(&result[0], pos);
    return result;
}

int RSA_GENERATE_KEYPAIR(std::string sPublicKeyPath, std::string sPrivateKeyPath)
{
	if (sPublicKeyPath.empty() || sPrivateKeyPath.empty()) return -1;
    EVP_PKEY *remotePublicKey = NULL;
	EVP_PKEY_CTX *context = EVP_PKEY_CTX_new_id(EVP_PKEY_RSA, NULL);
	if(EVP_PKEY_keygen_init(context) <= 0) return -1;
	if(EVP_PKEY_CTX_set_rsa_keygen_bits(context, RSA_KEYLEN) <= 0) return -1;
	if(EVP_PKEY_keygen(context, &remotePublicKey) <= 0) return -1;
	EVP_PKEY_CTX_free(context);
    FILE *outFile = fopen(sPublicKeyPath.c_str(), "w");
	RSA_WRITE_KEY_TO_FILE(outFile, KEY_CLIENT_PUB, remotePublicKey);
	fclose(outFile);
	FILE *outFile2 = fopen(sPrivateKeyPath.c_str(), "w");
	RSA_WRITE_KEY_TO_FILE(outFile2, KEY_CLIENT_PRI, remotePublicKey);
	fclose(outFile2);
	return 1;
}

EVP_PKEY *RSA_LOAD_PUBKEY(const char *file)
{
	RSA *rsa_pkey = NULL;
	BIO *rsa_pkey_file = NULL;
	EVP_PKEY *pkey = EVP_PKEY_new();
	// Create a new BIO file structure to be used with PEM file
	rsa_pkey_file = BIO_new(BIO_s_file());
	if (rsa_pkey_file == NULL)
	{
		fprintf(stderr, "RSA_LOAD_PUBKEY::Error creating a new BIO file.\n");
		goto end;
	}

	// Read PEM file using BIO's file structure
	if (BIO_read_filename(rsa_pkey_file, file) <= 0)
	{
		fprintf(stderr, "RSA_LOAD_PUBKEY::Error opening %s\n",file);
		goto end;
	}

	// Read RSA based PEM file into rsa_pkey structure
	if (!PEM_read_bio_RSA_PUBKEY(rsa_pkey_file, &rsa_pkey, NULL, NULL))
	{
		fprintf(stderr, "Error loading RSA Public Key File.\n");
		goto end;
	}

	// Populate pkey with the rsa key. rsa_pkey is owned by pkey, therefore if we free pkey, rsa_pkey will be freed  too
    if (!EVP_PKEY_assign_RSA(pkey, rsa_pkey))
    {
        fprintf(stderr, "Error assigning EVP_PKEY_assign_RSA: failed.\n");
        goto end;
    }

end:
	if (rsa_pkey_file != NULL)
		BIO_free(rsa_pkey_file);
	if (pkey == NULL)
	{
		fprintf(stderr, "RSA_LOAD_PUBKEY::Error unable to load %s\n", file);
	}
	return(pkey);
}

EVP_PKEY *RSA_LOAD_PRIVKEY(const char *file)
{
	RSA *rsa_pkey = NULL;
	BIO *rsa_pkey_file = NULL;
	EVP_PKEY *pkey = EVP_PKEY_new();
	// Create a new BIO file structure to be used with PEM file
	rsa_pkey_file = BIO_new(BIO_s_file());
	if (rsa_pkey_file == NULL)
	{
		fprintf(stderr, "Error creating a new BIO file.\n");
		goto end;
	}
	// Read PEM file using BIO's file structure
	if (BIO_read_filename(rsa_pkey_file, file) <= 0)
	{
		fprintf(stderr, "Error opening %s\n",file);
		goto end;
	}
	// Read RSA based PEM file into rsa_pkey structure
	if (!PEM_read_bio_RSAPrivateKey(rsa_pkey_file, &rsa_pkey, NULL, NULL))
	{
		fprintf(stderr, "Error loading RSA Private Key File.\n");
		goto end;
	}
	// Populate pkey with the rsa key. rsa_pkey is owned by pkey,
	// therefore if we free pkey, rsa_pkey will be freed  too
    if (!EVP_PKEY_assign_RSA(pkey, rsa_pkey))
    {
        fprintf(stderr, "Error assigning EVP_PKEY_assign_RSA: failed.\n");
        goto end;
    }

end:
	if (rsa_pkey_file != NULL)
		BIO_free(rsa_pkey_file);
	if (pkey == NULL)
	{
		fprintf(stderr, "RSA_Load_Private_Key::Error unable to load %s\n", file);
	}
	return(pkey);
}

unsigned char *RSA_ENCRYPT_CHAR(std::string sPubKeyPath, unsigned char *plaintext, int plaintext_length, int& cipher_len, std::string& sError)
{
	EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();
	EVP_CIPHER_CTX_init(ctx);
	EVP_PKEY *pkey;
	unsigned char iv[EVP_MAX_IV_LENGTH];
	unsigned char *encrypted_key;
	int encrypted_key_length;
	uint32_t eklen_n;
	unsigned char *ciphertext;
	// Load RSA Public Key File (usually this is a PEM ASCII file with boundaries or armor)
	pkey = RSA_LOAD_PUBKEY(sPubKeyPath.c_str());
	if (pkey == NULL)
	{
	    ciphertext = (unsigned char*)malloc(1);
		sError = "Error loading public key.";
		return ciphertext;
	}
	encrypted_key = (unsigned char*)malloc(EVP_PKEY_size(pkey));
	encrypted_key_length = EVP_PKEY_size(pkey);
	if (!EVP_SealInit(ctx, EVP_des_ede_cbc(), &encrypted_key, &encrypted_key_length, iv, &pkey, 1))
	{
		fprintf(stdout, "EVP_SealInit: failed.\n");
	}
	eklen_n = htonl(encrypted_key_length);
	int size_header = sizeof(eklen_n) + encrypted_key_length + EVP_CIPHER_iv_length(EVP_des_ede_cbc());
	/* compute max ciphertext len, see man EVP_CIPHER */
	int max_cipher_len = plaintext_length + EVP_CIPHER_CTX_block_size(ctx) - 1;
	ciphertext = (unsigned char*)malloc(size_header + max_cipher_len);
	/* Write out the encrypted key length, then the encrypted key, then the iv (the IV length is fixed by the cipher we have chosen). */
	int pos = 0;
	memcpy(ciphertext + pos, &eklen_n, sizeof(eklen_n));
	pos += sizeof(eklen_n);
	memcpy(ciphertext + pos, encrypted_key, encrypted_key_length);
	pos += encrypted_key_length;
	memcpy(ciphertext + pos, iv, EVP_CIPHER_iv_length(EVP_des_ede_cbc()));
	pos += EVP_CIPHER_iv_length(EVP_des_ede_cbc());
	/* Process the plaintext data and write the encrypted data to the ciphertext. cipher_len is filled with the length of ciphertext generated, len is the size of plaintext in bytes
	 * Also we have our updated position, we can skip the header via ciphertext + pos */
	int total_len = 0;
	int bytes_processed = 0;
	if (!EVP_SealUpdate(ctx, ciphertext + pos, &bytes_processed, plaintext, plaintext_length))
	{
		fprintf(stdout, "EVP_SealUpdate: failed.\n");
	}

	LogPrintf(" precipherlen %f ", bytes_processed);
	total_len += bytes_processed;
	pos += bytes_processed;
	if (!EVP_SealFinal(ctx, ciphertext + pos, &bytes_processed))
	{
		fprintf(stdout, "RSA_Encrypt::EVP_SealFinal: failed.\n");
	}
	total_len += bytes_processed;
	cipher_len = total_len;
	EVP_PKEY_free(pkey);
	free(encrypted_key);
	EVP_CIPHER_CTX_free(ctx);
	return ciphertext;
}

void RSA_Encrypt_File(std::string sPubKeyPath, std::string sSourcePath, std::string sEncryptPath, std::string& sError)
{
	std::vector<char> vMessage = ReadAllBytes(sSourcePath.c_str());
	std::vector<unsigned char> uData(vMessage.begin(), vMessage.end());
	unsigned char *ciphertext;
	int cipher_len = 0;
	unsigned char *long_ciphertext = (unsigned char *)malloc(uData.size() + 10000);
	memcpy(long_ciphertext, &uData[0], uData.size());
	size_t messageLength = uData.size() + 10000;
	ciphertext = RSA_ENCRYPT_CHAR(sPubKeyPath, long_ciphertext, messageLength, cipher_len, sError);
	if (sError.empty())
	{
		std::ofstream fd(sEncryptPath.c_str());
		fd.write((const char*)ciphertext, cipher_len);
		fd.close();
	}
}

unsigned char *RSA_DECRYPT_CHAR(std::string sPriKeyPath, unsigned char *ciphertext, int ciphrtext_size, int& plaintxt_len, std::string& sError)
{
	EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();
	EVP_CIPHER_CTX_init(ctx);
	EVP_PKEY *pkey;
	unsigned char *encrypted_key;
	unsigned int encrypted_key_length;
	uint32_t eklen_n;
	unsigned char iv[EVP_MAX_IV_LENGTH];
	int bytes_processed = 0;
	// the length of udata is at most ciphertextlen + ciphers block size.
	int ciphertext_len = ciphrtext_size + EVP_CIPHER_block_size(EVP_des_ede_cbc());
	unsigned char *plaintext = (unsigned char *)malloc(ciphertext_len);
	pkey = RSA_LOAD_PRIVKEY(sPriKeyPath.c_str());
	if (pkey==NULL)
	{
		sError = "No private key provided.";
		return plaintext;
	}
	encrypted_key = (unsigned char*)malloc(EVP_PKEY_size(pkey));
	int pos = 0;
	memcpy(&eklen_n, ciphertext + pos, sizeof(eklen_n));
	pos += sizeof(eklen_n);
	encrypted_key_length = ntohl(eklen_n);
	memcpy(encrypted_key, ciphertext + pos, encrypted_key_length);
	pos += encrypted_key_length;
	memcpy(iv, ciphertext + pos, EVP_CIPHER_iv_length(EVP_des_ede_cbc()));
	pos += EVP_CIPHER_iv_length(EVP_des_ede_cbc());
	int total_len = 0;
	// Now we have our encrypted_key and the iv we can decrypt the reamining
	if (!EVP_OpenInit(ctx, EVP_des_ede_cbc(), encrypted_key, encrypted_key_length, iv, pkey))
	{
		fprintf(stderr, "RSADecrypt::EVP_OpenInit: failed.\n");
		sError = "EVP_OpenInit Failed.";
		return plaintext;
	}
	if (!EVP_OpenUpdate(ctx, plaintext, &bytes_processed, ciphertext + pos, ciphrtext_size))
	{
		fprintf(stderr, "RSA::Decrypt::EVP_OpenUpdate: failed.\n");
		sError = "EVP_OpenUpdate failed.";
		return plaintext;
	}
	total_len += bytes_processed;
	if (!EVP_OpenFinal(ctx, plaintext + total_len, &bytes_processed))
	{
		fprintf(stderr, "RSA::Decrypt::EVP_OpenFinal warning: failed.\n");
	}
	total_len += bytes_processed;

	EVP_PKEY_free(pkey);
	free(encrypted_key);

	EVP_CIPHER_CTX_free(ctx);
	plaintxt_len = total_len;
	return plaintext;
}

void RSA_Decrypt_File(std::string sPrivKeyPath, std::string sSourcePath, std::string sDecryptPath, std::string sError)
{
	std::vector<char> vMessage = ReadAllBytes(sSourcePath.c_str());
	std::vector<unsigned char> uData(vMessage.begin(), vMessage.end());
	size_t messageLength = uData.size();
	int plaintextsize = 0;
	unsigned char *decrypted;
	decrypted = RSA_DECRYPT_CHAR(sPrivKeyPath, &uData[0], messageLength, plaintextsize, sError);
	if (sError.empty())
	{
		if (plaintextsize < 10000)
		{
			sError = "RSA_Decrypt_File::Encrypted file is corrupted.";
			return;
		}
		unsigned char *short_plaintext = (unsigned char *)malloc(plaintextsize - 10000);
		memcpy(short_plaintext, decrypted, plaintextsize - 10000);
		std::ofstream fd(sDecryptPath.c_str());
		fd.write((const char*)short_plaintext, plaintextsize - 10000);
		fd.close();
	}
}

std::string RSA_Encrypt_String(std::string sPubKeyPath, std::string sData, std::string& sError)
{
	std::string sEncPath =  GetSANDirectory2() + "temp";
	std::ofstream fd(sEncPath.c_str());
	fd.write(sData.c_str(), sizeof(char)*sData.size());
	fd.close();
	std::string sTargPath = GetSANDirectory2() + "temp_enc";
	RSA_Encrypt_File(sPubKeyPath, sEncPath, sTargPath, sError);
	if (!sError.empty()) return "";
	std::vector<char> vOut = ReadAllBytes(sTargPath.c_str());
	std::string sResults(vOut.begin(), vOut.end());
	return sResults;
}

std::string RSA_Decrypt_String(std::string sPrivKeyPath, std::string sData, std::string& sError)
{
	std::string sEncPath =  GetSANDirectory2() + "temp_dec";
	std::ofstream fd(sEncPath.c_str());
	fd.write(sData.c_str(), sizeof(char)*sData.size());

	fd.close();
	std::string sTargPath = GetSANDirectory2() + "temp_dec_unenc";
	RSA_Decrypt_File(sPrivKeyPath, sEncPath, sTargPath, sError);
	if (!sError.empty()) return "";
	std::vector<char> vOut = ReadAllBytes(sTargPath.c_str());
	std::string sResults(vOut.begin(), vOut.end());
	return sResults;
}

/* END OF RSA SUPPORT - BIBLEPAY */

bool CCryptoKeyStore::SetCrypted()
{
    LOCK(cs_KeyStore);
    if (fUseCrypto)
        return true;
    if (!mapKeys.empty())
        return false;
    fUseCrypto = true;
    return true;
}

bool CCryptoKeyStore::Lock(bool fAllowMixing)
{
    if (!SetCrypted())
        return false;

    if(!fAllowMixing) {
        LOCK(cs_KeyStore);
        vMasterKey.clear();
    }

    fOnlyMixingAllowed = fAllowMixing;
    NotifyStatusChanged(this);
    return true;
}

bool CCryptoKeyStore::Unlock(const CKeyingMaterial& vMasterKeyIn, bool fForMixingOnly)
{
    {
        LOCK(cs_KeyStore);
        if (!SetCrypted())
            return false;

        bool keyPass = false;
        bool keyFail = false;
        CryptedKeyMap::const_iterator mi = mapCryptedKeys.begin();
        for (; mi != mapCryptedKeys.end(); ++mi)
        {
            const CPubKey &vchPubKey = (*mi).second.first;
            const std::vector<unsigned char> &vchCryptedSecret = (*mi).second.second;
            CKey key;
            if (!DecryptKey(vMasterKeyIn, vchCryptedSecret, vchPubKey, key))
            {
                keyFail = true;
                break;
            }
            keyPass = true;
            if (fDecryptionThoroughlyChecked)
                break;
        }
        if (keyPass && keyFail)
        {
            LogPrintf("The wallet is probably corrupted: Some keys decrypt but not all.\n");
            assert(false);
        }
        if (keyFail || !keyPass)
            return false;
        vMasterKey = vMasterKeyIn;
        fDecryptionThoroughlyChecked = true;
    }
    fOnlyMixingAllowed = fForMixingOnly;
    NotifyStatusChanged(this);
    return true;
}

bool CCryptoKeyStore::AddKeyPubKey(const CKey& key, const CPubKey &pubkey)
{
    {
        LOCK(cs_KeyStore);
        if (!IsCrypted())
            return CBasicKeyStore::AddKeyPubKey(key, pubkey);

        if (IsLocked(true))
            return false;

        std::vector<unsigned char> vchCryptedSecret;
        CKeyingMaterial vchSecret(key.begin(), key.end());
        if (!EncryptSecret(vMasterKey, vchSecret, pubkey.GetHash(), vchCryptedSecret))
            return false;

        if (!AddCryptedKey(pubkey, vchCryptedSecret))
            return false;
    }
    return true;
}

bool CCryptoKeyStore::AddCryptedKey(const CPubKey &vchPubKey, const std::vector<unsigned char> &vchCryptedSecret)
{
    {
        LOCK(cs_KeyStore);
        if (!SetCrypted())
            return false;

        mapCryptedKeys[vchPubKey.GetID()] = make_pair(vchPubKey, vchCryptedSecret);
    }
    return true;
}

bool CCryptoKeyStore::GetKey(const CKeyID &address, CKey& keyOut) const
{
    {
        LOCK(cs_KeyStore);
        if (!IsCrypted())
            return CBasicKeyStore::GetKey(address, keyOut);

        CryptedKeyMap::const_iterator mi = mapCryptedKeys.find(address);
        if (mi != mapCryptedKeys.end())
        {
            const CPubKey &vchPubKey = (*mi).second.first;
            const std::vector<unsigned char> &vchCryptedSecret = (*mi).second.second;
            return DecryptKey(vMasterKey, vchCryptedSecret, vchPubKey, keyOut);
        }
    }
    return false;
}

bool CCryptoKeyStore::GetPubKey(const CKeyID &address, CPubKey& vchPubKeyOut) const
{
    {
        LOCK(cs_KeyStore);
        if (!IsCrypted())
            return CBasicKeyStore::GetPubKey(address, vchPubKeyOut);

        CryptedKeyMap::const_iterator mi = mapCryptedKeys.find(address);
        if (mi != mapCryptedKeys.end())
        {
            vchPubKeyOut = (*mi).second.first;
            return true;
        }
        // Check for watch-only pubkeys
        return CBasicKeyStore::GetPubKey(address, vchPubKeyOut);
    }
    return false;
}

bool CCryptoKeyStore::EncryptKeys(CKeyingMaterial& vMasterKeyIn)
{
    {
        LOCK(cs_KeyStore);
        if (!mapCryptedKeys.empty() || IsCrypted())
            return false;

        fUseCrypto = true;
        BOOST_FOREACH(KeyMap::value_type& mKey, mapKeys)
        {
            const CKey &key = mKey.second;
            CPubKey vchPubKey = key.GetPubKey();
            CKeyingMaterial vchSecret(key.begin(), key.end());
            std::vector<unsigned char> vchCryptedSecret;
            if (!EncryptSecret(vMasterKeyIn, vchSecret, vchPubKey.GetHash(), vchCryptedSecret))
                return false;
            if (!AddCryptedKey(vchPubKey, vchCryptedSecret))
                return false;
        }
        mapKeys.clear();
    }
    return true;
}


//==============================================================================
//         Copyright 2014        LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2014        LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2014        NumScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_FAST_TOINT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_FAST_TOINT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/arithmetic/functions/fast_toint.hpp>
#include <boost/simd/include/functions/simd/split_low.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_toint_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_< double_<A0>
                                             , boost::simd::tag::sse_
                                             >
                                       ))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    typedef typename dispatch::meta::downgrade<result_type>::type int_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const int_type out = _mm_cvttpd_epi32(a0);
      return boost::simd::split_low(out);
    }
  };
} } }

#endif
#endif


/*
 * Remember to turn off CPU frequency scaling before testing.
 */

#include <polarssl/ctr_drbg.h>
#include <polarssl/error.h>
#include <polarssl/entropy.h>
#include <polarssl/ssl.h>

#include <polarssl/certs.h>

#include <muduo/base/Thread.h>

#include <boost/bind.hpp>

#include <stdio.h>
#include <sys/socket.h>
#include <sys/time.h>

bool useRSA = false;
bool useECDHE = false;
const int N = 500;

double now()
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return tv.tv_sec + tv.tv_usec / 1000000.0;
}

// FIXME: net_recv with buffer

void clientThread(entropy_context* entropy, int* clientFd)
{
  ctr_drbg_context ctr_drbg;
  ctr_drbg_init(&ctr_drbg, entropy_func, entropy, NULL, 0);

  ssl_context ssl;
  bzero(&ssl, sizeof ssl);
  ssl_init(&ssl);
  ssl_set_rng(&ssl, ctr_drbg_random, &ctr_drbg);
  ssl_set_bio(&ssl, &net_recv, clientFd, &net_send, clientFd);
  ssl_set_endpoint(&ssl, SSL_IS_CLIENT);
  ssl_set_authmode(&ssl, SSL_VERIFY_NONE);

  for (int i = 0; i < N; ++i)
  {
    ssl_session_reset( &ssl );
    int ret = 0;
    while ( (ret = ssl_handshake(&ssl)) != 0)
    {
      if (ret != POLARSSL_ERR_NET_WANT_READ && ret != POLARSSL_ERR_NET_WANT_WRITE)
      {
        printf("client handshake failed %d\n", ret);
        break;
      }
    }
    if (i == 0)
      printf("client done %s %s\n", ssl_get_version(&ssl), ssl_get_ciphersuite(&ssl));
  }

  ssl_free(&ssl);
}

void serverThread(entropy_context* entropy, int* serverFd)
{
  const char* srv_cert = test_srv_crt_ec;
  const char* srv_key = test_srv_key_ec;
  if (useRSA)
  {
    srv_cert = test_srv_crt;
    srv_key = test_srv_key;
  }
  x509_crt cert;
  x509_crt_init(&cert);
  x509_crt_parse(&cert, reinterpret_cast<const unsigned char*>(srv_cert), strlen(srv_cert));
  x509_crt_parse(&cert, reinterpret_cast<const unsigned char*>(test_ca_list), strlen(test_ca_list));

  pk_context pkey;
  pk_init(&pkey);
  pk_parse_key(&pkey, reinterpret_cast<const unsigned char*>(srv_key), strlen(srv_key), NULL, 0);

  ctr_drbg_context ctr_drbg;
  ctr_drbg_init(&ctr_drbg, entropy_func, entropy, NULL, 0);

  ssl_context ssl_server;
  bzero(&ssl_server, sizeof ssl_server);
  ssl_init(&ssl_server);
  ssl_set_rng(&ssl_server, ctr_drbg_random, &ctr_drbg);
  ssl_set_bio(&ssl_server, &net_recv, serverFd, &net_send, serverFd);
  ssl_set_endpoint(&ssl_server, SSL_IS_SERVER);
  ssl_set_authmode(&ssl_server, SSL_VERIFY_NONE);
  ssl_set_ca_chain(&ssl_server, cert.next, NULL, NULL);
  ssl_set_own_cert(&ssl_server, &cert, &pkey);
  // ssl_set_dbg(&ssl_server, my_debug, (void*)"server");
  ecp_group_id curves[] = { POLARSSL_ECP_DP_SECP256R1, POLARSSL_ECP_DP_NONE };
  ssl_set_curves(&ssl_server, curves);
  int ciphersuites[] = { TLS_ECDH_ECDSA_WITH_AES_256_CBC_SHA, TLS_RSA_WITH_AES_256_CBC_SHA, 0 };
  if (!useECDHE)
    ssl_set_ciphersuites(&ssl_server, ciphersuites);

  for (int i = 0; i < N; ++i)
  {
    ssl_session_reset(&ssl_server);
    int ret = 0;
    while ( (ret = ssl_handshake(&ssl_server)) != 0)
    {
      if (ret != POLARSSL_ERR_NET_WANT_READ && ret != POLARSSL_ERR_NET_WANT_WRITE)
      {
        printf("server handshake failed %d\n", ret);
        break;
      }
    }
    if (i == 0)
      printf("server done %s %s\n", ssl_get_version(&ssl_server), ssl_get_ciphersuite(&ssl_server));
  }

  ssl_free(&ssl_server);
  pk_free(&pkey);
  x509_crt_free(&cert);
}

int main(int argc, char* argv[])
{
  unsigned char buf[16384] = { 0 };
  entropy_context entropy;
  entropy_init(&entropy);

  if (argc > 1)
    useRSA = true;

  useECDHE = argc > 2;

  int fds[2];
  if (::socketpair(AF_UNIX, SOCK_STREAM, 0, fds))
    abort();

  double start = now();
  muduo::Thread client(boost::bind(&clientThread, &entropy, &fds[0]), "ssl client");
  muduo::Thread server(boost::bind(&serverThread, &entropy, &fds[1]), "ssl server");
  client.start();
  server.start();

  client.join();
  server.join();
  double elapsed = now() - start;
  printf("%.2fs %.1f handshakes/s\n", elapsed, N / elapsed);
  entropy_free(&entropy);
}


#include "vector.h"
#include "MinIMU9.h"
#include "version.h"
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <system_error>
#include <boost/program_options.hpp>

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"

#define LOOP_RATE 10

namespace opts = boost::program_options;

// TODO: print warning if accelerometer magnitude is not close to 1 when starting up

// An Euler angle could take 8 chars: -234.678, but usually we only need 6.
float field_width = 6;
#define FLOAT_FORMAT std::fixed << std::setprecision(3) << std::setw(field_width)

std::ostream & operator << (std::ostream & os, const vector & vector)
{
    return os << FLOAT_FORMAT << vector(0) << ' '
              << FLOAT_FORMAT << vector(1) << ' '
              << FLOAT_FORMAT << vector(2);
}

std::ostream & operator << (std::ostream & os, const matrix & matrix)
{
    return os << (vector)matrix.row(0) << ' '
              << (vector)matrix.row(1) << ' '
              << (vector)matrix.row(2);
}

std::ostream & operator << (std::ostream & os, const quaternion & quat)
{
    return os << FLOAT_FORMAT << quat.w() << ' '
              << FLOAT_FORMAT << quat.x() << ' '
              << FLOAT_FORMAT << quat.y() << ' '
              << FLOAT_FORMAT << quat.z();
}

typedef void rotation_output_function(quaternion& rotation);

void output_quaternion(quaternion & rotation)
{
    std::cout << rotation;
}

void output_matrix(quaternion & rotation)
{
    std::cout << rotation.toRotationMatrix();
}

void output_euler(quaternion & rotation)
{
    std::cout << (vector)(rotation.toRotationMatrix().eulerAngles(2, 1, 0) * (180 / M_PI));
}

int millis()
{
/*    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (tv.tv_sec) * 1000 + (tv.tv_usec)/1000;*/
    return ros::Time::now().toSec();
}

void streamRawValues(IMU& imu)
{
    imu.enable();
    while(1)
    {
        imu.read();
        printf("%7d %7d %7d  %7d %7d %7d  %7d %7d %7d\n",
               imu.raw_m[0], imu.raw_m[1], imu.raw_m[2],
               imu.raw_a[0], imu.raw_a[1], imu.raw_a[2],
               imu.raw_g[0], imu.raw_g[1], imu.raw_g[2]
        );
        usleep(20*1000);
    }
}

//! Uses the acceleration and magnetic field readings from the compass
// to get a noisy estimate of the current rotation matrix.
// This function is where we define the coordinate system we are using
// for the ground coords:  North, East, Down.
matrix rotationFromCompass(const vector& acceleration, const vector& magnetic_field)
{
    vector down = -acceleration;     // usually true
    vector east = down.cross(magnetic_field); // actually it's magnetic east
    vector north = east.cross(down);

    east.normalize();
    north.normalize();
    down.normalize();

    matrix r;
    r.row(0) = north;
    r.row(1) = east;
    r.row(2) = down;
    return r;
}

typedef void fuse_function(quaternion& rotation, float dt, const vector& angular_velocity,
                  const vector& acceleration, const vector& magnetic_field);

void fuse_compass_only(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    // Implicit conversion of rotation matrix to quaternion.
    rotation = rotationFromCompass(acceleration, magnetic_field);
}

// Uses the given angular velocity and time interval to calculate
// a rotation and applies that rotation to the given quaternion.
// w is angular velocity in radians per second.
// dt is the time.
void rotate(quaternion& rotation, const vector& w, float dt)
{
    // Multiply by first order approximation of the
    // quaternion representing this rotation.
    rotation *= quaternion(1, w(0)*dt/2, w(1)*dt/2, w(2)*dt/2);
    rotation.normalize();
}

void fuse_gyro_only(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    rotate(rotation, angular_velocity, dt);
}

void fuse_default(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    vector correction = vector(0, 0, 0);

    if (abs(acceleration.norm() - 1) <= 0.3)
    {
        // The magnetidude of acceleration is close to 1 g, so
        // it might be pointing up and we can do drift correction.

        const float correction_strength = 1;

        matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);
        matrix rotationMatrix = rotation.toRotationMatrix();

        correction = (
            rotationCompass.row(0).cross(rotationMatrix.row(0)) +
            rotationCompass.row(1).cross(rotationMatrix.row(1)) +
            rotationCompass.row(2).cross(rotationMatrix.row(2))
          ) * correction_strength;

    }

    rotate(rotation, angular_velocity + correction, dt);
}

void ahrs(IMU & imu, fuse_function * fuse, rotation_output_function * output)
{
    imu.loadCalibration();
    imu.enable();
    imu.measureOffsets();

    // The quaternion that can convert a vector in body coordinates
    // to ground coordinates when it its changed to a matrix.
    quaternion rotation = quaternion::Identity();

    int start = millis(); // truncate 64-bit return value
    ros::NodeHandle nh;
    ros::Publisher imu_publisher = nh.advertise<sensor_msgs::Imu>("imu", 1);
    ros::Publisher imu_raw_publisher = nh.advertise<sensor_msgs::Imu>("imu/raw", 1);
    ros::Publisher magnetic_field_publisher = nh.advertise<sensor_msgs::MagneticField>("magnetic_field", 1);
    ros::Rate rate(LOOP_RATE);
    while(ros::ok())
    {
        int last_start = start;
        start = millis();
        float dt = (start-last_start)/1000.0;
        if (dt < 0){ throw std::runtime_error("Time went backwards."); }

        vector angular_velocity = imu.readGyro();
        vector acceleration = imu.readAcc();
        vector magnetic_field = imu.readMag();

        ros::Time generated_message_time = ros::Time::now();

        sensor_msgs::MagneticField magnetic_field_raw;
        magnetic_field_raw.header.stamp = generated_message_time;
        magnetic_field_raw.header.frame_id = "imu_link"
        magnetic_field_raw.magnetic_field.x = magnetic_field[0];
        magnetic_field_raw.magnetic_field.y = magnetic_field[1];
        magnetic_field_raw.magnetic_field.z = magnetic_field[2];

        sensor_msgs::IMU imu_msg_raw;
        imu_msg_raw.header.stamp = generated_message_time;
        imu_msg_raw.header.frame_id = "imu_link"
        imu_msg_raw.linear_acceleration.x = acceleration[0];
        imu_msg_raw.linear_acceleration.y = acceleration[1];
        imu_msg_raw.linear_acceleration.z = acceleration[2];

        imu_msg_raw.angular_velocity.x = angular_velocity[0];
        imu_msg_raw.angular_velocity.y = angular_velocity[1];
        imu_msg_raw.angular_velocity.z = angular_velocity[2];

        fuse(rotation, dt, angular_velocity, acceleration, magnetic_field);

        sensor_msgs::IMU imu_msg;
        imu_msg.header.stamp = generated_message_time;
        imu_msg.header.frame_id = "imu_link"
        imu_msg.orientation.x = rotation.x()
        imu_msg.orientation.y = rotation.y()
        imu_msg.orientation.z = rotation.z()
        imu_msg.orientation.w = rotation.w()

        imu_msg.angular_velocity.x = angular_velocity[0];
        imu_msg.angular_velocity.y = angular_velocity[1];
        imu_msg.angular_velocity.z = angular_velocity[2];

        imu_msg.linear_acceleration.x = acceleration[0];
        imu_msg.linear_acceleration.y = acceleration[1];
        imu_msg.linear_acceleration.z = acceleration[2];

        imu_raw_publisher.publish(imu_msg_raw);
        magnetic_field_publisher.publish(magnetic_field_raw);
        imu_publisher.publish(imu_msg);

        //output(rotation);
        //std::cout << "  " << acceleration << "  " << magnetic_field << std::endl << std::flush;

        // Ensure that each iteration of the loop takes at least 20 ms.
        /*while(millis() - start < 20)
        {
            usleep(1000);
        }*/

        ros::spinOnce(); // Need to call this function often to allow ROS to process incoming messages
        rate.sleep(); // Sleep for the rest of the cycle, to enforce the FSM loop rate
    }
}

int main(int argc, char *argv[])
{
    try
    {
        // Define what all the command-line parameters are.
        std::string mode, output_mode, i2cDevice;
        opts::options_description desc("Allowed options");
        desc.add_options()
            ("help,h", "produce help message")
            ("version,v", "print version number")
            ("i2c-bus,b", opts::value<std::string>(&i2cDevice)->default_value("/dev/i2c-0"),
             "i2c-bus the IMU is connected to")
            ("mode", opts::value<std::string>(&mode)->default_value("normal"),
             "specifies what algorithm to use.\n"
             "normal: Fuse compass and gyro.\n"
             "gyro-only:  Use only gyro (drifts).\n"
             "compass-only:  Use only compass (noisy).\n"
             "raw: Just print raw values from sensors.")
            ("output", opts::value<std::string>(&output_mode)->default_value("matrix"),
             "specifies how to output the orientation.\n"
             "matrix: Direction Cosine Matrix.\n"
             "quaternion: Quaternion.\n"
             "euler: Euler angles (yaw, pitch, roll).\n")
            ;
        opts::variables_map options;
        opts::store(opts::command_line_parser(argc, argv).options(desc).run(), options);
        opts::notify(options);

        if(options.count("help"))
        {
            std::cout << desc << std::endl;
            std::cout << "For more information, run: man minimu9-ahrs" << std::endl;
            return 0;
        }

        if (options.count("version"))
        {
            std::cout << VERSION << std::endl;
            return 0;
        }

        MinIMU9 imu(i2cDevice.c_str());

        rotation_output_function * output;

        // Figure out the output mode.
        if (output_mode == "matrix")
        {
            output = &output_matrix;
        }
        else if (output_mode == "quaternion")
        {
            output = &output_quaternion;
        }
        else if (output_mode == "euler")
        {
            field_width += 2;  // See comment above for field_width.
            output = &output_euler;
        }
        else
        {
            std::cerr << "Unknown output mode '" << output_mode << "'" << std::endl;
            return 1;
        }

        ros::init(argc, argv, "minimu9_ahrs");

        // Figure out the basic operating mode and start running.
        if (mode == "raw")
        {
            streamRawValues(imu);
        }
        else if (mode == "gyro-only")
        {
            ahrs(imu, &fuse_gyro_only, output);
        }
        else if (mode == "compass-only")
        {
            ahrs(imu, &fuse_compass_only, output);
        }
        else if (mode == "normal")
        {
            ahrs(imu, &fuse_default, output);
        }
        else
        {
            std::cerr << "Unknown mode '" << mode << "'" << std::endl;
            return 1;
        }
        return 0;
    }
    catch(const std::system_error & error)
    {
        std::string what = error.what();
        const std::error_code & code = error.code();
        std::cerr << "Error: " << what << "  " << code.message() << " (" << code << ")" << std::endl;
        return 2;
    }
    catch(const opts::multiple_occurrences & error)
    {
        std::cerr << "Error: " << error.what() << " of " << error.get_option_name() << " option." << std::endl;
        return 1;
    }
    catch(const std::exception & error)
    {
        std::cerr << "Error: " << error.what() << std::endl;
        return 9;
    }
}


// Boost.Geometry

// Copyright (c) 2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_INDEX_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_INDEX_SPHERICAL_HPP

// TEMP - move to strategy
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/spherical/distance_segment_box.hpp>

#include <boost/geometry/strategies/index/services.hpp>
#include <boost/geometry/strategies/relate/spherical.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace index
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename RadiusTypeOrSphere, typename CalculationType>
class spherical
    : public strategies::relate::detail::spherical<RadiusTypeOrSphere, CalculationType>
{
    using base_t = strategies::relate::detail::spherical<RadiusTypeOrSphere, CalculationType>;

public:
    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                    && util::is_pointlike<Geometry2>::value
                                > * = nullptr) const
    {
        //return geometry::strategy::distance::comparable::haversine
        return geometry::strategy::distance::haversine
            <
                typename base_t::radius_type, CalculationType
            >(base_t::radius());
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    (util::is_pointlike<Geometry1>::value
                                    && util::is_segment<Geometry2>::value)
                                    || (util::is_segment<Geometry1>::value
                                    && util::is_pointlike<Geometry2>::value)
                                > * = nullptr) const
    {
        //return geometry::strategy::distance::comparable::cross_track
        return geometry::strategy::distance::cross_track
            <
                CalculationType,
                //geometry::strategy::distance::comparable::haversine
                geometry::strategy::distance::haversine
                    <
                        typename base_t::radius_type, CalculationType
                    >
            >(base_t::radius());
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    (util::is_pointlike<Geometry1>::value
                                    && util::is_box<Geometry2>::value)
                                    || (util::is_box<Geometry1>::value
                                    && util::is_pointlike<Geometry2>::value)
                                > * = nullptr) const
    {
        //return geometry::strategy::distance::comparable::cross_track_point_box
        return geometry::strategy::distance::cross_track_point_box
            <
                CalculationType,
                //geometry::strategy::distance::comparable::haversine
                geometry::strategy::distance::haversine
                    <
                        typename base_t::radius_type, CalculationType
                    >
            >(base_t::radius());
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    (util::is_segment<Geometry1>::value
                                    && util::is_box<Geometry2>::value)
                                    || (util::is_box<Geometry1>::value
                                        && util::is_segment<Geometry2>::value)
                                > * = nullptr) const
    {
        return geometry::strategy::distance::spherical_segment_box
            <
                CalculationType,
                //geometry::strategy::distance::comparable::haversine
                geometry::strategy::distance::haversine
                    <
                        typename base_t::radius_type, CalculationType
                    >
            // TODO:
            >(/*base_t::radius()*/);
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    util::is_segment<Geometry1>::value
                                    && util::is_segment<Geometry2>::value
                                > * = nullptr) const
    {
        //return geometry::strategy::distance::comparable::cross_track
        return geometry::strategy::distance::cross_track
            <
                CalculationType,
                //geometry::strategy::distance::comparable::haversine
                geometry::strategy::distance::haversine
                    <
                        typename base_t::radius_type, CalculationType
                    >
            >(base_t::radius());
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

template <typename CalculationType = void>
class spherical
    : public strategies::index::detail::spherical<void, CalculationType>
{};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, spherical_tag>
{
    using type = strategies::index::spherical<>;
};

template <typename Geometry>
struct default_strategy<Geometry, spherical_equatorial_tag>
{
    using type = strategies::index::spherical<>;
};

template <typename Geometry>
struct default_strategy<Geometry, spherical_polar_tag>
{
    using type = strategies::index::spherical<>;
};

// TEMP - needed in distance until umbrella strategies are supported
template <typename CalculationType, typename SubStrategy>
struct strategy_converter<strategy::distance::comparable::cross_track<CalculationType, SubStrategy>>
{
    static auto get(strategy::distance::comparable::cross_track<CalculationType, SubStrategy> const& )
    {
        return strategies::index::spherical<CalculationType>();
    }
};
// TEMP - needed in distance until umbrella strategies are supported
template <typename RadiusOrSphere, typename CalculationType>
struct strategy_converter<strategy::distance::comparable::haversine<RadiusOrSphere, CalculationType>>
{
    static auto get(strategy::distance::comparable::haversine<RadiusOrSphere, CalculationType> const&)
    {
        return strategies::index::spherical<CalculationType>();
    }
};

} // namespace services

}}}} // namespace boost::geometry::strategy::index

#endif // BOOST_GEOMETRY_STRATEGIES_INDEX_SPHERICAL_HPP


/*
 * Copyright (C) 2005-2017 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * 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 "itkNumericTraits.h"

#include "otbSailModel.h"
#include "otb_boost_expint_header.h"
#include <boost/shared_ptr.hpp>
#include "otbMath.h"

//TODO check EPSILON matlab
#define EPSILON 0.0000000000000000000000001

namespace otb
{

/** Constructor */
SailModel
::SailModel() : m_LAI(2), m_Angl(50), m_PSoil(1), m_Skyl(70), m_HSpot(0.2),
                m_TTS(30), m_TTO(0), m_PSI(0), m_FCoverView(0.0), m_UseSoilFile(false), m_SoilIndex(0)
{
  this->ProcessObject::SetNumberOfRequiredInputs(2);
  this->ProcessObject::SetNumberOfRequiredOutputs(4);

  SpectralResponseType::Pointer vRefl = static_cast<SpectralResponseType *>(this->MakeOutput(0).GetPointer());
  this->itk::ProcessObject::SetNthOutput(0, vRefl.GetPointer());

  SpectralResponseType::Pointer hRefl = static_cast<SpectralResponseType *>(this->MakeOutput(1).GetPointer());
  this->itk::ProcessObject::SetNthOutput(1, hRefl.GetPointer());

  SpectralResponseType::Pointer vAbs = static_cast<SpectralResponseType *>(this->MakeOutput(2).GetPointer());
  this->itk::ProcessObject::SetNthOutput(2, vAbs.GetPointer());

  SpectralResponseType::Pointer hAbs = static_cast<SpectralResponseType *>(this->MakeOutput(3).GetPointer());
  this->itk::ProcessObject::SetNthOutput(3, hAbs.GetPointer());
}

/** Destructor */
SailModel
::~SailModel()
{}

/** Set/Get input reflectance */
void
SailModel
::SetReflectance(const SpectralResponseType * object)
{
  this->itk::ProcessObject::SetNthInput(0, const_cast<SpectralResponseType *>(object));
}

SailModel::SpectralResponseType *
SailModel
::GetReflectance()
                  {
                    if(this->GetNumberOfInputs() != 2)
                      {
                      //exit
                      return nullptr;
                      }
                    return static_cast<SpectralResponseType *>(this->itk::ProcessObject::GetInput(0));
                  }

/** Set/Get input transmittance */
                  void
                  SailModel
                  ::SetTransmittance(const SpectralResponseType * object)
                  {
                    this->itk::ProcessObject::SetNthInput(1, const_cast<SpectralResponseType *>(object));
                  }

                  SailModel::SpectralResponseType *
                  SailModel
                  ::GetTransmittance()
                  {
                    if(this->GetNumberOfInputs() != 2)
                      {
                      //exit
                      return nullptr;
                      }
                    return static_cast<SpectralResponseType *>(this->itk::ProcessObject::GetInput(1));
                  }

/** Make output */
                  SailModel::DataObjectPointer
                  SailModel
                  ::MakeOutput(DataObjectPointerArraySizeType)
                  {
                    return static_cast<itk::DataObject*>(SpectralResponseType::New().GetPointer());
                  }

/** Get output viewing reflectance */
                  SailModel::SpectralResponseType *
                  SailModel
                  ::GetViewingReflectance()
                  {
                    if(this->GetNumberOfOutputs() < 4)
                      {
                      //exit
                      return nullptr;
                      }
                    return static_cast<SpectralResponseType *>(this->itk::ProcessObject::GetOutput(0));
                  }

/** Get output hemispherical reflectance */
                  SailModel::SpectralResponseType *
                  SailModel
                  ::GetHemisphericalReflectance()
                  {
                    if(this->GetNumberOfOutputs() < 4)
                      {
                      //exit
                      return nullptr;
                      }
                    return static_cast<SpectralResponseType *>(this->itk::ProcessObject::GetOutput(1));
                  }

/** Get output viewing absorptance */
                  SailModel::SpectralResponseType *
                  SailModel
                  ::GetViewingAbsorptance()
                  {
                    if(this->GetNumberOfOutputs() < 4)
                      {
                      //exit
                      return nullptr;
                      }
                    return static_cast<SpectralResponseType *>(this->itk::ProcessObject::GetOutput(2));
                  }

/** Get output hemispherical absorptance */
                  SailModel::SpectralResponseType *
                  SailModel
                  ::GetHemisphericalAbsorptance()
                  {
                    if(this->GetNumberOfOutputs() < 4)
                      {
                      //exit
                      return nullptr;
                      }
                    return static_cast<SpectralResponseType *>(this->itk::ProcessObject::GetOutput(3));
                  }

/** Set Parameters */
                  void
                  SailModel
                  ::SetInput(const ParametersType & params)
                  {

                    if(params.Size()!=8) itkExceptionMacro( << "Must have 8 parameters in that order : LAI, Angl, PSoil, Skyl, HSpot, TTS, TTO, PSI" );
                    this->SetParameters(params);
                    m_LAI=params[0];
                    m_Angl=params[1];
   m_PSoil=params[2];
   m_Skyl=params[3];
   m_HSpot=params[4];
   m_TTS=params[5];
   m_TTO=params[6];
   m_PSI=params[7];
}

/** Get Parameters */
const SailModel::ParametersType
SailModel
::GetInput()
{
  ParametersType parameters=this->GetParameters();
   if(parameters.Size()!=8)
   {
      parameters[0]=m_LAI;
      parameters[1]=m_Angl;
      parameters[2]=m_PSoil;
      parameters[3]=m_Skyl;
      parameters[4]=m_HSpot;
      parameters[5]=m_TTS;
      parameters[6]=m_TTO;
      parameters[7]=m_PSI;
      this->SetParameters(parameters);
   }
   return this->GetParameters();
}

/** Generate data */
void
SailModel
::GenerateData()
{

   SpectralResponseType::Pointer inRefl = this->GetReflectance();
   SpectralResponseType::Pointer inTrans = this->GetTransmittance();
   SpectralResponseType::Pointer outVRefl = this->GetViewingReflectance();
   SpectralResponseType::Pointer outHRefl = this->GetHemisphericalReflectance();
   SpectralResponseType::Pointer outVAbs = this->GetViewingAbsorptance();
   SpectralResponseType::Pointer outHAbs = this->GetHemisphericalAbsorptance();

   // LEAF ANGLE DISTRIBUTION
   double rd = CONST_PI/180;
   VectorType lidf;
   this->Calc_LIDF(m_Angl, lidf);

   double cts, cto, ctscto, tants, tanto, cospsi, dso;
   cts = std::cos(rd*m_TTS);
   cto = std::cos(rd*m_TTO);
   ctscto = cts*cto;
   tants = std::tan(rd*m_TTS);
   tanto = std::tan(rd*m_TTO);
   cospsi = std::cos(rd*m_PSI);
   dso = std::sqrt(tants*tants+tanto*tanto-2.*tants*tanto*cospsi);

   // angular distance, compensation of shadow length
   // Calculate geometric factors associated with extinction and scattering
   // Initialise sums
   double ks = 0;
   double ko = 0;
   double bf = 0;
   double sob = 0;
   double sof = 0;
   double ttl, ctl, ksli, koli, sobli, sofli, bfli;
   double chi_s, chi_o, frho, ftau;
   VectorType result(4);

   // Weighted sums over LIDF
   for(unsigned int i=0; i<lidf.size(); ++i)
   {
      ttl = 2.5+5*i;       // leaf inclination discrete values
      ctl = std::cos(rd*ttl);
      // SAIL volume scattering phase function gives interception and portions to be
      // multiplied by rho and tau

      this->Volscatt(m_TTS, m_TTO, m_PSI, ttl, result);
      chi_s = result[0];
      chi_o = result[1];
      frho = result[2];
      ftau = result[3];

      //********************************************************************************
      //*                   SUITS SYSTEM COEFFICIENTS
      //*
      //*       ks  : Extinction coefficient for direct solar flux
      //*       ko  : Extinction coefficient for direct observed flux
      //*       att : Attenuation coefficient for diffuse flux
      //*       sigb : Backscattering coefficient of the diffuse downward flux
      //*       sigf : Forwardscattering coefficient of the diffuse upward flux
      //*       sf  : Scattering coefficient of the direct solar flux for downward diffuse flux
      //*       sb  : Scattering coefficient of the direct solar flux for upward diffuse flux
      //*       vf   : Scattering coefficient of upward diffuse flux in the observed direction
      //*       vb   : Scattering coefficient of downward diffuse flux in the observed direction
      //*       w   : Bidirectional scattering coefficient
      //********************************************************************************

      // Extinction coefficients
      ksli = chi_s/cts;
      koli = chi_o/cto;

      // Area scattering coefficient fractions
      sobli       = frho*CONST_PI/ctscto;
      sofli       = ftau*CONST_PI/ctscto;
      bfli       = ctl*ctl;
      ks       = ks+ksli*lidf[i];
      ko       = ko+koli*lidf[i];
      bf       = bf+bfli*lidf[i];
      sob       = sob+sobli*lidf[i];
      sof       = sof+sofli*lidf[i];
   }

   // Geometric factors to be used later with rho and tau
   double sdb, sdf, dob, dof, ddb, ddf;
   sdb       = 0.5*(ks+bf);
   sdf       = 0.5*(ks-bf);
   dob       = 0.5*(ko+bf);
   dof       = 0.5*(ko-bf);
   ddb       = 0.5*(1.+bf);
   ddf       = 0.5*(1.-bf);

   double lambda, Es, Ed, Rsoil1, Rsoil2, rsoil0, rho, tau, PARdiro, PARdifo;
   double sigb, sigf, att, m2, m, sb, sf, vb, vf, w;
   double tss, too, tsstoo, rdd, tdd, rsd, tsd, rdo, tdo, rsos, rsod;
   double rddt, rsdt, rdot, rsodt, rsost, rsot, dn;
   double e1, e2, rinf, rinf2, re, denom, J1ks, J2ks, J1ko, J2ko;
   double Ps, Qs, Pv, Qv, z, g1, g2, Tv1, Tv2, T1, T2, T3;
   double alf, sumint, fhot, x1, y1, f1, fint, x2, y2, f2;
   double resh, resv, absh, absv;

   int nbdata = sizeof(DataSpecP5B) / sizeof(DataSpec);
   for (int i = 0; i < nbdata; ++i)
   {
      lambda = DataSpecP5B[i].lambda;
      Es = DataSpecP5B[i].directLight; //8
      Ed = DataSpecP5B[i].diffuseLight; //9
      Rsoil1 = DataSpecP5B[i].drySoil; //10
      Rsoil2 = DataSpecP5B[i].wetSoil; //11
      rho = inRefl->GetResponse()[i].second; //rho = LRT[1][i];
      tau = inTrans->GetResponse()[i].second; //tau = LRT[2][i];

      // direct/diffuse light
      //Es = direct
      //Ed = diffuse
      PARdiro = (1-m_Skyl/100.)*Es;
      PARdifo = (m_Skyl/100.)*Ed;

      // Soil Reflectance Properties
      //rsoil1 = dry soil
      //rsoil2 = wet soil
      if(!m_UseSoilFile)
        {
        rsoil0 = m_PSoil*Rsoil1+(1-m_PSoil)*Rsoil2;
        }
      else
        {
        rsoil0 = m_SoilDataBase->GetReflectance(m_SoilIndex, lambda)*m_PSoil;
        }

      // Here rho and tau come in
      sigb = ddb*rho+ddf*tau;
      sigf = ddf*rho+ddb*tau;
      att = 1.-sigf;
      m2 = (att+sigb)*(att-sigb);
      if(m2<=0) m2 = 0;
      m = std::sqrt(m2);

      sb = sdb*rho+sdf*tau;
      sf = sdf*rho+sdb*tau;
      vb = dob*rho+dof*tau;
      vf = dof*rho+dob*tau;
      w = sob*rho+sof*tau;

      // Here the LAI comes in
      // Outputs for the case LAI = 0
      if (m_LAI<0)
          {
          //tss = 1;
          too = 1;
          tsstoo = 1;
          rdd = 0;
          tdd = 1;
          rsd = 0;
          tsd = 0;
          rdo = 0;
          tdo = 0;
          //rso = 0;
          rsos = 0;
          rsod = 0;

          rddt = rsoil0;
          rsdt = rsoil0;
          rdot = rsoil0;
          rsodt = 0;
          rsost = rsoil0;
          //rsot = rsoil0;
          }

        // Other cases (LAI > 0)
        e1 = exp(-m*m_LAI);
        e2 = e1*e1;
        rinf = (att-m)/sigb;
        rinf2 = rinf*rinf;
        re = rinf*e1;
        denom = 1.-rinf2*e2;

        J1ks=Jfunc1(ks, m, m_LAI);
        J2ks=Jfunc2(ks, m, m_LAI);
        J1ko=Jfunc1(ko, m, m_LAI);
        J2ko=Jfunc2(ko, m, m_LAI);

        Ps = (sf+sb*rinf)*J1ks;
        Qs = (sf*rinf+sb)*J2ks;
        Pv = (vf+vb*rinf)*J1ko;
        Qv = (vf*rinf+vb)*J2ko;

        rdd = rinf*(1.-e2)/denom;
        tdd = (1.-rinf2)*e1/denom;
        tsd = (Ps-re*Qs)/denom;
        rsd = (Qs-re*Ps)/denom;
        tdo = (Pv-re*Qv)/denom;
        rdo = (Qv-re*Pv)/denom;

        tss = exp(-ks*m_LAI);
        too = exp(-ko*m_LAI);
        z = Jfunc3(ks, ko, m_LAI);
        g1 = (z-J1ks*too)/(ko+m);
        g2 = (z-J1ko*tss)/(ks+m);

        Tv1 = (vf*rinf+vb)*g1;
        Tv2 = (vf+vb*rinf)*g2;
        T1 = Tv1*(sf+sb*rinf);
        T2 = Tv2*(sf*rinf+sb);
        T3 = (rdo*Qs+tdo*Ps)*rinf;

        // Multiple scattering contribution to bidirectional canopy reflectance
        rsod = (T1+T2-T3)/(1.-rinf2);

        // Treatment of the hotspot-effect
        alf=1e6;
        // Apply correction 2/(K+k) suggested by F.-M. Bron
        if (m_HSpot>0) alf=(dso/m_HSpot)*2./(ks+ko);
        if (alf>200) alf=200;
        if (alf==0)
          {
          // The pure hotspot - no shadow
          tsstoo = tss;
          sumint = (1-tss)/(ks*m_LAI);
          }
        else
          {
          // Outside the hotspot
          fhot=m_LAI*std::sqrt(ko*ks);
          // Integrate by exponential Simpson method in 20 steps
          // the steps are arranged according to equal partitioning
          // of the slope of the joint probability function
          x1=0;
          y1=0;
          f1=1;
          fint=(1.-exp(-alf))*0.05;
          sumint=0;

          for(unsigned int j=1; j<=20; ++j)
            {
            if (j<20) x2 = -std::log(1.-j*fint)/alf;
            else x2 = 1;
            y2 = -(ko+ks)*m_LAI*x2+fhot*(1.-exp(-alf*x2))/alf;
            f2 = exp(y2);
            sumint = sumint+(f2-f1)*(x2-x1)/(y2-y1);
            x1=x2;
            y1=y2;
            f1=f2;
            }
          tsstoo=f1;
          }

        // Bidirectional reflectance
        // Single scattering contribution
        rsos = w*m_LAI*sumint;
      // Total canopy contribution
      // rso=rsos+rsod;
      //Interaction with the soil
      dn=1.-rsoil0*rdd;

      rddt = rdd+tdd*rsoil0*tdd/dn;
      rsdt = rsd+(tsd+tss)*rsoil0*tdd/dn;
      rdot = rdo+tdd*rsoil0*(tdo+too)/dn;

      rsodt = rsod+((tss+tsd)*tdo+(tsd+tss*rsoil0*rdd)*too)*rsoil0/dn;
      rsost = rsos+tsstoo*rsoil0;
      rsot = rsost+rsodt;

      resh = (rddt*PARdifo+rsdt*PARdiro)/(PARdiro+PARdifo);
      resv = (rdot*PARdifo+rsot*PARdiro)/(PARdiro+PARdifo);

      absh = (1-rddt-(1-rsoil0)*(tdd+(tdd*rdd*rsoil0)/dn));
      absv = (1-rsdt-(1-rsoil0)*(tss+(tss*rsoil0*rdd+tsd)/dn));

      SpectralResponseType::PairType response;
      response.first=lambda/1000.0;
      response.second=resh;
      outHRefl->GetResponse().push_back(response);
      response.second=resv;
      outVRefl->GetResponse().push_back(response);
      response.second=absh;
      outHAbs->GetResponse().push_back(response);
      response.second=absv;
      outVAbs->GetResponse().push_back(response);
   }
   m_FCoverView = 1-too;
}

void
SailModel
::Calc_LIDF(const double a, VectorType &lidf) const
{
   int ala=a;
   VectorType freq;
   Campbell(ala, freq);
   lidf=freq;

}

void
SailModel
::Campbell(const double ala, VectorType &freq) const
{
   unsigned int n=18;
   double excent = exp(-1.6184e-5*std::pow(ala, 3)+2.1145e-3*ala*ala-1.2390e-1*ala+3.2491);
   double sum=0;
   unsigned int tx2, tx1;
   double tl1, tl2, x1, x2, v, alpha, alpha2, x12, x22, alpx1, alpx2, dum, almx1, almx2;
   VectorType temp;

   for(unsigned int i=0; i<n; ++i)
   {
      tx2 = 5*i;
      tx1 = 5*(i+1);
      tl1 = tx1*CONST_PI/180;
      tl2 = tx2*CONST_PI/180;

      x1 = excent/sqrt(1.+excent*excent*std::tan(tl1)*std::tan(tl1));
      x2 = excent/sqrt(1.+excent*excent*std::tan(tl2)*std::tan(tl2));
      if(excent==1)
      {
         v = std::abs(cos(tl1)-cos(tl2));
         temp.push_back( v );
         sum = sum + v;
      }
      else
      {
         alpha = excent/std::sqrt(std::abs(1.-excent*excent));
         alpha2 = alpha*alpha;
         x12 = x1*x1;
         x22 = x2*x2;
         if(excent>1)
         {
            alpx1 = std::sqrt(alpha2+x12);
            alpx2 = std::sqrt(alpha2+x22);
            dum   = x1*alpx1+alpha2*log(x1+alpx1);
            v = std::abs(dum-(x2*alpx2+alpha2*log(x2+alpx2)));
            temp.push_back( v );
            sum = sum + v;
         }
         else
         {
            almx1 = sqrt(alpha2-x12);
            almx2 = sqrt(alpha2-x22);
            dum   = x1*almx1+alpha2*asin(x1/alpha);
            v = std::abs(dum-(x2*almx2+alpha2*asin(x2/alpha)));
            temp.push_back( v );
            sum = sum + v;
         }
      }
   }

   for(unsigned int i=0; i<n; ++i)
   {
      freq.push_back(temp[i]/sum);
   }

}

void
SailModel
::Volscatt(const double tts, const double tto, const double psi, const double ttl, VectorType &result) const
{

   double rd = CONST_PI/180;
   double costs = std::cos(rd*tts);
   double costo = std::cos(rd*tto);
   double sints = std::sin(rd*tts);
   double sinto = std::sin(rd*tto);
   double cospsi = std::cos(rd*psi);
   double psir = rd*psi;
   double costl = std::cos(rd*ttl);
   double sintl = std::sin(rd*ttl);
   double cs = costl*costs;
   double co = costl*costo;
   double ss = sintl*sints;
   double so = sintl*sinto;

   // ..............................................................................
   //     betas -bts- and betao -bto- computation
   //     Transition angles (beta) for solar (betas) and view (betao) directions
   //     if thetav+thetal>pi/2, bottom side of the leaves is observed for leaf azimut
   //     interval betao+phi<leaf azimut<2pi-betao+phi.
   //     if thetav+thetal<pi/2, top side of the leaves is always observed, betao=pi
   //     same consideration for solar direction to compute betas
   // ..............................................................................
   double cosbts, cosbto, bts, ds, chi_s, bto, doo, chi_o;
   double btran1, btran2, bt1, bt2, bt3, t1, t2 , denom, frho, ftau;

   cosbts = 5;
   if (std::abs(ss)>1e-6) cosbts = -cs/ss;

   cosbto=5;
   if (std::abs(so)>1e-6) cosbto = -co/so;

   if (std::abs(cosbts)<1)
   {
      bts = std::acos(cosbts);
      ds = ss;
   }
   else
   {
      bts = CONST_PI;
      ds = cs;
   }

   chi_s = 2./CONST_PI*((bts-CONST_PI*0.5)*cs+std::sin(bts)*ss);

   if (std::abs(cosbto)<1)
   {
      bto = std::acos(cosbto);
      doo = so;
   }
   else if(tto<90)
   {
      bto = CONST_PI;
      doo = co;
   }
   else
   {
      bto = 0;
      doo = -co;
   }
   chi_o = 2./CONST_PI*((bto-CONST_PI*0.5)*co+std::sin(bto)*so);

   // ..............................................................................
   //   Computation of auxiliary azimut angles bt1, bt2, bt3 used
   //   for the computation of the bidirectional scattering coefficient w
   // .............................................................................

   btran1 = std::abs(bts-bto);
   btran2 = CONST_PI - std::abs(bts+bto-CONST_PI);

   if (psir<=btran1)
   {
      bt1=psir;
      bt2=btran1;
      bt3=btran2;
   }
   else
   {
      bt1=btran1;
      if (psir<=btran2)
      {
         bt2=psir;
         bt3=btran2;
      }
      else
      {
         bt2=btran2;
         bt3=psir;
      }
   }

   t1 = 2.*cs*co+ss*so*cospsi;
   t2 = 0;
   if (bt2>0) t2=sin(bt2)*(2.*ds*doo+ss*so*cos(bt1)*cos(bt3));
   denom = 2.*CONST_PI*CONST_PI;
   frho = ((CONST_PI-bt2)*t1+t2)/denom;
   ftau = (-bt2*t1+t2)/denom;

   if (frho<0) frho = 0;
   if (ftau<0) ftau = 0;

   result[0] = chi_s;
   result[1] = chi_o;
   result[2] = frho;
   result[3] = ftau;

}

double
SailModel
::Jfunc1(const double k, const double l, const double t) const
{
   //J1 function with avoidance of singularity problem
   double v;
   double del=(k-l)*t;
   if(std::abs(del)>1e-3)
   {
      v = (exp(-l*t)-exp(-k*t))/(k-l);
      return v;
   }
   else
   {
      v = 0.5*t*(exp(-k*t)+exp(-l*t))*(1.-del*del/12.);
      return v;
   }
}

double
SailModel
::Jfunc2(const double k, const double l, const double t) const
{
   double v;
   v = (1.-exp(-(k+l)*t))/(k+l);
   return v;
}

double
SailModel
::Jfunc3(const double k, const double l, const double t) const
{
   double v;
   v =  (1.-exp(-(k+l)*t))/(k+l);
   return v;
}

void
SailModel
::PrintSelf(std::ostream& os, itk::Indent indent) const
{
   Superclass::PrintSelf(os, indent);

}

void SailModel::UseExternalSoilDB(std::shared_ptr<SoilDataBase> SoilDB,
                                  size_t SoilIndex)
{
  m_UseSoilFile = true;
  m_SoilIndex = SoilIndex;
  m_SoilDataBase = SoilDB;
}
} // end namespace otb