#include <iostream>
#include <string>
#include <fstream>
#include <iomanip>

using namespace std;

// abs(i), labs(l)                     Absolute value
// acos(d), acosl(ld)                  Inverse cosine
// acosh(d)                            Inverse hyperbolic cosine
// asin(d), asinl(ld)                  Inverse sine
// asinh(d)                            Inverse hyperbolic sine
// atan(d), atanl(ld)                  Inverse tangent
// atan2(d,d), atan2l(ld,ld)           Inverse tangent
// atanh(d)                            Inverse hyperbolic tangent
// _class(d)                           Classification of floating-point values
// cbrt(x)                             Cube root
// ceil(d), ceill(ld)                  Smallest f-int not less than x
// cos(d), cosl(ld)                    Cosine
// cosh(d), coshl(ld)                  Hyperbolic cosine
// copysign(d,d)                       Return 1st arg with same sign as 2nd
// drem(x,x)                           IEEE remainder
// exp(d), expl(ld)                    Exponential
// expm1(d)                            Exp(x)-1
// erf(d), erfl(ld)                    Error function
// erfc(d), erfcl(ld)                  Complementary error function
// fabs(d), fabsl(ld)                  Floating point absolute value
// int finite(d)                       Nonzero if finite
// floor(d), floor(ld)                 Largest f-int not greater than x
// fmod(d,d), fmodl(ld,ld)             Floating point remainder
// frexp(d, int* e)                    Break into mantissa/exponent  (*)
// frexpl(ld, int* e)                  Break into mantissa/exponent  (*)
// gammaFunc(d)                        Gamma function (** needs special
//                                     implementation using lgamma)
// hypot(d,d)                          Hypotenuse: sqrt(x*x+y*y)
// int ilogb(d)                        Integer unbiased exponent
// int isnan(d)                        Nonzero if NaNS or NaNQ
// int itrunc(d)                       Truncate and convert to integer
// j0(d)                               Bessel function first kind, order 0
// j1(d)                               Bessel function first kind, order 1
// jn(int, double)                     Bessel function first kind, order i
// ldexp(d,i), ldexpl(ld,i)            Compute d * 2^i
// lgamma(d), lgammal(ld)              Log absolute gamma
// log(d), logl(ld)                    Natural logarithm
// logb(d)                             Unbiased exponent (IEEE)
// log1p(d)                            Compute log(1 + x)
// log10(d), log10l(ld)                Logarithm base 10
// modf(d, int* i), modfl(ld, int* i)  Break into integral/fractional part
// double nearest(double)              Nearest floating point integer
// nextafter(d, d)                     Next representable neighbor of 1st
//                                     in direction of 2nd
// pow(d,d), powl(ld,ld)               Computes x ^ y
// d remainder(d,d)                    IEEE remainder
// d rint(d)                           Round to f-integer (depends on mode)
// d rsqrt(d)                          Reciprocal square root
// d scalb(d,d)                        Return x * (2^y)
// sin(d), sinl(ld)                    Sine
// sinh(d), sinhl(ld)                  Hyperbolic sine
// sqr(x)                              Return x * x
// sqrt(d), sqrtl(ld)                  Square root
// tan(d), tanl(ld)                    Tangent
// tanh(d), tanhl(ld)                  Hyperbolic tangent
// trunc(d)                            Nearest f-int in the direction of 0
// unsigned uitrunc(d)                 Truncate and convert to unsigned
// int unordered(d,d)                  Nonzero if comparison is unordered
// y0(d)                               Bessel function 2nd kind, order 0
// y1(d)                               Bessel function 2nd kind, order 1
// yn(i,d)                             Bessel function 2nd kind, order d

ofstream ofs;

const int ldflag = 1;
const int cflag = 2;
const int ieeeflag = 3;
const int bsdflag = 4;
const int cflag1 = 5;
const int cflag2 = 6;
const int nofuncflag = 7;

void one(const char* applicName, const char* specialization, const char* funcName,
    const char* returnType, const char* comment, int flag=0, int noCastFlag=0)
{
    if (specialization != 0 && !strlen(specialization))
        specialization = 0;
    if (returnType != 0 && !strlen(returnType))
        returnType = 0;
    if (comment != 0 && !strlen(comment))
        comment = 0;

    ofs << "// " << applicName << "(";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << ")";
    if (comment)
        ofs << "    " << comment;
    ofs << std::endl;

    if (flag == cflag)
        ofs << "#ifdef BZ_HAVE_COMPLEX_FCNS" << std::endl;
    else if (flag == cflag1)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH1" << std::endl;
    else if (flag == cflag2)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH2" << std::endl;
    else if (flag == ieeeflag)
        ofs << "#ifdef BZ_HAVE_IEEE_MATH" << std::endl;
    else if (flag == bsdflag)
        ofs << "#ifdef BZ_HAVE_SYSTEM_V_MATH" << std::endl;
//    else if (flag == ldflag)
//        ofs << "#ifdef BZ_LONGDOUBLE128" << std::endl;

    if (!specialization)
    {
        ofs << "template<typename P_numtype1>" << std::endl;
    }
    else {
        ofs << "template<>" << std::endl;
    }
    ofs << "class _bz_" << applicName;
    if (specialization)
        ofs << "<" << specialization << ">";

    ofs << " : public OneOperandApplicativeTemplatesBase {" << std::endl;

    ofs << "public:" << std::endl;
    ofs << "    typedef ";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << " T_numtype1;" << std::endl;

    ofs << "    typedef ";
    if (returnType)
        ofs << returnType;
    else if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << " T_numtype;" << std::endl;

    if (strcmp(applicName,"blitz_isnan") == 0) // Special case nan
    {
        ofs << std::endl << "    static inline T_numtype apply(T_numtype1 x)"
            << std::endl << "    {" << std::endl;
        ofs << "#ifdef isnan" << std::endl;
        ofs << "        "
            << "// Some platforms define isnan as a macro, which causes the"
            << std::endl << "        "
            << "// BZ_IEEEMATHFN_SCOPE macro to break." << std::endl;
        ofs << "        return isnan(x);" << std::endl;
        ofs << "#else" << std::endl;
        ofs << "        return BZ_IEEEMATHFN_SCOPE(isnan)(x);" << std::endl;
        ofs << "#endif" << std::endl << "    }" << std::endl;
    }
    else
    {
        ofs << std::endl << "    static inline T_numtype apply(T_numtype1 x)"
            << std::endl << "    { return ";

        if (noCastFlag == nofuncflag)
        {
            ofs << funcName;
        }
        else {
        if ((flag == cflag) || (flag == cflag1) || (flag == cflag2))
            ofs << "BZ_CMATHFN_SCOPE(";
        else if ((flag == ieeeflag) || (flag == bsdflag))
            ofs << "BZ_IEEEMATHFN_SCOPE(";
        else
            ofs << "BZ_MATHFN_SCOPE(";

        ofs << funcName << ")(";
        if (specialization != 0)
            ofs << "(" << specialization << ")";
        else if ((returnType)&&(!noCastFlag))
            ofs << "(" << returnType << ")";

        ofs << "x)";
        }
        ofs << "; }" << std::endl;
    }

    ofs << std::endl << "    template<typename T1>" << std::endl
        << "    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,"
        << std::endl
        << "        const T1& a)" << std::endl
        << "    {" << std::endl
        << "        str += \"" << funcName;
      ofs  << "(\";" << std::endl
        << "        a.prettyPrint(str,format);" << std::endl
        << "        str += \")\";" << std::endl
        << "    }" << std::endl
        << "};" << std::endl;

   if ((flag != ldflag) && (flag != 0))
        ofs << "#endif" << std::endl;

    ofs << std::endl;
}

void two(const char* applicName, const char* specialization, const char* funcName,
    const char* returnType, const char* comment, int flag=0, int noCastFlag=0)
{
    if (specialization != 0 && !strlen(specialization))
        specialization = 0;
    if (returnType != 0 && !strlen(returnType))
        returnType = 0;
    if (comment != 0 && !strlen(comment))
        comment = 0;

    ofs << "// " << applicName << "(";
    if (specialization)
        ofs << specialization << ", " << specialization;
    else
        ofs << "P_numtype1, P_numtype2";
    ofs << ")";
    if (comment)
        ofs << "    " << comment;
    ofs << std::endl;

    if (flag == cflag)
        ofs << "#ifdef BZ_HAVE_COMPLEX_FCNS" << std::endl;
    else if (flag == cflag1)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH1" << std::endl;
    else if (flag == cflag2)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH2" << std::endl;
    else if (flag == ieeeflag)
        ofs << "#ifdef BZ_HAVE_IEEE_MATH" << std::endl;
    else if (flag == bsdflag)
        ofs << "#ifdef BZ_HAVE_SYSTEM_V_MATH" << std::endl;
//    else if (flag == ldflag)
//        ofs << "#ifdef BZ_LONGDOUBLE128" << std::endl;

    if (!specialization)
    {
        ofs << "template<typename P_numtype1, typename P_numtype2>" << std::endl;
    }
    else {
        ofs << "template<>" << std::endl;
    }
    ofs << "class _bz_" << applicName;
    if (specialization)
        ofs << "<" << specialization  << ", " << specialization << " >";
    ofs << " : public TwoOperandApplicativeTemplatesBase {" << std::endl;

    ofs << "public:" << std::endl;
    ofs << "    typedef ";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << " T_numtype1;" << std::endl;

    ofs << "    typedef ";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype2";
    ofs << " T_numtype2;" << std::endl;

    ofs << "    typedef ";
    if (returnType)
        ofs << returnType;
    else if (specialization)
        ofs << specialization;
    else
        ofs << "BZ_PROMOTE(T_numtype1, T_numtype2)";
    ofs << " T_numtype;" << std::endl;

    ofs << std::endl << "    static inline T_numtype apply(T_numtype1 x, T_numtype2 y)"
        << std::endl << "    { return ";

    if ((flag == cflag) || (flag == cflag1) || (flag == cflag2))
        ofs << "BZ_CMATHFN_SCOPE(";
    else if ((flag == ieeeflag) || (flag == bsdflag))
        ofs << "BZ_IEEEMATHFN_SCOPE(";
    else
        ofs << "BZ_MATHFN_SCOPE(";

    ofs << funcName << ")(";

    if (specialization != 0)
        ofs << "(" << specialization << ")";
    else if ((returnType) && (!noCastFlag))
        ofs << "(" << returnType << ")";

    ofs << "x,";
    if (specialization != 0)
        ofs << "(" << specialization << ")";
    else if ((returnType) && (!noCastFlag))
        ofs << "(" << returnType << ")";
    ofs << "y); }" << std::endl;

    ofs << std::endl << "    template<typename T1, typename T2>" << std::endl
        << "    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,"
        << std::endl
        << "        const T1& a, const T2& b)" << std::endl
        << "    {" << std::endl
        << "        str += \"" << funcName;
      ofs  << "(\";" << std::endl
        << "        a.prettyPrint(str,format);" << std::endl
        << "        str += \",\";" << std::endl
        << "        b.prettyPrint(str,format);" << std::endl
        << "        str += \")\";" << std::endl
        << "    }" << std::endl;

    ofs << "};" << std::endl;

    if ((flag != ldflag) && (flag != 0))
        ofs << "#endif" << std::endl;

    ofs << std::endl;
}

int main()
{
    std::cout << "Generating <mathfunc.h>" << std::endl;

    ofs.open("../mathfunc.h");

    ofs <<
"// Generated: " << __FILE__ << " " << __DATE__ << " " << __TIME__
                 << std::endl << std::endl <<
"#ifndef BZ_MATHFUNC_H\n"
"#define BZ_MATHFUNC_H\n"
"\n"
"#ifndef BZ_APPLICS_H\n"
" #error <blitz/mathfunc.h> should be included via <blitz/applics.h>\n"
"#endif\n\n"
"\n"
"#ifndef BZ_PRETTYPRINT_H\n"
" #include <blitz/prettyprint.h>\n"
"#endif\n\n"
"BZ_NAMESPACE(blitz)\n\n";

    one("abs", 0, "abs", 0, "Absolute value");
    one("abs","long","labs","long", 0);
    one("abs","float"       ,"fabs",    "float",       0);

one("abs"    ,"double"      ,"fabs"    ,"double"       ,"");
one("abs"    ,"long double" ,"fabs"   ,"long double"  ,"", ldflag);
one("abs"    ,"complex<float> ", "abs","float", "", cflag);
one("abs"    ,"complex<double> ", "abs", "double", "", cflag);
one("abs"    ,"complex<long double> ", "abs", "long double", "", cflag);
one("acos"   ,""            ,"acos"    ,"double"       ,"Inverse cosine");
one("acos"   ,"float"       ,"acos"  ,"float"         ,"");
one("acos"   ,"long double" ,"acos"   ,"long double"  ,"", ldflag);
one("acos"   ,"complex<float> ", "acos", "complex<float>", "", cflag2);
one("acos"   ,"complex<double> ", "acos", "complex<double>", "", cflag2);
one("acos", "complex<long double> ", "acos", "complex<long double>", "", cflag2);
one("acosh"  ,""            ,"acosh"   ,"double"       ,"Inverse hyperbolic cosine", ieeeflag);
one("asin"   ,""            ,"asin"    ,"double"       ,"Inverse sine");
one("asin",   "float",       "asin",    "float", "");
one("asin"   ,"long double" ,"asin"   ,"long double"  ,"", ldflag);
one("asin"   ,"complex<float> ", "asin", "complex<float>", "", cflag2);
one("asin"   ,"complex<double> ", "asin", "complex<double>", "", cflag2);
one("asin", "complex<long double> ", "asin", "complex<long double>", "", cflag2);
one("asinh"  ,""            ,"asinh"   ,"double"       ,"Inverse hyperbolic sine", ieeeflag);
one("arg",   ""            ,"0"    ,0             ,"", cflag, nofuncflag);
one("arg",   "complex<float> ", "arg", "float", "", cflag, 0);
one("arg",   "complex<double> ", "arg", "double", "", cflag, 0);
one("arg",   "complex<long double> ", "arg", "long double", "", cflag, 0);
one("atan"   ,""            ,"atan"    ,"double"       ,"Inverse tangent");
one("atan",   "float",       "atan",    "float",        "");
one("atan"   ,"long double" ,"atan"   ,"long double"  ,"", ldflag);
one("atan"   ,"complex<float> ", "atan", "complex<float>", "", cflag2);
one("atan"   ,"complex<double> ", "atan", "complex<double>", "", cflag2);
one("atan", "complex<long double> ", "atan", "complex<long double>", "", cflag2);
one("atanh"  ,""            ,"atanh"   ,"double"       ,"Inverse hyperbolic tangent", ieeeflag);
two("atan2"  ,""            ,"atan2"   ,"double"       ,"Inverse tangent");
two("atan2"  ,"float"       ,"atan2"   ,"float"        ,"");
two("atan2"  ,"long double" ,"atan2"   ,"long double"  ,"");
one("_class" ,""            ,"_class"  ,"int"          ,"Classification of float-point value (FP_xxx)", bsdflag,1);
one("cbrt"   ,""            ,"cbrt"    ,"double"       ,"Cube root", ieeeflag);
one("ceil"   ,""            ,"ceil"    ,"double"       ,"Ceiling");
one("ceil",   "float",       "ceil",    "float",       "");
one("ceil"   ,"long double" ,"ceil"   ,"long double"  ,"", ldflag);
one("conj",   ""            ,"conj"    ,0             ,"", cflag);
one("cos"    ,""            ,"cos"     ,"double"       ,"Cosine");
one("cos",    "float",       "cos",     "float",       "");
one("cos"    ,"long double" ,"cos"    ,"long double"  ,"", ldflag);
one("cos"   ,"complex<float> ", "cos", "complex<float>", "", cflag1);
one("cos"   ,"complex<double> ", "cos", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("cos", "complex<long double> ", "cos", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
two("copysign", ""          ,"copysign","double"       ,"", bsdflag);
one("cosh"   ,""            ,"cosh"    ,"double"       ,"Hyperbolic cosine");
one("cosh",   "float",       "cosh",    "float", "");
one("cosh"   ,"long double" ,"cosh"   ,"long double"  ,"", ldflag);
one("cosh"   ,"complex<float> ", "cosh", "complex<float>", "", cflag1);
one("cosh"   ,"complex<double> ", "cosh", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("cosh", "complex<long double> ", "cosh", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
two("drem"   ,""            ,"drem"    ,"double"       ,"Remainder", bsdflag);
one("exp"    ,""            ,"exp"     ,"double"       ,"Exponential");
one("exp",    "float",       "exp",     "float",       "");
one("exp"    ,"long double" ,"exp"    ,"long double"  ,"", ldflag      );
one("exp"   ,"complex<float> ", "exp", "complex<float>", "", cflag1);
one("exp"   ,"complex<double> ", "exp", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("exp", "complex<long double> ", "exp", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("expm1"  ,""            ,"expm1"   ,"double"       ,"Exp(x)-1", ieeeflag);
one("erf"    ,""            ,"erf"     ,"double"       ,"Error function", ieeeflag);
one("erfc"   ,""            ,"erfc"    ,"double"       ,"Complementary error function", ieeeflag);

// blitz-bugs/archive/0189.html
// one("finite" ,""            ,"finite"  ,"int"          ,"Nonzero if finite", ieeeflag,1);

one("floor"  ,""            ,"floor"   ,"double"       ,"Floor function");
one("floor",  "float",       "floor",   "float",        "");
one("floor"  ,"long double" ,"floor"   ,"long double"  ,"");
two("fmod"   ,""            ,"fmod"    ,"double"       ,"Modulo remainder");
two("hypot"  ,""            ,"hypot"   ,"double"       ,"sqrt(x*x+y*y)",bsdflag);
one("ilogb"  ,""            ,"ilogb"   ,"int"          ,"Integer unbiased exponent", ieeeflag,1);
one("blitz_isnan"  ,""            ,"blitz_isnan"   ,"int"          ,"Nonzero if NaNS or NaNQ", ieeeflag,nofuncflag);
one("itrunc" ,""            ,"itrunc"  ,"int"          ,"Truncate and convert to integer", bsdflag,1);
one("j0"     ,""            ,"j0"      ,"double"       ,"Bessel function first kind, order 0", ieeeflag);
one("j1"     ,""            ,"j1"      ,"double"       ,"Bessel function first kind, order 1", ieeeflag);
one("lgamma" ,""            ,"lgamma"  ,"double"       ,"Log absolute gamma", ieeeflag);
one("log"    ,""            ,"log"     ,"double"       ,"Natural logarithm");
one("log",    "float",       "log",     "float",        "");
one("log"    ,"long double" ,"log"     ,"long double"  ,"", ldflag);
one("log"   ,"complex<float> ", "log", "complex<float>", "", cflag1);
one("log"   ,"complex<double> ", "log", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("log", "complex<long double> ", "log", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("logb"   ,""            ,"logb"    ,"double"       ,"Unbiased exponent (IEEE)", ieeeflag);
one("log1p"  ,""            ,"log1p"   ,"double"       ,"Compute log(1 + x)", ieeeflag);
one("log10"  ,""            ,"log10"   ,"double"       ,"Logarithm base 10");
one("log10",  "float",       "log10",   "float",        "");
one("log10"  ,"long double" ,"log10"  ,"long double"  ,"", ldflag);
one("log10"   ,"complex<float> ", "log10", "complex<float>", "", cflag2);
one("log10"   ,"complex<double> ", "log10", "complex<double>", "", cflag2);
one("log10", "complex<long double> ", "log10", "complex<long double>", "", cflag2);
one("nearest", ""           ,"nearest" ,"double"       ,"Nearest floating point integer", bsdflag);
two("nextafter", "",         "nextafter", "double",     "Next representable number after x towards y", bsdflag);

ofs <<
"template<typename P_numtype>\n"
"class _bz_negate : public OneOperandApplicativeTemplatesBase {\n"
"public:\n"
"    typedef BZ_SIGNEDTYPE(P_numtype) T_numtype;\n\n"
"    static inline T_numtype apply(T_numtype x)\n"
"    { return -x; }\n\n"
"        template<typename T1>\n"
"        "
"static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format, const T1& a)\n"
"        {\n"
"                str += \"-(\";\n"
"                       a.prettyPrint(str,format);\n"
"                       str += \")\";\n"
"        }\n"
"};\n\n"
;

one("norm",   ""            ,"norm"    ,0             ,"", cflag);

two("polar"  ,""            ,"polar"   ,"complex<T_numtype1>", "", cflag, 1);
two("pow"    ,""            ,"pow"     ,"double"       ,"Power");
 ofs << "#ifndef __PGI\n";
two("pow"    ,"float"       ,"pow"     ,"float"        ,"");
 ofs << "#endif\n";
two("pow"    ,"long double" ,"pow"     ,"long double"  ,"");
two("pow"    ,"complex<float>","pow"   ,"complex<float>" ,"",cflag1);
two("pow"    ,"complex<double>","pow"  ,"complex<double>","",cflag1);
 ofs << "#ifndef __PGI\n";
two("pow"    ,"complex<long double>","pow","complex<long double>","",cflag1);
 ofs << "#endif\n";
two("remainder", "",         "remainder", "double",     "Remainder", bsdflag);

one("rint"   ,""            ,"rint"    ,"double"       ,"Round to floating point integer", ieeeflag);
one("rsqrt"  ,""            ,"rsqrt"   ,"double"       ,"Reciprocal square root", bsdflag);
two("scalb"  ,""            ,"scalb"   ,"double"       ,"x * (2**y)", bsdflag);
one("sin"    ,""            ,"sin"     ,"double"       ,"Sine");
one("sin",    "float",       "sin",     "float",       "");
one("sin"    ,"long double" ,"sin"    ,"long double"  ,"", ldflag);
one("sin"   ,"complex<float> ", "sin", "complex<float>", "", cflag1);
one("sin"   ,"complex<double> ", "sin", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("sin", "complex<long double> ", "sin", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("sinh"   ,""            ,"sinh"    ,"double"       ,"Hyperbolic sine");
one("sinh",   "float",       "sinh",    "float",        "");
one("sinh"   ,"long double" ,"sinh"   ,"long double"  ,"", ldflag);
one("sinh"   ,"complex<float> ", "sinh", "complex<float>", "", cflag1);
one("sinh"   ,"complex<double> ", "sinh", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("sinh", "complex<long double> ", "sinh", "complex<long double>", "", cflag1);
 ofs << "#endif\n";

ofs <<
"template<typename P_numtype>\n"
"class _bz_sqr : public OneOperandApplicativeTemplatesBase {\n"
"public:\n"
"    typedef P_numtype T_numtype;\n\n"
"    static inline T_numtype apply(T_numtype x)\n"
"    { return x*x; }\n"
"    template<typename T1>\n"
"    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,\n"
"        const T1& a)\n"
"    {\n"
"        str += \"sqr(\";\n"
"        a.prettyPrint(str,format);\n"
"        str += \")\";\n"
"    }\n"
"};\n\n"
"#ifdef BZ_HAVE_COMPLEX\n"
"// Specialization of _bz_sqr for complex<T>\n"
"template<typename T>\n"
"class _bz_sqr<complex<T> > : public OneOperandApplicativeTemplatesBase {\n"
"public:\n"
"    typedef complex<T> T_numtype;\n\n"
"    static inline T_numtype apply(T_numtype x)\n"
"    {\n"
"        T r = x.real();  T i = x.imag();\n"
"        return T_numtype(r*r-i*i, 2*r*i);\n"
"    }\n"
"    template<typename T1>\n"
"    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,\n"
"        const T1& a)\n"
"    {\n"
"        str += \"sqr(\";\n"
"        a.prettyPrint(str,format);\n"
"        str += \")\";\n"
"    }\n"
"};\n"
"#endif\n\n"
;

one("sqrt"   ,""            ,"sqrt"    ,"double"       ,"Square root");
one("sqrt",   "float",       "sqrt",    "float",        "");
one("sqrt"   ,"long double" ,"sqrt"   ,"long double"  ,"", ldflag);
one("sqrt"   ,"complex<float> ", "sqrt", "complex<float>", "", cflag1);
one("sqrt"   ,"complex<double> ", "sqrt", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("sqrt", "complex<long double> ", "sqrt", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("tan"    ,""            ,"tan"     ,"double"       ,"Tangent");
one("tan",    "float",       "tan",    "float",         "");
one("tan"    ,"long double" ,"tan"    ,"long double"  ,"");
one("tan"   ,"complex<float> ", "tan", "complex<float>", "", cflag1);
one("tan"   ,"complex<double> ", "tan", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("tan", "complex<long double> ", "tan", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("tanh"   ,""            ,"tanh"    ,"double"       ,"Hyperbolic tangent");
one("tanh",   "float",       "tanh",    "float",        "");
one("tanh"   ,"long double" ,"tanh"   ,"long double"  ,"", ldflag);
one("tanh"   ,"complex<float> ", "tanh", "complex<float>", "", cflag1);
one("tanh"   ,"complex<double> ", "tanh", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("tanh", "complex<long double> ", "tanh", "complex<long double>", "", cflag1);
 ofs << "#endif\n";

// blitz-bugs/archive/0189.html
// one("trunc"  ,""            ,"trunc"   ,"double"       ,"Nearest floating integer in the direction of zero", ieeeflag);

one("uitrunc", ""           ,"uitrunc" ,"unsigned"     ,"Truncate and convert to unsigned", bsdflag);
two("unordered", "",         "unordered", "int",       "True if a comparison of x and y would be unordered", bsdflag,1);
one("y0"     ,""            ,"y0"      ,"double"       ,"Bessel function of the second kind, order zero", ieeeflag);
one("y1"     ,""            ,"y1"      ,"double"       ,"Bessel function of the second kind, order one", ieeeflag);

    ofs << std::endl << std::endl <<
"BZ_NAMESPACE_END\n\n"
"#endif // BZ_MATHFUNC_H\n";

    return 0;
}


/*
   confidence_intervals.cc
   Copyright (c) 2011 mldb.ai inc.  All rights reserved.
   This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.
*/

#include "mldb/utils/confidence_intervals.h"
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <math.h>
#include "mldb/arch/exception.h"
#include "mldb/utils/string_functions.h"
#include <random>

using namespace std;
using namespace boost;
using boost::math::normal; // typedef provides default type is double.

namespace MLDB
{

ConfidenceIntervals::
ConfidenceIntervals(float alpha, std::string m) : alpha_(alpha) {
    init(m);
}

void ConfidenceIntervals::init(std::string m) {
    if(m=="wilson") {
        method = WILSON;

        // Construct a standard normal distribution s
        normal s; // (default mean = zero, and standard deviation = unity)
        const double b = quantile(s, 1-alpha_);
        const double d = pow(b, 2);
        const double a = d / 2;
        const double c = d / 4;

        wilsonFnct = [=] (double sumNum, double sumDenom, double sign)
            {
                return (a/sumDenom+sumNum/sumDenom+b*
                        sign*sqrt(c/(sumDenom*sumDenom)+(sumNum*
                        (1-sumNum/sumDenom))/(sumDenom*sumDenom)))/(1+d/sumDenom);
            };
    }
    else if (m=="clopper_pearson") {
        method = CLOPPER_PEARSON;
    }
    else {
        throw MLDB::Exception(MLDB::format("Unknown confidence interval method '%s'", m.c_str()));
    }
}

unsigned
ConfidenceIntervals::getMethod()
{
    return method;
}

double
ConfidenceIntervals::
wilsonBinomialUpperLowerBound(int trials,
        int successes, WilsonBoundDirection dir) const
{
    double sumNum = (double)successes;
    double sumDenom = (double)trials;
    double sign = (double)dir;
    return wilsonFnct(sumNum, sumDenom, sign);
}

double
ConfidenceIntervals::
binomialUpperBound(int trials, int successes) const
{
    switch(method){
    case WILSON:          return wilsonBinomialUpperLowerBound(trials, successes, UPPER);
    case CLOPPER_PEARSON: return math::binomial_distribution<>::find_upper_bound_on_p(trials, successes, alpha_);
    }
    throw MLDB::Exception(MLDB::format("Unknown method '%s'", method));
}

double
ConfidenceIntervals::
binomialLowerBound(int trials, int successes) const
{
    switch(method){
    case WILSON:          return wilsonBinomialUpperLowerBound(trials, successes, LOWER);
    case CLOPPER_PEARSON: return math::binomial_distribution<>::find_lower_bound_on_p(trials, successes, alpha_);
    }
    throw MLDB::Exception(MLDB::format("Unknown method '%s'", method));
}

pair<double,double>
ConfidenceIntervals::
binomialTwoSidedBound(int trials, int successes) const
{
    assertClopperPearson();
    return make_pair(
            math::binomial_distribution<>::find_lower_bound_on_p(trials, successes, alpha_/2.0),
            math::binomial_distribution<>::find_upper_bound_on_p(trials, successes, alpha_/2.0)
        );
}

vector<double>
ConfidenceIntervals::
createBootstrapSamples(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    int sampleSize = sample.size();

    if(sampleSize ==0)
    {
        throw MLDB::Exception("Can't compute bootstrap mean from empty sample");
    }

    std::mt19937 rng;

    vector<double> resampleMeans;
    for(int i=0; i<replications;i++)
    {
        double accumulator = 0;
        for(int j=0;j<resampleSize;j++)
        {
            accumulator += sample[rng() % sampleSize];
        }
        resampleMeans.push_back(accumulator/resampleSize);
    }
    sort(resampleMeans.begin(), resampleMeans.end());
    return resampleMeans;
}

double
ConfidenceIntervals::
bootstrapMeanUpperBound(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    vector<double> resampleMeans = createBootstrapSamples(sample, replications, resampleSize);
    return resampleMeans[replications*(1-alpha_)];
}

double
ConfidenceIntervals::
bootstrapMeanLowerBound(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    vector<double> resampleMeans = createBootstrapSamples(sample, replications, resampleSize);
    return resampleMeans[replications*alpha_];
}

pair<double,double>
ConfidenceIntervals::
bootstrapMeanTwoSidedBound(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    vector<double> resampleMeans = createBootstrapSamples(sample, replications, resampleSize);
    return make_pair(
            resampleMeans[replications*(alpha_/2.0)],resampleMeans[replications*(1-alpha_/2.0)]
        );
}

void ConfidenceIntervals::assertClopperPearson() const
{
    if (method != CLOPPER_PEARSON)
        throw MLDB::Exception("Can only use this method with Clopper-Peason method!");
}

std::string ConfidenceIntervals::
print(Method m) const
{
    switch(m) {
    case WILSON:          return "wilson";
    case CLOPPER_PEARSON: return "clopper_pearson";
    default:
        throw MLDB::Exception("Unknown method");
    }
}

#if 0
void ConfidenceIntervals::
serialize(MLDB::DB::Store_Writer & store) const
{
    int version = 1;
    store << version << print(method) << alpha_;
}

void ConfidenceIntervals::
reconstitute(MLDB::DB::Store_Reader & store)
{
    int version;
    int REQUIRED_V = 1;
    store >> version;
    if(version!=REQUIRED_V) {
        throw MLDB::Exception(MLDB::format(
                    "invalid ConfidenceInterval version! exptected %d, got %d",
                    REQUIRED_V, version));
    }

    string method;
    store >> method >> alpha_;
    init(method);
}
#endif

}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int x, k, d; cin >> x >> k >> d;
    if (x < 0) x *= -1;
    cpp_int a = min(x / d, k);
    k -= a, x -= a * d;
    if (k % 2 == 0) cout << x << endl;
    else cout << abs(x - d) << endl;
}


#ifndef STAN_MATH_REV_FUN_FFT_HPP
#define STAN_MATH_REV_FUN_FFT_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <stan/math/prim/fun/typedefs.hpp>
#include <stan/math/rev/meta.hpp>
#include <stan/math/prim/fun/fft.hpp>
#include <stan/math/prim/fun/to_complex.hpp>
#include <Eigen/Dense>
#include <complex>
#include <type_traits>
#include <vector>

namespace stan {
namespace math {

/**
 * Return the discrete Fourier transform of the specified complex
 * vector.
 *
 * Given an input complex vector `x[0:N-1]` of size `N`, the discrete
 * Fourier transform computes entries of the resulting complex
 * vector `y[0:N-1]` by
 *
 * ```
 * y[n] = SUM_{i < N} x[i] * exp(-n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 * The adjoint computation is given by
 * ```
 * adjoint(x) += length(y) * inv_fft(adjoint(y))
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 *
 * @tparam V type of complex vector argument
 * @param[in] x vector to transform
 * @return discrete Fourier transform of `x`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr,
          require_var_t<base_type_t<value_type_t<V>>>* = nullptr>
inline plain_type_t<V> fft(const V& x) {
  if (unlikely(x.size() <= 1)) {
    return plain_type_t<V>(x);
  }

  arena_t<V> arena_v = x;
  arena_t<V> res = fft(to_complex(arena_v.real().val(), arena_v.imag().val()));

  reverse_pass_callback([arena_v, res]() mutable {
    auto adj_inv_fft = inv_fft(to_complex(res.real().adj(), res.imag().adj()));
    adj_inv_fft *= res.size();
    arena_v.real().adj() += adj_inv_fft.real();
    arena_v.imag().adj() += adj_inv_fft.imag();
  });

  return plain_type_t<V>(res);
}

/**
 * Return the inverse discrete Fourier transform of the specified
 * complex vector.
 *
 * Given an input complex vector `y[0:N-1]` of size `N`, the inverse
 * discrete Fourier transform computes entries of the resulting
 * complex vector `x[0:N-1]` by
 *
 * ```
 * x[n] = SUM_{i < N} y[i] * exp(n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 *  * The adjoint computation is given by
 * ```
 * adjoint(y) += (1 / length(x)) * fft(adjoint(x))
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 * The only difference between the discrete DFT and its inverse is
 * the sign of the exponent.
 *
 * @tparam V type of complex vector argument
 * @param[in] y vector to inverse transform
 * @return inverse discrete Fourier transform of `y`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr,
          require_var_t<base_type_t<value_type_t<V>>>* = nullptr>
inline plain_type_t<V> inv_fft(const V& y) {
  if (unlikely(y.size() <= 1)) {
    return plain_type_t<V>(y);
  }

  arena_t<V> arena_v = y;
  arena_t<V> res
      = inv_fft(to_complex(arena_v.real().val(), arena_v.imag().val()));

  reverse_pass_callback([arena_v, res]() mutable {
    auto adj_fft = fft(to_complex(res.real().adj(), res.imag().adj()));
    adj_fft /= res.size();

    arena_v.real().adj() += adj_fft.real();
    arena_v.imag().adj() += adj_fft.imag();
  });
  return plain_type_t<V>(res);
}

}  // namespace math
}  // namespace stan
#endif


#include <wmtk/TriMesh.h>
#include <wmtk/utils/VectorUtils.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <wmtk/ExecutionScheduler.hpp>
#include "EdgeOperations2d.h"

using namespace Edge2d;
using namespace wmtk;
// get the quadrix in form of an array of 10 floating point numbers
Eigen::MatrixXd compute_Q_f(const EdgeOperations2d& m, const wmtk::TriMesh::Tuple& f_tuple)
{
    auto conn_indices = m.oriented_tri_vertices(f_tuple);
    Eigen::Vector3d A = m.vertex_attrs[conn_indices[0].vid(m)].pos;
    Eigen::Vector3d B = m.vertex_attrs[conn_indices[1].vid(m)].pos;
    Eigen::Vector3d C = m.vertex_attrs[conn_indices[2].vid(m)].pos;

    Eigen::Vector3d n = ((A - B).cross(C - B)).normalized();
    Eigen::Vector4d p;
    p(0) = n(0);
    p(1) = n(1);
    p(2) = n(2);
    p(3) = -n.dot(B);
    return p * p.transpose();
}

Eigen::MatrixXd compute_Q_v(const EdgeOperations2d& m, const TriMesh::Tuple& v_tuple)
{
    auto conn_tris = m.get_one_ring_tris_for_vertex(v_tuple);
    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, 4);
    auto Q_t = [](auto& m, auto& f_tuple) {
        auto conn_indices = m.oriented_tri_vertices(f_tuple);
        Eigen::Vector3d A = m.vertex_attrs[conn_indices[0].vid(m)].pos;
        Eigen::Vector3d B = m.vertex_attrs[conn_indices[1].vid(m)].pos;
        Eigen::Vector3d C = m.vertex_attrs[conn_indices[2].vid(m)].pos;

        Eigen::Vector3d n = ((A - B).cross(C - B)).normalized();
        Eigen::Vector4d p;
        p(0) = n(0);
        p(1) = n(1);
        p(2) = n(2);
        p(3) = -n.dot(B);

        return (p * p.transpose());
    };
    for (auto tri : conn_tris) {
        auto Q_tmp = compute_Q_f(m, tri);
        Q += Q_tmp;
    }
    return Q;
}

double Edge2d::EdgeOperations2d::compute_cost_for_e(const TriMesh::Tuple& v_tuple)
{
    Eigen::MatrixXd Q = compute_Q_v(*this, v_tuple);
    Q += compute_Q_v(*this, v_tuple.switch_vertex(*this));

    Eigen::Vector4d t(0.0, 0.0, 0.0, 1.0);
    Eigen::MatrixXd vQ = Q;
    vQ.row(3) = t;

    Eigen::Vector4d v;
    if (vQ.determinant() < 1e-6) {
        Eigen::Vector3d tmp =
            (vertex_attrs[v_tuple.vid(*this)].pos + vertex_attrs[switch_vertex(v_tuple).vid(*this)].pos) / 2;
        v << tmp, 1.0;
    }

    else
        v = vQ.inverse() * t;

    // wmtk::logger().info("Q is \n {} \n v is \n {}", Q, v);
    Eigen::Vector3d newv = v.head(3);

    return (v.transpose() * Q * v);
}

bool Edge2d::EdgeOperations2d::collapse_qec(int target)
{
    // find the valid pairs (for each vertex)
    size_t vertex_number = vert_capacity();
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_collapse", loc);

    auto executor = wmtk::ExecutePass<EdgeOperations2d, wmtk::ExecutionPolicy::kSeq>();
    executor.renew_neighbor_tuples = [](auto& m, auto op, auto& tris) {
        auto edges = m.new_edges_after(tris);
        auto optup = std::vector<std::pair<std::string, TriMesh::Tuple>>();
        // for (auto& e : edges) optup.emplace_back(op, e);
        return optup;
    };

    executor.priority = [this](auto& m, auto _, auto& e) {
        //     return -(m.vertex_attrs.pos[e.vid(*this)] -
        //     m.vertex_attrs.pos[e.switch_vertex(m).vid(*this)])
        //                 .norm();
        // };

        // wmtk::logger().info(
        //     "{} \n{}\n {}",
        //     vertex_attrs->m_attributes[e.vid(*this)].pos,
        //     vertex_attrs->m_attributes[e.switch_vertex(m).vid(*this)].pos,
        //     compute_cost_for_e(e));
        return -compute_cost_for_e(e);
    };
    executor.is_weight_up_to_date = [&collect_all_ops, this](auto& m, auto& ele) {
        auto& [val, op, e] = ele;

        if (val > 0) return false; // priority is negated.
        double pri = -compute_cost_for_e(e);
        if ((val - pri) < 1e-5) {
            wmtk::logger().info("the priority is different");
            return false;
        }
        for (auto edge : get_edges()) {
            if (pri < -compute_cost_for_e(edge)) {
                wmtk::logger().info("!!!! should not happen !!!!");
                return false;
            }
        }

        return true;
    };
    executor.stopping_criterion_checking_frequency = 1000;
    executor.stopping_criterion = [&target](auto& m) {
        if (m.get_vertices().size() < target) return true;
        return false;
    };

    executor(*this, collect_all_ops);
    return true;
}


#ifndef MOCHIMOCHI_NHERD_HPP_
#define MOCHIMOCHI_NHERD_HPP_

#include <Eigen/Dense>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include <functional>
#include "../../functions/enumerate.hpp"

class NHERD {
private :
  const std::size_t kDim;
  const double kC;
  const int kDiagonal;

private :
  Eigen::VectorXd _covariances;
  Eigen::VectorXd _means;

private :
  std::function<double(double, double, double)> _compute_covariance;

public :
  NHERD(const std::size_t dim, const double C, const int diagonal = 0)
    : kDim(dim),
      kC(C),
      kDiagonal(diagonal),
      _covariances(Eigen::VectorXd::Ones(kDim)),
      _means(Eigen::VectorXd::Zero(kDim)) {

    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(C)>::max() > 0, "Hyper Parameter Error. (C > 0)");

    // int diagonal : switching the diagonal covariance
    // 0 : Full covariance
    // 1 : Exact covariance
    // 2 : Project covariance
    // 3 : Drop covariance
    switch(kDiagonal) {
    case 0 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        const auto v = covariance * value;
        return covariance - (v * v * (kC * kC * confidence + 2 * kC) / std::pow((1.0 + kC * confidence), 2));
      };
      break;
    case 1 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        return covariance / std::pow(1.0 + kC * value * value * covariance, 2);
      };
      break;
    case 2 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        return 1.0 / ((1.0 / covariance) + (2 * kC + kC * kC * confidence) * value * value);
      };
      break;
    case 3 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        const auto v = (std::pow(covariance * value, 2) * (kC * kC * confidence + 2 * kC) / std::pow(1.0 + kC * confidence, 2));
        return covariance - v;
      };
      break;
    default:
      std::runtime_error("Error in switching the diagonal covariance.");
    }

  }

  virtual ~NHERD() { }

private :

  double suffer_loss(const double margin, const int label) const {
    return margin * label;
  }

  double compute_margin(const Eigen::VectorXd& x) const {
    return _means.dot(x);
  }

  double compute_confidence(const Eigen::VectorXd& feature) const {
    auto confidence = 0.0;
    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                         [&](const int index, const double value) {
                           confidence += _covariances[index] * value * value;
                         });
    return confidence;
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    const auto margin = compute_margin(feature);

    if (suffer_loss(margin, label) >= 1.0) { return false; }

    const auto confidence = compute_confidence(feature);
    const auto alpha = std::max(0.0, 1.0 - label * margin) / (confidence + 1 / kC) ;

    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                       [&](const std::size_t index, const double value) {
                         _means[index] += alpha * label * _covariances[index] * value;
                         _covariances[index] = _compute_covariance(_covariances[index], confidence, value);
                       });
    return true;
  }

  int predict(const Eigen::VectorXd& x) const {
    return compute_margin(x) > 0.0 ? 1 : -1;
  }

  Eigen::VectorXd get_means(void) const {
    return _means;
  }

  void save(const std::string& filename) {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> covariances_vector(_covariances.data(), _covariances.data() + _covariances.size());
    std::vector<double> means_vector(_means.data(), _means.data() + _means.size());
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("C", const_cast<double&>(kC));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> covariances_vector;
    std::vector<double> means_vector;
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("C", const_cast<double&>(kC));
    _covariances = Eigen::Map<Eigen::VectorXd>(&covariances_vector[0], covariances_vector.size());
    _means = Eigen::Map<Eigen::VectorXd>(&means_vector[0], means_vector.size());
  }
};

#endif //MOCHIMOCHI_NHERD_HPP_


// -*- mode: C++; c-indent-level: 4; c-basic-offset: 4; indent-tabs-mode: nil; -*-

#include <RcppArmadillo.h>
#include <random>
#include <boost/math/distributions/normal.hpp>
#include "myrng.h"

using namespace Rcpp;
using namespace RcppArmadillo;
using namespace boost::math;
// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::depends(BH)]]
// [[Rcpp::plugins(cpp11)]]

// accesses myrng.h to make a random uniform variable
// [[Rcpp::export]]
arma::colvec crunif(unsigned int n, unsigned int seed) {
    Uniform unif(seed);
    arma::colvec out(n, arma::fill::none);
    for(auto i = 0; i < n; i++) {
        out[i] = unif.draw();
    }
    return out;
}

// Pseudocode 2 of Finley et al. 2019
// [[Rcpp::export]]
Rcpp::List csolve_for_A_and_D(arma::sp_mat& cov_cur,
                              Rcpp::List& neighbor_list) {
    int s = cov_cur.n_rows;
    arma::sp_mat A = arma::sp_mat(s, s);
    arma::sp_mat D = arma::sp_mat(s, s);
    arma::uvec N;
    int sizen;
    D[0] = cov_cur[0];

    for(auto i = 0; i < (s-1); i++) {
        // minus 1 because 0 vs 1-based indexing
        N = as<arma::uvec>(neighbor_list[i+1]) - 1;
        sizen = N.size()-1;
        A.row(i+1).cols(N[0], N[sizen]) = arma::solve(arma::mat(cov_cur.cols(N[0], N[sizen]).rows(N[0], N[sizen])), arma::colvec(cov_cur.col(i+1).rows(N[0], N[sizen]))).t();
        D.row(i+1).col(i+1) = cov_cur.col(i+1).row(i+1) - dot(arma::rowvec(cov_cur.cols(N[0], N[sizen]).row(i+1)), A.cols(N[0], N[sizen]).row(i+1));
    }
    return Rcpp::List::create(
        Rcpp::Named("A") = A,
        Rcpp::Named("D") = D);
}

// [[Rcpp::export]]
double csparse_quadratic_form_symm(arma::colvec& u,
                                      arma::sp_mat& A,
                                      arma::colvec& D,
                                      Rcpp::List& neighbor_list) {
    double tmp = pow(u[0], 2) / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += pow(u[i] - arma::dot(A.cols(N[0], N[sizen]).row(i), u.elem(N).t()), 2) / D[i];
    }
    return(tmp);
}

// [[Rcpp::export]]
double csparse_quadratic_form_asymm(arma::colvec u,
                                    arma::colvec v,
                                    arma::sp_mat A,
                                    arma::colvec D,
                                    Rcpp::List neighbor_list) {
    double tmp = u[0] * v[0] / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += (u[i] - arma::dot(A.cols(N[0], N[sizen]).row(i), u.elem(N).t())) *
            (v[i] - arma::dot(A.cols(N[0], N[sizen]).row(i), v.elem(N).t())) / D[i];
    }
    return(tmp);
}

// [[Rcpp::export]]
Rcpp::List csolve_for_B_and_b(arma::mat& y,
                              arma::mat& X,
                              arma::sp_mat& A,
                              arma::colvec& D,
                              Rcpp::List& neighbor_list,
                              arma::mat& precision_beta) {
    int p = X.n_cols;
    arma::colvec b(p, arma::fill::ones);
    arma::mat B(p, p, arma::fill::ones);
    arma::colvec ybar = mean(y, 1);

    for(auto i = 0; i < p; i++) {
        b[i] = csparse_quadratic_form_asymm(X.col(i), ybar, A, D, neighbor_list);
        for(auto j = 0; j < p; j++) {
            B.col(j).row(i) = csparse_quadratic_form_asymm(X.col(i), X.col(j), A, D, neighbor_list);
        }
    }

    return(Rcpp::List::create(
            Rcpp::Named("B") = B + precision_beta,
            Rcpp::Named("b") = b)
               );
}

// [[Rcpp::export]]
arma::mat cmake_one_pred_sparse(Rcpp::List& neighbor_list,
                                arma::mat& y,
                                arma::mat& s,
                                arma::mat& X,
                                arma::mat& cond_cov,
                                int BOOT,
                                unsigned int SEED) {
    int n = y.n_rows;
    arma::mat L;

    arma::mat uniform_rv;
    uniform_rv = arma::reshape(crunif(n * BOOT, SEED), n, BOOT);
    arma::mat z(n, BOOT, arma::fill::none);
    arma::mat preds(n, BOOT, arma::fill::none);
    normal standard_normal;

    for(auto i = 0; i < n; i++) {
        for(auto j = 0; j < BOOT; j++) {
            z(i,j) = quantile(standard_normal, uniform_rv(i,j));
        }
    }

    arma::uvec Nidx;
    int sizen;
    preds.row(0) = sqrt(cond_cov[0]) * z.row(0);
    for(auto i = 1; i < n; i++) {
        Nidx = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = Nidx.size()-1;
        L = arma::chol(cond_cov.cols(Nidx[0], Nidx[sizen]).rows(Nidx[0], Nidx[sizen]), "upper");
        preds.row(i) = L.col(L.n_cols-1).t() * (z.rows(Nidx[0], Nidx[sizen]));
    }

    return(preds);
}

// [[Rcpp::export]]
Rcpp::List csolve_for_A_and_D_2d(arma::sp_mat& cov_cur,
                                 Rcpp::List& neighbor_list) {
    int s = cov_cur.n_rows;
    arma::mat cov_cur_dense = arma::mat(cov_cur);

    arma::mat A = arma::mat(s, s);
    arma::sp_mat D = arma::sp_mat(s, s);
    arma::uvec N;

    D[0] = cov_cur[0];
    arma::uvec v = { 1 };
    for(auto i = 0; i < (s-1); i++) {
        // minus 1 because 0 vs 1-based indexing
        N = as<arma::uvec>(neighbor_list[i+1]) - 1;
        A.submat(v, N) = arma::solve(cov_cur_dense.submat(N, N), arma::colvec(cov_cur_dense.submat(N, v))).t();
        D.row(i+1).col(i+1) = cov_cur_dense.col(i+1).row(i+1) - dot(arma::rowvec(cov_cur_dense.submat(v, N)), A.submat(v, N));
        v = v+1;
    }

    return Rcpp::List::create(
        Rcpp::Named("A") = arma::sp_mat(A),
        Rcpp::Named("D") = D);
}

// [[Rcpp::export]]
double csparse_quadratic_form_symm_2d(arma::colvec& u,
                                   arma::sp_mat& A,
                                   arma::colvec& D,
                                   Rcpp::List& neighbor_list) {
    double tmp = pow(u[0], 2) / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;

    arma::mat A_dense = arma::mat(A);
    arma::uvec v = { 1 };
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += pow(u[i] - arma::dot(A_dense.submat(v, N), u.elem(N).t()), 2) / D[i];
        v = v+1;
    }
    return(tmp);
}

// [[Rcpp::export]]
double csparse_quadratic_form_asymm_2d(arma::colvec u,
                                    arma::colvec v,
                                    arma::sp_mat A,
                                    arma::colvec D,
                                    Rcpp::List neighbor_list) {
    double tmp = u[0] * v[0] / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;

    arma::mat A_dense = arma::mat(A);
    arma::uvec vv = { 1 };
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += (u[i] - arma::dot(A_dense.submat(vv, N), u.elem(N).t())) *
            (v[i] - arma::dot(A_dense.submat(vv, N), v.elem(N).t())) / D[i];
        vv = vv+1;
    }
    return(tmp);
}

// [[Rcpp::export]]
Rcpp::List csolve_for_B_and_b_2d(arma::mat& y,
                              arma::mat& X,
                              arma::sp_mat& A,
                              arma::colvec& D,
                              Rcpp::List& neighbor_list,
                              arma::mat& precision_beta) {
    int p = X.n_cols;
    arma::colvec b(p, arma::fill::ones);
    arma::mat B(p, p, arma::fill::ones);
    arma::colvec ybar = mean(y, 1);

    for(auto i = 0; i < p; i++) {
        b[i] = csparse_quadratic_form_asymm_2d(X.col(i), ybar, A, D, neighbor_list);
        for(auto j = 0; j < p; j++) {
            B.col(j).row(i) = csparse_quadratic_form_asymm_2d(X.col(i), X.col(j), A, D, neighbor_list);
        }
    }

    return(Rcpp::List::create(
            Rcpp::Named("B") = B + precision_beta,
            Rcpp::Named("b") = b)
    );
}


#include "mode_calc_pca.hpp"

#include <mill/util/cmdarg.hpp>
#include <mill/math/Vector.hpp>
// #include <mill/math/EigenSolver.hpp>
#include <mill/traj.hpp>
#include <toml/toml.hpp>
#include <numeric>

#include <Eigen/Dense>

namespace mill
{

const char* mode_calc_pca_usage() noexcept
{
    return "usage: mill calc pca {trajectory} [--top=(3 by default)] [--top-contribution=(95% by default)] [--output=(\"{base name of trajectory}_pca.dat\" by default)]\n"
           "         determines principal component from traj file.\n"
           "         --top=N specifies the number of component will be written out.\n"
           "         --top-contribution=% specifies the number of component via the accumulated contribution rate.\n"
           "         if you specify `--top=3 --top-contribution=95` and it would be found that top 5 components are\n"
           "         needed to achieve 95% contribution, 5 components are written.\n"
           "     $ mill calc pca {input.toml}\n"
           "         determines principal component using customized input.\n"
           "         ```toml\n"
           "         input  = \"traj.dcd\"\n"
           "         output_basename = \"pca.dat\"\n"
           "         # (optional) how many components are reported.\n"
           "         # by default, 3.\n"
           "         top    = 10\n"
           "         # (optional) instead of `top`, output top N% contributing PCs.\n"
           "         top_contribution = 95 # %\n"
           "         # (optional) indices of particles to be used.\n"
           "         # by default, all the particles are used.\n"
           "         # {first, last} specifies the range. last is not included.\n"
           "         use    = [0, 2, 5, {first=10, last=20}]\n"
           "         ```";
}

int mode_calc_pca(std::deque<std::string_view> args)
{
    using namespace std::literals::string_literals;

    auto top_n_opt       = pop_argument<std::size_t>(args, "top");
    auto top_contrib_opt = pop_argument<double     >(args, "top-contribution");
    auto output_opt      = pop_argument<std::string>(args, "output");
    std::vector<std::size_t> particles_to_be_used;

    if(args.empty())
    {
        log::error("mill calc pca: too few arguments.");
        log::error(mode_calc_pca_usage());
        return 1;
    }

    for(const auto& arg : args)
    {
        if(arg.substr(0, 2) == "--")
        {
            log::error("unknown argument ", arg, " found. It will be ignored. please check it.");
        }
    }

    const std::string fname(args.front());
    if(fname == "help")
    {
        log::info(mode_calc_pca_usage());
        return 0;
    }

    std::string trajfile;
    if(extension_of(fname) == ".toml")
    {
        const auto data = toml::parse(fname);

        trajfile = toml::find<std::string>(data, "input");

        if(data.contains("top"))
        {
            top_n_opt = toml::find<std::size_t>(data, "top");
        }

        if(data.contains("top_contribution"))
        {
            top_contrib_opt = toml::find<double>(data, "top_contribution");
        }
        if(data.contains("top-contribution"))
        {
            top_contrib_opt = toml::find<double>(data, "top-contribution");
        }

        if(data.contains("output_basename"))
        {
            output_opt = toml::find<std::string>(data, "output_basename");
        }
        if(data.contains("use"))
        {
            for(auto&& elem : toml::find<toml::array>(data, "use"))
            {
                if(elem.is_integer())
                {
                    particles_to_be_used.push_back(elem.as_integer());
                }
                else
                {
                    for(std::size_t i=elem.at("first").as_integer(), e=elem.at("last").as_integer(); i<e; ++i)
                    {
                        particles_to_be_used.push_back(i);
                    }
                }
            }
        }
        else
        {
            particles_to_be_used.resize(reader(trajfile).read_frame()->size(), 0);
            std::iota(particles_to_be_used.begin(), particles_to_be_used.end(), 0);
        }
    }
    else
    {
        trajfile = std::string(fname);
        particles_to_be_used.resize(reader(trajfile).read_frame()->size(), 0);
        std::iota(particles_to_be_used.begin(), particles_to_be_used.end(), 0);
    }

    if(not output_opt.has_value())
    {
        output_opt = std::string(base_name_of(trajfile));
    }

    const std::string output_basename = output_opt.value();

    log::info("It will use ", particles_to_be_used.size(), " particles in total.");
    if(top_n_opt.has_value())
    {
        log::info("It will output ", top_n_opt.value(), " components");
    }
    if(top_contrib_opt.has_value())
    {
        log::info("It will output top ", top_contrib_opt.value(), "% contributing components.");
    }
    log::info("The results will be written in ", output_basename);

    // we will use this `traj` to write the movement corresponds to the principal components.
    Trajectory traj = reader(trajfile).read();

    log::debug("input files are loaded.");
    log::info("trajectory has ", traj.size(), " snapshots.");

    // -----------------------------------------------------------------------
    // constructing covariance matrix

    std::size_t num_frames = 0;
    std::vector<double> means(particles_to_be_used.size() * 3, 0.0);
    for(const auto frame : traj)
    {
#pragma omp parallel for
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            means[i*3+0] += frame.at(particles_to_be_used[i]).position()[0];
            means[i*3+1] += frame.at(particles_to_be_used[i]).position()[1];
            means[i*3+2] += frame.at(particles_to_be_used[i]).position()[2];
        }
        num_frames += 1;
    }

    const double normalize = 1.0 / num_frames;
#pragma omp parallel for
    for(std::size_t i=0; i<means.size(); ++i)
    {
        auto& mean = means[i];
        mean *= normalize;
    }

    log::info("mean positions are calculated");

    // {x1, y1, z1, x2, y2, z2, ...}
    Eigen::MatrixXd mat = Eigen::MatrixXd::Zero(particles_to_be_used.size() * 3,
                                                particles_to_be_used.size() * 3);

    for(const auto frame : traj)
    {
        // the matrix often will be large enough for this
#pragma omp parallel for
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            const auto pix = frame.at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            const auto piy = frame.at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            const auto piz = frame.at(particles_to_be_used[i]).position()[2] - means[i*3+2];

            for(std::size_t j=i; j<particles_to_be_used.size(); ++j)
            {
                const auto pjx = frame.at(particles_to_be_used[j]).position()[0] - means[j*3+0];
                const auto pjy = frame.at(particles_to_be_used[j]).position()[1] - means[j*3+1];
                const auto pjz = frame.at(particles_to_be_used[j]).position()[2] - means[j*3+2];

                mat(i*3+0, j*3+0) += pix * pjx;
                mat(i*3+0, j*3+1) += pix * pjy;
                mat(i*3+0, j*3+2) += pix * pjz;

                mat(i*3+1, j*3+0) += piy * pjx;
                mat(i*3+1, j*3+1) += piy * pjy;
                mat(i*3+1, j*3+2) += piy * pjz;

                mat(i*3+2, j*3+0) += piz * pjx;
                mat(i*3+2, j*3+1) += piz * pjy;
                mat(i*3+2, j*3+2) += piz * pjz;
            }
        }
    }

#pragma omp parallel for
    for(std::size_t i=0; i<particles_to_be_used.size() * 3; ++i)
    {
        mat(i, i) *= normalize;
        for(std::size_t j=i+1; j<particles_to_be_used.size() * 3; ++j)
        {
            mat(i, j) *= normalize;
            mat(j, i) = mat(i, j);
        }
    }

    log::info("co-variance matrix is constructed");

    // -----------------------------------------------------------------------
    // calculating eigenvalues of covariance matrix

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(mat);
    std::vector<std::pair<double, Eigen::VectorXd>> eigens(particles_to_be_used.size() * 3);

#pragma omp parallel for
    for(std::size_t i=0; i<particles_to_be_used.size() * 3; ++i)
    {
        eigens[i].first  = solver.eigenvalues()[i];
        eigens[i].second = solver.eigenvectors().col(i);
    }

    std::sort(eigens.begin(), eigens.end(), [](const auto& lhs, const auto& rhs) {
            return lhs.first > rhs.first;
        });

    log::info("eigenvalues are calculated");

    // -----------------------------------------------------------------------
    // determining contribution rate

    const auto sum_eigenvalues = std::accumulate(eigens.begin(), eigens.end(),
        0.0, [](const auto& acc, const auto& elem) {
            return acc + elem.first;
        }) * 0.01; // scale to the per-cent
    std::vector<double> contribution_rate(eigens.size());
    std::transform(eigens.begin(), eigens.end(), contribution_rate.begin(),
            [sum_eigenvalues](const auto& eigenpair) noexcept -> double {
                return eigenpair.first / sum_eigenvalues;
            });

    std::vector<double> accumulated_contribution_rate(eigens.size());
    const double contribution_threshold = top_contrib_opt.value_or(95.0);
    std::size_t top_contributions = 0;
    double accum = 0.0;
    std::transform(contribution_rate.begin(), contribution_rate.end(),
            accumulated_contribution_rate.begin(),
            [&accum, &top_contributions, contribution_threshold](const auto& rate) {
                if(accum <= contribution_threshold)
                {
                    top_contributions += 1;
                }
                accum += rate;
                return accum;
            });

    std::size_t num_components = std::max<std::size_t>(3, top_contributions);
    if(top_n_opt.has_value())
    {
        num_components = top_n_opt.value();
    }
    if(top_contrib_opt.has_value())
    {
        num_components = top_contributions;
    }
    if(top_n_opt.has_value() && top_contrib_opt.has_value())
    {
        num_components = std::max(top_contributions, top_n_opt.value());
    }

    if(num_components < eigens.size())
    {
        eigens.resize(num_components);
    }

    log::info("top ", num_components, " components (",
            accumulated_contribution_rate.at(num_components), "% contribution) will be written");

    // ------------------------------------------------------------------------
    // determine direction (sign) of eigenvector to make the trajectory (-)->(+)

    for(auto& [eigval, eigvec] : eigens)
    {
        Eigen::VectorXd first_frame = Eigen::VectorXd::Zero(particles_to_be_used.size() * 3);
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            first_frame[i*3+0] = traj.front().at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            first_frame[i*3+1] = traj.front().at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            first_frame[i*3+2] = traj.front().at(particles_to_be_used[i]).position()[2] - means[i*3+2];
        }
        Eigen::VectorXd  last_frame = Eigen::VectorXd::Zero(particles_to_be_used.size() * 3);
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            last_frame[i*3+0] = traj.back().at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            last_frame[i*3+1] = traj.back().at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            last_frame[i*3+2] = traj.back().at(particles_to_be_used[i]).position()[2] - means[i*3+2];
        }

        const auto first = eigvec.dot(first_frame);
        const auto  last = eigvec.dot( last_frame);
        if(last < first)
        {
            eigvec *= -1.0;
        }
    }

    // -----------------------------------------------------------------------
    // writing trajectory along the eigenvector

    std::vector<std::pair<double, double>> component_range(eigens.size(),
            std::make_pair(std::numeric_limits<double>::max(),
                          -std::numeric_limits<double>::max()));

    std::ofstream ofs(output_basename + "_PCA.dat"s);
    if(not ofs.good())
    {
        log::fatal("file open error: ", output_basename + "_PCA.dat"s);
    }

    ofs << '#';
    for(std::size_t i=0; i<eigens.size(); ++i)
    {
        ofs << " PC" << i;
    }
    ofs << '\n';

    ofs << '#';
    for(std::size_t i=0; i<eigens.size(); ++i)
    {
        ofs << " " << std::fixed << std::setprecision(5) << contribution_rate.at(i) << "%";
    }
    ofs << '\n';

    for(const auto frame : traj)
    {
        Eigen::VectorXd snapshot = Eigen::VectorXd::Zero(particles_to_be_used.size() * 3);
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            snapshot[i*3+0] = frame.at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            snapshot[i*3+1] = frame.at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            snapshot[i*3+2] = frame.at(particles_to_be_used[i]).position()[2] - means[i*3+2];
        }

        std::size_t idx=0;
        for(const auto& eigen : eigens)
        {
            const auto component = snapshot.dot(eigen.second);
            ofs << component << ' ';

            component_range.at(idx).first  = std::min(component, component_range.at(idx).first);
            component_range.at(idx).second = std::max(component, component_range.at(idx).second);

            ++idx;
        }
        ofs << '\n';
    }

    log::info("trajectory along PCs are written in ", output_basename + "_PCA.dat");

    // -----------------------------------------------------------------------
    // output principal motion

    // freeze the trajectory at the mean position to see the movement of selected particles
    auto& init = traj.at(0);
    for(std::size_t i=1; i<traj.size(); ++i)
    {
        const auto frame = traj.at(i);
        for(std::size_t p_idx=0; p_idx<frame.size(); ++p_idx)
        {
            init.at(p_idx).position() += frame.at(p_idx).position();
        }
    }
    for(std::size_t p_idx=0; p_idx<init.size(); ++p_idx)
    {
        init.at(p_idx).position() *= normalize;
    }

    const std::size_t PC_movement_len = std::min<std::size_t>(1000, traj.size());
    traj.snapshots().resize(PC_movement_len);

    for(std::size_t i=1; i<traj.size(); ++i)
    {
        traj.at(i) = init;
    }

    // construct movement along PCs

    std::size_t idx=0;
    for(const auto& eigen : eigens)
    {
        const auto [lower, upper] = component_range.at(idx);

        const auto dx = (upper - lower) / PC_movement_len;
        for(std::size_t t=0; t<PC_movement_len; ++t)
        {
            auto& frame = traj.at(t);

            const auto x = lower + dx * t;
            for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
            {
                frame.at(particles_to_be_used[i]).position()[0] = means.at(i*3+0) + eigen.second[i*3+0] * x;
                frame.at(particles_to_be_used[i]).position()[1] = means.at(i*3+1) + eigen.second[i*3+1] * x;
                frame.at(particles_to_be_used[i]).position()[2] = means.at(i*3+2) + eigen.second[i*3+2] * x;
            }
        }
        const auto outtrajname = output_basename + "_PC"s +
            std::to_string(idx+1) + std::string(extension_of(trajfile));
        auto w = writer(outtrajname);
        w.write(traj);

        log::info("structure change along PC", idx+1, " (", contribution_rate.at(idx) , "% contribution) is written in ", outtrajname);

        ++idx;
    }

    // -----------------------------------------------------------------------
    // output principal vectors as a trajectory

    // normally, number of Eigen vectors to be written is smaller than 1000...
    traj.snapshots().resize(eigens.size());
    for(std::size_t i=0; i<eigens.size(); ++i)
    {
        const auto& [val, vec] = eigens.at(i);

        auto& frame = traj.at(i);
        for(auto& p : frame)
        {
            p.position() = Vector<double, 3>(0, 0, 0); // clear frame
        }
        // write eigen vector
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            frame.at(particles_to_be_used[i]).position()[0] = vec[i*3+0];
            frame.at(particles_to_be_used[i]).position()[1] = vec[i*3+1];
            frame.at(particles_to_be_used[i]).position()[2] = vec[i*3+2];
        }
    }
    const auto eigen_vec_name = output_basename + "_EigenVectors" +
                                std::string(extension_of(trajfile));
    auto w = writer(eigen_vec_name);
    w.write(traj);

    return 0;
}

} // mill


/// @file types.hpp Simple typedefs used by Biggles

#ifndef BIGGLES_TYPES_HPP__
#define BIGGLES_TYPES_HPP__

#include <Eigen/Dense>

namespace biggles {

typedef Eigen::Matrix2f matrix2f;
typedef Eigen::Matrix4f matrix4f;
typedef Eigen::Matrix<float, 2, 4> matrix2x4f;
typedef Eigen::Matrix<float, 4, 2> matrix4x2f;

typedef Eigen::Vector4f state_t; // the underlying state vector type: [x, x', y, y']'

namespace model {

/// @brief A tuple representing the model parameters.
///
/// The model parameters are represented by the tuple \f$ ( \lambda_b, \lambda_f, p_s, p_d, R ) \f$ where
///
/// - \f$ \lambda_b \f$: the mean number of new tracks appearing per frame.
/// - \f$ \lambda_f \f$: the mean number of false observations per frame.
/// - \f$ p_s \f$: the probability that a target will survive from frame \f$ t \f$ to \f$ t+1 \f$.
/// - \f$ p_d \f$: the probability that a target will generate an observation.
/// - \f$ r \f$: the constraint radius.
/*
typedef boost::tuples::tuple<float, float, float, float, matrix2f, float, matrix4f> parameters;
*/

struct parameters {
    float birth_rate;
    float clutter_rate;
    float survival_probability;
    float observation_probability;
    matrix2f observation_error_covariance;
    float constraint_radius;
    matrix4f process_noise_covariance;
    bool operator==(const parameters &other) const {
        return
        other.birth_rate == birth_rate and
        other.clutter_rate == clutter_rate and
        other.survival_probability == survival_probability and
        other.observation_probability == observation_probability and
        other.constraint_radius == constraint_radius and
        other.observation_error_covariance == observation_error_covariance and
        other.process_noise_covariance == process_noise_covariance;
    }
};

}

}

#endif // BIGGLES_MODEL_HPP__


#pragma once

#include <boost/optional.hpp>
#include <math/Plane.hpp>
#include <math/Ray.hpp>
#include <math/Vec.hpp>

template <int n, class T>
inline boost::optional<Vec<n, T> > intersection(Ray<n, T> const & ray, Plane<n, T> const & plane) {
	T denom = dot(ray.get_dir(), plane.get_normal());
	if (denom == 0) {
        if (plane.contains(ray.get_start())) { return ray.get_start(); }
        else { return boost::none; }
    }

	T t = (plane.get_dist() - dot(ray.get_start(), plane.get_normal())) / denom;
    if (t < 0) { return boost::none; }
	return ray(t);
}

template <int n, class T>
inline boost::optional<Vec<n, T> > intersection(Plane<n, T> const & plane, Ray<n, T> const & ray) {
	return intersection(ray, plane);
}


#include <stdio.h>
#include "EM.h"
#include <cstdlib>
#include <string>
#include <time.h>
#include <boost/random.hpp>

using namespace ai::em;
using namespace std;
int main(int argc, char** argv)
{
    Parameters param[6];
    // peff[j][i]: probability of effect #i = 0 given gender = j
	// j = 0 for male, j=1 for female
	// i = 0 for height, i = 1 for weight
	// so peff[0][0] is the probability of height > 55  given male gender
	//    peff[0][1] is the probability of weight > 130 given male gender
	//    peff[1][0] is the probability of height > 55  given female gender
	//    peff[1][1] is the probability of weight > 130 given female gender
    param[0].pg0 = 0.7;
    param[0].peff[0][0] = 0.7; // height 0 (>55) given male
    param[0].peff[0][1] = 0.8; // weight 0 (>130) given male
    param[0].peff[1][0] = 0.3; // height 0 (>55) given female
    param[0].peff[1][1] = 0.4; // weight 0 (>130) given female

    if(argc == 7)
    {
        param[0].pg0 = atof(argv[2]);
        param[0].peff[0][0] = atof(argv[3]); // height 0 (>55) given male
        param[0].peff[0][1] = atof(argv[4]); // weight 0 (>130) given male
        param[0].peff[1][0] = atof(argv[5]); // height 0 (>55) given female
        param[0].peff[1][1] = atof(argv[6]); // weight 0 (>130) given female
    }
//    //generating five random parameters set
    time_t timer;
    time(&timer);  /* get current time; same as: timer = time(NULL)  */
    boost::mt19937 seed( (int)timer );
    boost::uniform_real<> dist(0.0,1.0);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > random(seed,dist);
    for (int i=1; i <6 ; ++i)
    {
        param[i].pg0 = random();
        param[i].peff[0][0] = random(); // height 0 (>55) given male
        param[i].peff[0][1] = random(); // weight 0 (>130) given male
        param[i].peff[1][0] = random();// height 0 (>55) given female
        param[i].peff[1][1] = random();// weight 0 (>130) given female
    }
    EM em;
    em.read_input(argv[1]);
    // five more random starting parameters
//    cout<<"random number 1 = "<<random()<<endl;
//    cout<<"random number 2 = "<<random()<<endl;
//    cout<<"random number 3= "<<random()<<endl;
    for (int i=0; i <6 ; ++i)
    {

        std::cout <<"Starting parameter set "<<i<<": " << endl;
        em.setParameters(param[i]);
        //em.read_input(argv[1]);
        Parameters final_param = em.optimize();
        printf ("%40s\t %20s\t %20s\n", "Probability", "Starting parameter", "final parameters");
        printf ("%40s\t %20.6f\t %20.6f\n", "P(gender=M)" ,  param[i].pg0, final_param.pg0);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(weight=greater_than_130|gender=M)", param[i].peff[0][0], final_param.peff[0][0]);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(weight=greater_than_130|gender=F)", param[i].peff[0][1], final_param.peff[0][1]);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(height= greater_than_55|gender=M)", param[i].peff[1][0], final_param.peff[1][0]);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(height= greater_than_55|gender=F)", param[i].peff[1][1], final_param.peff[1][1]);
    }

    return 0;

}


// arrayopt.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#define FMT_HEADER_ONLY
#define CXXOPTIONS_NO_EXCEPTIONS
#include <iostream>
#include <filesystem>
#include <complex>
#include <fmt/format.h> //while waiting for official C++20 support
#include <Eigen/Dense>
#include <boost/math/quadrature/gauss.hpp>
#include <cxxopts.hpp>

namespace bmq = boost::math::quadrature;

#define pi EIGEN_PI

constexpr int EXIT_UNRECOGNIZED_OPTION = 2;
constexpr int EXIT_LINALG_FAILURE = 3;

std::string header(const size_t width, const std::string msg)
{
    std::string hdr = "";
    size_t hmn = (width - msg.size()-4);
    if (hmn % 2 == 1)
    {
        hmn += 1;
    }
    hdr += "\n";
    for (int i = 0; i < hmn / 2; i++)
    {
        hdr += "-";
    }
    hdr += " " + msg + " ";
    while (!(hdr.size() == width))
    {
        hdr += "-";
    }
    hdr += "\n";
    return hdr;
}

double f_dipole(double theta, double phi)
{
    return std::sin(theta);
}

std::complex<double> blm_integrand(double k, double theta, double phi, double xl, double xm, double yl, double ym)
{
    double amp = std::pow(f_dipole(theta, phi), 2);
    double tmx = k * (xl - xm) * std::sin(theta) * std::cos(phi);
    double tmy = k * (yl - ym) * std::sin(theta) * std::sin(phi);
    std::complex<double> arg(0, -(tmx+tmy)); //-i(tmx+tmy)

    return amp * std::exp(arg);
}

std::complex<double> blm(double k, double xl, double xm, double yl, double ym)
{
    // what are our options to do the double integral here??
    // https://royalsocietypublishing.org/doi/pdf/10.1098/rspa.2016.0401
    // delunay integration https://scholarworks.gsu.edu/cgi/viewcontent.cgi?article=1158&context=math_theses
    // monte carlo brute force?
    // can we get this in closed form for dipole /
    return std::complex(nan(""), nan(""));
}

std::string efstr = "\n**ERROR! {}\n";
std::string msg;

int main(int argc, char** argv)
{
    cxxopts::Options options("arrayopt", "Uses M.T. Ma's approach to compute array currents for optimal directivity.");
    options.add_options()
        ("help", "Print this screen")
        ("freq", "Frequency in MHz", cxxopts::value<double>()->default_value("1.8"))
        ("az", "Target azimuth angle, degrees", cxxopts::value<double>()->default_value("0.0"))
        ("el", "Target elevation angle, degrees", cxxopts::value<double>()->default_value("0.0"))
        ("file", "Wire description file, EZNEC export format, units in meters/mm", cxxopts::value<std::string>())
        ;

    double az, el, azrad, elrad, freq;
    std::string fn;
    try
    {
        auto opts = options.parse(argc, argv);
        if (opts.count("help"))
        {
            std::cout << options.help() << std::endl;
            return EXIT_FAILURE;
        }
        az = opts["az"].as<double>();
        el = opts["el"].as<double>();
        freq = opts["freq"].as<double>();
        azrad = pi * az / 180;
        elrad = pi * az / 180;
    }
    catch (cxxopts::OptionParseException& e)
    {
        std::cout << fmt::format(efstr, e.what()) << std::endl;
        return EXIT_UNRECOGNIZED_OPTION;
    }

    msg = "N3OX / M.T. Ma optimal directivity array calculator";
    std::cout << header(80, msg) << std::endl;

    double k0 = 2 * pi / (freq * 1e6); //wavenumber

    msg = "azimuth: {0:.3f} degrees, elevation: {1:.3f} degrees, freq: {2:.3f} MHz, wavenumber: {3:.3e}/m";
    std::cout << fmt::format(msg, az, el, freq, k0) << std::endl;

    // ==== hardcode some element positions for now ====
    std::vector<double> elx;
    std::vector<double> ely;
    for (int i = 0; i < 3; i++)
    {
        elx.push_back(20.0 * i);
        ely.push_back(0.0);
    }
    if (!(elx.size() == ely.size()))
    {
        std::cout << fmt::format(efstr, "Element x, y coordinates not the same size!") << std::endl;
    }
    size_t Nel = elx.size();

    // ==== B matrix ====
    Eigen::MatrixXcd B;
    B.resize(Nel, Nel);

    for (size_t l = 0; l < Nel; l++)
    {
        for (size_t m = 0; m < Nel; m++)
        {
            B(l, m) = std::complex(l / 10.0, m / 10.0);
        }
    }

    std::cout << B << std::endl;

    std::cout << header(80, "Finished");
    return EXIT_SUCCESS;
}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started:
//   1. Use the Solution Explorer window to add/manage files
//   2. Use the Team Explorer window to connect to source control
//   3. Use the Output window to see build output and other messages
//   4. Use the Error List window to view errors
//   5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
//   6. In the future, to open this project again, go to File > Open > Project and select the .sln file


#include <cstdlib>
#include <iostream>
#include <complex>
#define BOOST_UBLAS_NO_ELEMENT_PROXIES
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/unit_lower.hpp>
#include <boost/numeric/bindings/unit_upper.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include <boost/numeric/bindings/left.hpp>
#include <boost/numeric/bindings/right.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef typename vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix A_l(n, n), A_u(n, n), B(n, n);
    for (size_type j=0; j<n; ++j) {
      for (size_type i=0; i<j; ++i) {
    	A_u(i, j)=rand_normal<complex>::get();
	A_l(j, i)=rand_normal<complex>::get();
      }
      A_u(j, j)=rand_normal<complex>::get();
      A_l(j, j)=rand_normal<complex>::get();
      for (size_type i=j+1; i<n; ++i) {
    	A_u(i, j)=0;
	A_l(j, i)=0;
      }
    }
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
	B(j, i)=rand_normal<complex>::get();
    complex alpha=rand_normal<complex>::get();
    {
      matrix B1(alpha*ublas::prod(A_l, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(A_u, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_l), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_u), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_l)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_u)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

    {
      matrix B1(alpha*ublas::prod(B, A_l));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, A_u));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_l)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_u)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_l))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_u))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

    for (size_type i=0; i<n; ++i) {
      A_l(i, i)=1;
      A_u(i, i)=1;
    }

    {
      matrix B1(alpha*ublas::prod(A_l, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::unit_lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(A_u, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::unit_upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_l), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_u), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_l)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_u)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

    {
      matrix B1(alpha*ublas::prod(B, A_l));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::unit_lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, A_u));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::unit_upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_l)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_u)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_l))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_u))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

  }
  return EXIT_SUCCESS;
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>
#include <limits>
#include <stdexcept>

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

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h"

namespace tudat
{

namespace gravitation
{

//! Compute gravitational acceleration due to multiple spherical harmonics terms, defined using geodesy-normalization.
Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameter,
        const double equatorialRadius,
        const Eigen::MatrixXd& cosineHarmonicCoefficients,
        const Eigen::MatrixXd& sineHarmonicCoefficients,
        std::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache,
        std::map< std::pair< int, int >, Eigen::Vector3d >& accelerationPerTerm,
        const bool saveSeparateTerms,
        const Eigen::Matrix3d& accelerationRotation )
{
    // Set highest degree and order.
    const int highestDegree = cosineHarmonicCoefficients.rows( );
    const int highestOrder = cosineHarmonicCoefficients.cols( );

    // Declare spherical position vector.
    Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = coordinate_conversions::
            convertCartesianToSpherical( positionOfBodySubjectToAcceleration );
    sphericalpositionOfBodySubjectToAcceleration( 1 ) = mathematical_constants::PI / 2.0 -
            sphericalpositionOfBodySubjectToAcceleration( 1 );

    double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) );
    sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ),
                                     sineOfAngle,
                                     sphericalpositionOfBodySubjectToAcceleration( 2 ),
                                     equatorialRadius );

    std::shared_ptr< basic_mathematics::LegendreCache > legendreCacheReference =
            sphericalHarmonicsCache->getLegendreCache( );

    // Compute gradient premultiplier.
    const double preMultiplier = gravitationalParameter / equatorialRadius;

    // Initialize gradient vector.
    Eigen::Vector3d sphericalGradient = Eigen::Vector3d::Zero( );

    Eigen::Matrix3d transformationToCartesianCoordinates = coordinate_conversions::getSphericalToCartesianGradientMatrix(
                positionOfBodySubjectToAcceleration );

    // Loop through all degrees.
    for ( int degree = 0; degree < highestDegree; degree++ )
    {
        // Loop through all orders.
        for ( int order = 0; ( order <= degree ) && ( order < highestOrder ); order++ )
        {
            // Compute geodesy-normalized Legendre polynomials.
            const double legendrePolynomial = legendreCacheReference->getLegendrePolynomial( degree, order );

            // Compute geodesy-normalized Legendre polynomial derivative.
            const double legendrePolynomialDerivative = legendreCacheReference->getLegendrePolynomialDerivative(
                        degree, order );

            // Compute the potential gradient of a single spherical harmonic term.
            if( saveSeparateTerms )
            {
                accelerationPerTerm[ std::make_pair( degree, order ) ] =
                        basic_mathematics::computePotentialGradient(
                            sphericalpositionOfBodySubjectToAcceleration,
                            preMultiplier,
                            degree,
                            order,
                            cosineHarmonicCoefficients( degree, order ),
                            sineHarmonicCoefficients( degree, order ),
                            legendrePolynomial,
                            legendrePolynomialDerivative, sphericalHarmonicsCache );
                sphericalGradient += accelerationPerTerm[ std::make_pair( degree, order ) ];
                accelerationPerTerm[ std::make_pair( degree, order ) ] =
                        accelerationRotation * (
                            transformationToCartesianCoordinates * accelerationPerTerm[ std::make_pair( degree, order ) ] );
            }
            else
            {
                // Compute the potential gradient of a single spherical harmonic term.
                sphericalGradient += basic_mathematics::computePotentialGradient(
                            sphericalpositionOfBodySubjectToAcceleration,
                            preMultiplier,
                            degree,
                            order,
                            cosineHarmonicCoefficients( degree, order ),
                            sineHarmonicCoefficients( degree, order ),
                            legendrePolynomial,
                            legendrePolynomialDerivative, sphericalHarmonicsCache );
            }
        }
    }

    // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector) and
    // return the resulting acceleration vector.
    return accelerationRotation * ( transformationToCartesianCoordinates * sphericalGradient );
}

//! Compute gravitational acceleration due to single spherical harmonics term.
Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameter,
        const double equatorialRadius,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        std::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache )
{
    // Declare spherical position vector.
    Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = coordinate_conversions::
            convertCartesianToSpherical( positionOfBodySubjectToAcceleration );
    sphericalpositionOfBodySubjectToAcceleration( 1 ) = mathematical_constants::PI / 2.0 -
            sphericalpositionOfBodySubjectToAcceleration( 1 );

    double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) );
    sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ),
                                     sineOfAngle,
                                     sphericalpositionOfBodySubjectToAcceleration( 2 ),
                                     equatorialRadius );

    // Compute gradient premultiplier.
    const double preMultiplier = gravitationalParameter / equatorialRadius;

    // Compute geodesy-normalized Legendre polynomials.
    const double legendrePolynomial = sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomial( degree, order );

    // Compute geodesy-normalized Legendre polynomial derivative.
    const double legendrePolynomialDerivative =
            sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomialDerivative( degree, order );

    // Compute the potential gradient of a single spherical harmonic term.
    Eigen::Vector3d sphericalGradient = basic_mathematics::computePotentialGradient(
                sphericalpositionOfBodySubjectToAcceleration,
                preMultiplier,
                degree,
                order,
                cosineHarmonicCoefficient,
                sineHarmonicCoefficient,
                legendrePolynomial,
                legendrePolynomialDerivative, sphericalHarmonicsCache );

    // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector),
    // and return resulting acceleration vector.
    return coordinate_conversions::convertSphericalToCartesianGradient(
                sphericalGradient, positionOfBodySubjectToAcceleration );
}

} // namespace gravitation

} // namespace tudat