#ifndef MODULE_OPTIMIZER_HPP
#define MODULE_OPTIMIZER_HPP

#define _USE_MATH_DEFINES
#define M_PI 3.14159265358979323846

#include <math.h>
#include <Eigen/Core>
#include <algorithm>
#include <functional>
#include <numeric>
#include <vector>
#include "main.h"
#include "utility/io/uart.hpp"
#include "utility/math/saturation.hpp"

namespace module {
namespace MPC {
    namespace AdaptiveMPC {

        class Optimizer {
        public:
            Optimizer(int ch_max);

            template <typename T>
            float ProcessModel(const T& model,
                               int mode,
                               std::vector<float>& states,
                               std::vector<float>& setpoint,
                               std::vector<float>& input_states,
                               std::vector<float>& output_states) {

                utility::io::debug.out("Process model mode %d\n", mode);

                // Given a control action, select the correct model matrix, from that linearize the model about the
                // current state point. Calculate the next state vector. Call the control error function and return the
                // errors
                Eigen::Matrix<float, 4, 4> A_mat;

                if (mode == 1) {
                    // Negative
                    Linearise(model,
                              states,
                              -dotV_a(states[2], model.P_a, model.muscle1),
                              dotV_a(model.P_t, states[3], model.muscle2),
                              -dotV_a_P_m1(states[2], model.P_a, model.muscle1, mode),
                              dotV_a_P_m2(model.P_t, states[3], model.muscle2, mode),
                              A_mat);
                }
                else if (mode == 2) {
                    // Positive
                    Linearise(model,
                              states,
                              dotV_a(model.P_t, states[2], model.muscle1),
                              -dotV_a(states[3], model.P_a, model.muscle2),
                              dotV_a_P_m1(model.P_t, states[2], model.muscle1, mode),
                              -dotV_a_P_m2(states[3], model.P_a, model.muscle2, mode),
                              A_mat);
                }
                else if (mode == 3) {
                    // No move
                    Linearise(model,
                              states,
                              dotV_a(model.P_t, states[2], model.muscle1),
                              dotV_a(model.P_t, states[3], model.muscle2),
                              dotV_a_P_m1(model.P_t, states[2], model.muscle1, mode),
                              dotV_a_P_m2(model.P_t, states[3], model.muscle2, mode),
                              A_mat);
                }
                else {
                    Error_Handler();
                }

                utility::io::debug.out("A mat\n");
                utility::io::debug.out(
                    "%.2f\t %.2f\t %.2f\t %.2f\n", A_mat(0, 0), A_mat(0, 1), A_mat(0, 2), A_mat(0, 3));
                utility::io::debug.out(
                    "%.2f\t %.2f\t %.2f\t %.2f\n", A_mat(1, 0), A_mat(1, 1), A_mat(1, 2), A_mat(1, 3));
                utility::io::debug.out(
                    "%.2f\t %.2f\t %.2f\t %.2f\n", A_mat(2, 0), A_mat(2, 1), A_mat(2, 2), A_mat(2, 3));
                utility::io::debug.out(
                    "%.2f\t %.2f\t %.2f\t %.2f\n", A_mat(3, 0), A_mat(3, 1), A_mat(3, 2), A_mat(3, 3));

                float Sampling_time2 = 0.05;  // 0.01 T_s

                Eigen::Matrix<float, 4, 1> x_state(input_states[0], input_states[1], input_states[2], input_states[3]);

                Eigen::Matrix<float, 4, 1> x_states_update =
                    (Eigen::Matrix<float, 4, 4>::Identity() + Sampling_time2 * A_mat) * x_state;

                output_states.push_back(
                    utility::math::sat(x_states_update(0, 0), std::make_pair(M_PI / 2.0, -M_PI / 2.0)));
                output_states.push_back(utility::math::sat(x_states_update(1, 0), std::make_pair(10, -10)));
                output_states.push_back(utility::math::sat(x_states_update(2, 0), std::make_pair(413685, 0)));
                output_states.push_back(utility::math::sat(x_states_update(3, 0), std::make_pair(413685, 0)));

                return (ControlError(setpoint, output_states));
            }

            float ControlError(std::vector<float>& setpoint, std::vector<float>& output_states) {

                // Do the math to find the relative errors (state, input)

                std::vector<float> state_error = {setpoint[0] - output_states[0],
                                                  setpoint[1] - output_states[1],
                                                  setpoint[2] - output_states[2],
                                                  setpoint[3] - output_states[3]};

                std::vector<float>::const_iterator i = output_states.begin();
                std::vector<float>::const_iterator j = setpoint.begin();
                std::vector<float>::const_iterator k = state_error.begin();
                utility::io::debug.out("%f\t - %f\t = %f\n", *j, *i, *k);
                i++;
                j++;
                k++;
                utility::io::debug.out("%f\t - %f\t = %f\n", *j, *i, *k);
                i++;
                j++;
                k++;
                utility::io::debug.out("%f\t - %f\t = %f\n", *j, *i, *k);
                i++;
                j++;
                k++;
                utility::io::debug.out("%f\t - %f\t = %f\n", *j, *i, *k);
                // TODO Fix
                Eigen::Matrix<float, 4, 1> x_error(state_error[0], state_error[1], state_error[2], state_error[3]);
                return (x_error.transpose() * state_weight * x_error);
            }

            template <typename T>
            float dotV_a(float P_1, float P_2, const T& m) {
                float b       = m.critical_ratio;
                float C       = m.sonic_conductance;
                float A       = C * std::sqrt(m.T_0 / m.T_1);
                float P_b     = P_2 / P_1 - b;
                float b_1     = 1 - b;
                float P_bsqrt = std::sqrt(1 - (P_b / b_1) * (P_b / b_1));

                if (P_2 / P_1 > b) {
                    return std::isinf(P_1 * A * P_bsqrt) ? 0 : P_1 * A * P_bsqrt;
                }
                return std::isinf(P_1 * A) ? 0 : P_1 * A;
            }

            template <typename T>
            float dotV_a_P_m1(float P_1, float P_2, const T& m, int mode) {
                float b       = m.critical_ratio;
                float C       = m.sonic_conductance;
                float A       = C * std::sqrt(m.T_0 / m.T_1);
                float P_b     = P_2 / P_1 - b;
                float i       = A * P_b;
                float b_1     = 1 - b;
                float P_bsqrt = std::sqrt(1 - (P_b / b_1) * (P_b / b_1));

                if (mode == 2 || mode == 3) {
                    // P_2 == P_m1
                    if (P_2 / P_1 > b) {
                        return std::isinf(-(i / (b_1 * b_1 * P_bsqrt))) ? 0 : -(i / (b_1 * b_1 * P_bsqrt));
                    }
                    return 0;
                }
                else {
                    // P_1 == P_m1
                    if (P_2 / P_1 > b) {
                        return std::isinf(A * P_bsqrt + (i * P_2 / (b_1 * b_1 * P_1 * P_bsqrt)))
                                   ? 0
                                   : A * P_bsqrt + (i * P_2 / (b_1 * b_1 * P_1 * P_bsqrt));
                    }

                    return std::isinf(C * std::sqrt(m.T_0 / m.T_1)) ? 0 : C * std::sqrt(m.T_0 / m.T_1);
                }
                return 0;
            }

            template <typename T>
            float dotV_a_P_m2(float P_1, float P_2, const T& m, int mode) {
                float b       = m.critical_ratio;
                float C       = m.sonic_conductance;
                float A       = C * std::sqrt(m.T_0 / m.T_1);
                float P_b     = P_2 / P_1 - b;
                float i       = A * P_b;
                float b_1     = 1 - b;
                float P_bsqrt = std::sqrt(1 - (P_b / b_1) * (P_b / b_1));

                if (mode == 1 || mode == 3) {
                    // P_2 == P_m2
                    if (P_2 / P_1 > b) {
                        // utility::io::debug.out("dotV_a_P_m2 mode 13 >\n");
                        return std::isinf(-i / (b_1 * b_1 * P_bsqrt)) ? 0 : -i / (b_1 * b_1 * P_bsqrt);
                    }
                    // utility::io::debug.out("dotV_a_P_m2 mode 13 <\n");
                    return 0;
                }
                else {
                    // P_1 == P_m2
                    if (P_2 / P_1 > b) {
                        // utility::io::debug.out("dotV_a_P_m2 mode 2 >\n");
                        return std::isinf(A * P_bsqrt + ((P_2 * i) / (b_1 * b_1 * P_1 * P_bsqrt)))
                                   ? 0
                                   : A * P_bsqrt + ((P_2 * i) / (b_1 * b_1 * P_1 * P_bsqrt));
                    }

                    // utility::io::debug.out("dotV_a_P_m2 mode 2 <\n");
                    return std::isinf(C * std::sqrt(m.T_0 / m.T_1)) ? 0 : C * std::sqrt(m.T_0 / m.T_1);
                }
                return 0;
            }

            template <typename T>
            void Linearise(const T& m,
                           std::vector<float>& states,
                           float dotV_a1,
                           float dotV_a2,
                           float ddotV_a1,
                           float ddotV_a2,
                           Eigen::Matrix<float, 4, 4>& A_mat) {

                float L_10 = m.muscle1.L_0;
                float L_20 = m.muscle2.L_0;
                float k_10 = m.muscle1.K_0;
                float k_20 = m.muscle2.K_0;
                float y    = states[0];
                float dy   = states[1];
                float P_m1 = states[2];
                float P_m2 = states[3];
                float P_a  = m.P_a;
                float a1   = m.muscle1.muscle_coefficients[0];
                float b1   = m.muscle1.muscle_coefficients[1];
                float c1   = m.muscle1.muscle_coefficients[2];
                float d1   = m.muscle1.muscle_coefficients[3];
                float a2   = m.muscle2.muscle_coefficients[0];
                float b2   = m.muscle2.muscle_coefficients[1];
                float c2   = m.muscle2.muscle_coefficients[2];
                float d2   = m.muscle2.muscle_coefficients[3];
                auto F_ce1 = m.muscle1.F_ce;
                auto F_ce2 = m.muscle2.F_ce;
                float R1   = m.muscle1.damping_coefficient;
                float R2   = m.muscle2.damping_coefficient;
                float mass = m.mass;

                Eigen::Matrix<float, 6, 1> P_ce1;
                Eigen::Matrix<float, 6, 1> P_ce2;
                Eigen::Matrix<float, 6, 1> dP_ce1;
                Eigen::Matrix<float, 6, 1> dP_ce2;

                Eigen::Matrix<float, 1, 6> k_ce1;
                Eigen::Matrix<float, 1, 6> k_ce2;
                Eigen::Matrix<float, 1, 6> dk_ce1;
                Eigen::Matrix<float, 1, 6> dk_ce2;

                float F_s1;
                float F_s2;
                float F_d1;
                float F_d2;
                float V_m1;
                float V_m2;
                float dV_m1;
                float dV_m2;
                float dotV_m1;
                float dotV_m2;
                float ddotV_m1;
                float ddotV_m2;
                float ddotV_m1_dV_m1;
                float ddotV_m2_dV_m2;

                /* clang-format off */

                float k1    = k_10 + y / L_10;
                float k2    = k_20 - y / L_20;
                float dotk1 = k_10 + dy / L_10;
                float dotk2 = k_20 - dy / L_20;

                P_ce1 << 1,
                         P_m1,
                         (P_m1 * P_m1),
                         (P_m1 * P_m1 * P_m1),
                         (P_m1 * P_m1 * P_m1 * P_m1),
                         (P_m1 * P_m1 * P_m1 * P_m1 * P_m1);
                P_ce2 << 1,
                         P_m2,
                         (P_m2 * P_m2),
                         (P_m2 * P_m2 * P_m2),
                         (P_m2 * P_m2 * P_m2 * P_m2),
                         (P_m2 * P_m2 * P_m2 * P_m2 * P_m2);
                dP_ce1 << 0,
                          1,
                          (2 * P_m1),
                          (3 * P_m1 * P_m1),
                          (4 * P_m1 * P_m1 * P_m1),
                          (5 * P_m1 * P_m1 * P_m1 * P_m1);
                dP_ce2 << 0,
                          1,
                          (2 * P_m2),
                          (3 * P_m2 * P_m2),
                          (4 * P_m2 * P_m2 * P_m2),
                          (5 * P_m2 * P_m2 * P_m2 * P_m2);

                k_ce1 << 1,
                         k1,
                         (k1 * k1),
                         (k1 * k1 * k1),
                         (k1 * k1 * k1 * k1),
                         (k1 * k1 * k1 * k1 * k1);
                k_ce2 << 1,
                         k2,
                         (k2 * k2),
                         (k2 * k2 * k2),
                         (k2 * k2 * k2 * k2),
                         (k2 * k2 * k2 * k2 * k2);
                dk_ce1 << 0,
                          (1 / L_10),
                          (2 * k1 / L_10),
                          (3 * k1 * k1 / L_10),
                          (4 * k1 * k1 * k1 / L_10),
                          (5 * k1 * k1 * k1 * k1 / L_10);
                dk_ce2 << 0,
                          (-1 / L_20),
                          (-2 * k2 / L_20),
                          (-3 * k2 * k2 / L_20),
                          (-4 * k2 * k2 * k2 / L_20),
                          (-5 * k2 * k2 * k2 * k2 / L_20);

                /******************************************************************************************************/
                /********************************************** Column 1 **********************************************/
                /******************************************************************************************************/

                /********************************************* Lin_mat_0_0 ********************************************/
                float Lin_mat_0_0 = 0;
                /********************************************* Lin_mat_1_0 ********************************************/

                // Calculate our first spring force
                F_s1 = dk_ce1 * F_ce1 * P_ce1;

                // Calculate our second spring force
                F_s2 = dk_ce2 * F_ce2 * P_ce2;

                float Lin_mat_1_0 = (-F_s2 - F_s1) / mass;

                /********************************************* Lin_mat_2_0 ********************************************/
                dV_m1          = (3 * a1 * k1 * k1 + 2 * b1 * k1 + c1) / L_10;
                ddotV_m1_dV_m1 =
                    ((a1 * k1 * k1 * k1 + b1 * k1 * k1 + c1 * k1 + d1)
                        * (6 * a1 * k1 * dotk1 + 2 * b1 * dotk1)
                    - (3 * a1 * k1 * k1 * dotk1 + 2 * b1 * k1 * dotk1 + c1 * dotk1)
                        * (3 * a1 * k1 * k1 * dotk1 + 2 * b1 * k1 * dotk1 + c1 * dotk1))
                    / (L_10 * (a1 * k1 * k1 * k1 + b1 * k1 * k1 + c1 * k1 + d1)
                       * (a1 * k1 * k1 * k1 + b1 * k1 * k1 + c1 * k1 + d1));

                float Lin_mat_2_0 = (P_a * dotV_a1 / dV_m1) - (P_m1 * ddotV_m1_dV_m1);
                /********************************************* Lin_mat_3_0 ********************************************/
                dV_m2          = (3 * a2 * k2 * k2 + 2 * b2 * k2 + c2) / L_20;
                ddotV_m2_dV_m2 =
                    ((a2 * k2 * k2 * k2 + b2 * k2 * k2 + c2 * k2 + d2)
                        * (6 * a2 * k2 * dotk2 + 2 * b2 * dotk2)
                    - (3 * a2 * k2 * k2 * dotk2 + 2 * b2 * k2 * dotk2 + c2 * dotk2)
                        * (3 * a2 * k2 * k2 * dotk2 + 2 * b2 * k2 * dotk2 + c2 * dotk2))
                    / (L_20 * (a2 * k2 * k2 * k2 + b2 * k2 * k2 + c2 * k2 + d2)
                       * (a2 * k2 * k2 * k2 + b2 * k2 * k2 + c2 * k2 + d2));

                float Lin_mat_3_0 = (P_a * dotV_a2 / dV_m2) - (P_m2 * ddotV_m2_dV_m2);
                /******************************************************************************************************/
                /********************************************** Column 2 **********************************************/
                /******************************************************************************************************/

                /********************************************* Lin_mat_0_1 ********************************************/
                float Lin_mat_0_1 = 1;
                /********************************************* Lin_mat_1_1 ********************************************/
                F_d1 = -R1 * P_m1 / L_10;
                F_d2 = -R2 * P_m2 / L_20;

                float Lin_mat_1_1 = (-F_d2 - F_d1) / mass;
                /********************************************* Lin_mat_2_1 ********************************************/
                V_m1     = (a1 * k1 * k1 * k1) + (b1 * k1 * k1) + (c1 * k1) + d1;
                ddotV_m1 = (3 * a1 * k1 * k1 + 2 * b1 * k1 + c1) / L_10;

                float Lin_mat_2_1 = -P_m1 * ddotV_m1 / V_m1;
                /********************************************* Lin_mat_3_1 ********************************************/
                V_m2     = (a2 * k2 * k2 * k2) + (b2 * k2 * k2) + (c2 * k2) + d2;
                ddotV_m2 = (3 * a2 * k2 * k2 + 2 * b2 * k2 + c2) / L_20;

                float Lin_mat_3_1 = -P_m2 * ddotV_m2 / V_m2;
                /******************************************************************************************************/
                /********************************************** Column 3 **********************************************/
                /******************************************************************************************************/

                /********************************************* Lin_mat_0_2 ********************************************/
                float Lin_mat_0_2 = 0;
                /********************************************* Lin_mat_1_2 ********************************************/
                F_s1 = k_ce1 * F_ce1 * dP_ce1;
                F_d1 = -R1 * dotk1;

                utility::io::debug.out("F_s1 = %f\nk_ce1 = %f\n F_ce1 = %f\n dP_ce1 = %f\n\n", F_s1, k_ce1, F_ce1, dP_ce1);
                utility::io::debug.out("F_d1 = %f\n -R1 = %f\n dotk1 = %f\n", F_d1, -R1, dotk1);

                // TODO This is incorrect, but i don't know any better
                float Lin_mat_1_2 = (F_s1 + F_d1) / mass;
                /********************************************* Lin_mat_2_2 ********************************************/
                dotV_m1 = (3 * a1 * k1 * k1 * dotk1 + 2 * b1 * k1 * dotk1 + c1 * dotk1) ;
                V_m1    = (a1 * k1 * k1 * k1) + (b1 * k1 * k1) + (c1 * k1) + d1;

                float Lin_mat_2_2 = (P_a * ddotV_a1 - dotV_m1) / V_m1 + 0.34;

                /********************************************* Lin_mat_3_2 ********************************************/
                float Lin_mat_3_2 = 0;
                /******************************************************************************************************/
                /********************************************** Column 4 **********************************************/
                /******************************************************************************************************/

                /********************************************* Lin_mat_0_3 ********************************************/
                float Lin_mat_0_3 = 0;
                /********************************************* Lin_mat_1_3 ********************************************/
                F_s2 = k_ce2 * F_ce2 * dP_ce2;
                F_d2 = -R2 * dotk2;

                utility::io::debug.out("F_s2 = %f\nk_ce2 = %f\n F_ce2 = %f\n dP_ce2 = %f\n\n", F_s2, k_ce2, F_ce2, dP_ce2);
                utility::io::debug.out("F_d2 = %f\n -R2 = %f\n dotk2 = %f\n", F_d2, -R2, dotk2);

                float Lin_mat_1_3 = (-F_s2 - F_d2) / mass;
                /********************************************* Lin_mat_2_3 ********************************************/
                float Lin_mat_2_3 = 0;
                /********************************************* Lin_mat_3_3 ********************************************/
                dotV_m2 = (3 * a2 * k2 * k2 * dotk2 + 2 * b2 * k2 * dotk2 + c2 * dotk2) ;
                V_m2    = (a2 * k2 * k2 * k2) + (b2 * k2 * k2) + (c2 * k2) + d2;

                float Lin_mat_3_3 = (P_a * ddotV_a2 - dotV_m2) / V_m2 + 0.34;

                A_mat << Lin_mat_0_0, Lin_mat_0_1, Lin_mat_0_2, Lin_mat_0_3,
                         Lin_mat_1_0, Lin_mat_1_1, Lin_mat_1_2, Lin_mat_1_3,
                         Lin_mat_2_0, Lin_mat_2_1, Lin_mat_2_2, Lin_mat_2_3,
                         Lin_mat_3_0, Lin_mat_3_1, Lin_mat_3_2, Lin_mat_3_3;
                /* clang-format on */
            }

            template <typename T>
            std::pair<bool, bool> FirstLayer(const T& m, std::vector<float>& states, std::vector<float>& setpoint) {
                // Increment the depth (control horizon itt)
                // for (std::vector<float>::const_iterator i = states.begin(); i != states.end(); ++i) {
                //     utility::io::debug.out("%f ", *i);
                // }
                // utility::io::debug.out("\n");

                ch_itt = 1;

                // Remove our previous results
                cost_result.clear();

                // Create a cost for each root
                float cost_root_1;
                float cost_root_2;
                float cost_root_3;

                // Create a output state vector for each process
                std::vector<float> output_states_1;
                std::vector<float> output_states_2;
                std::vector<float> output_states_3;

                // Calculate the result of performing each action and add the result error to the cost vector
                cost_root_1 = ProcessModel(m, 1, states, setpoint, states, output_states_1);
                cost_root_2 = ProcessModel(m, 2, states, setpoint, states, output_states_2);
                cost_root_3 = ProcessModel(m, 3, states, setpoint, states, output_states_3);
                // utility::io::debug.out("output_states_1\n");
                // for (std::vector<float>::const_iterator i = output_states_1.begin(); i != output_states_1.end(); ++i)
                // {
                //     utility::io::debug.out("%f ", *i);
                // }
                // utility::io::debug.out("\n");
                // utility::io::debug.out("output_states_2\n");
                // for (std::vector<float>::const_iterator i = output_states_2.begin(); i != output_states_2.end(); ++i)
                // {
                //     utility::io::debug.out("%f ", *i);
                // }
                // utility::io::debug.out("\n");
                // utility::io::debug.out("output_states_3\n");
                // for (std::vector<float>::const_iterator i = output_states_3.begin(); i != output_states_3.end(); ++i)
                // {
                //     utility::io::debug.out("%f ", *i);
                // }
                // utility::io::debug.out("\n");

                // Decide if the next layer is the last or not
                if (ch_itt >= ch_max - 1) {
                    // Must be on our last layer
                    FinalLayer(m, states, setpoint, output_states_1, 1, cost_root_1);
                    FinalLayer(m, states, setpoint, output_states_2, 2, cost_root_2);
                    FinalLayer(m, states, setpoint, output_states_3, 3, cost_root_3);
                }
                else {
                    // Not our last layer lets add a general layer
                    AddLayer(m, states, setpoint, output_states_1, 1, cost_root_1);
                    AddLayer(m, states, setpoint, output_states_2, 2, cost_root_2);
                    AddLayer(m, states, setpoint, output_states_3, 3, cost_root_3);
                }

                // Now look through the cost_result vector and pick the lowest cost to perform the root action
                auto result = *std::min_element(cost_result.cbegin(),
                                                cost_result.cend(),
                                                [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; });
                utility::io::debug.out("Cost results\n");
                for (const auto& p : cost_result) {
                    utility::io::debug.out("Mode %d, cost %f\n", p.second, p.first);
                }
                utility::io::debug.out("\n");

                if (result.second == 1) {
                    utility::io::debug.out("Optimizer result mode1, %f\n", result.first);
                    return (std::make_pair(true, false));
                }
                else if (result.second == 2) {
                    utility::io::debug.out("Optimizer result mode2, %f\n", result.first);
                    return (std::make_pair(false, true));
                }
                else if (result.second == 3) {
                    utility::io::debug.out("Optimizer result mode3, %f\n", result.first);
                    return (std::make_pair(false, false));
                }
                utility::io::debug.out("Optimizer failed\n");
                return (std::make_pair(false, false));
            }

            template <typename T>
            void AddLayer(const T& m,
                          std::vector<float>& states,
                          std::vector<float>& setpoint,
                          std::vector<float>& input_states,
                          int root,
                          float cost) {
                // We're somewhere in the middle of our recursion
                ch_itt += 1;

                // Create a output state vector for each process
                std::vector<float> output_states_1;
                std::vector<float> output_states_2;
                std::vector<float> output_states_3;

                // Calculate the result of performing each action and add the result error to the cost vector
                float cost1 = cost + ProcessModel(m, 1, states, setpoint, input_states, output_states_1);
                float cost2 = cost + ProcessModel(m, 2, states, setpoint, input_states, output_states_2);
                float cost3 = cost + ProcessModel(m, 3, states, setpoint, input_states, output_states_3);

                // Decide if the next layer is the last or not
                if (ch_itt >= ch_max - 1) {
                    // Must be on our last layer
                    FinalLayer(m, states, setpoint, output_states_1, root, cost1);
                    FinalLayer(m, states, setpoint, output_states_2, root, cost2);
                    FinalLayer(m, states, setpoint, output_states_3, root, cost3);
                }
                else {
                    // Not our last layer lets add a general layer
                    AddLayer(m, states, setpoint, output_states_1, root, cost1);
                    AddLayer(m, states, setpoint, output_states_2, root, cost2);
                    AddLayer(m, states, setpoint, output_states_3, root, cost3);
                }

                // As we're leaving a level, decrement the itterator
                ch_itt -= 1;
            }

            template <typename T>
            void FinalLayer(const T& m,
                            std::vector<float>& states,
                            std::vector<float>& setpoint,
                            std::vector<float>& input_states,
                            const int& root,
                            float cost) {
                // We're on or last layer, let's calculate the result append the cost and root

                // utility::io::debug.out("input_states\n");
                // for (std::vector<float>::const_iterator i = input_states.begin(); i != input_states.end(); ++i) {
                //     utility::io::debug.out("%f ", *i);
                // }
                // utility::io::debug.out("\n");

                // TODO This isn't needed on the last layer
                // Create a output state vector for each process
                std::vector<float> output_states_1;
                std::vector<float> output_states_2;
                std::vector<float> output_states_3;

                // Calculate the result of performing each action and add the result error to the cost vector
                float cost1 = cost + ProcessModel(m, 1, states, setpoint, input_states, output_states_1);
                float cost2 = cost + ProcessModel(m, 2, states, setpoint, input_states, output_states_2);
                float cost3 = cost + ProcessModel(m, 3, states, setpoint, input_states, output_states_3);

                // utility::io::debug.out("Final Mode %d, Cost1 %f\n", root, cost1 - cost);
                // utility::io::debug.out("Final Mode %d, Cost2 %f\n", root, cost2 - cost);
                // utility::io::debug.out("Final Mode %d, Cost3 %f\n", root, cost3 - cost);

                // Tally the cost and append it to the cost function list for each final cost
                cost_result.push_back(std::pair<float, int>(cost1, root));
                cost_result.push_back(std::pair<float, int>(cost2, root));
                cost_result.push_back(std::pair<float, int>(cost3, root));
            }

        private:
            int ch_itt;
            const int ch_max;
            std::vector<std::pair<float, int>> cost_result;
            Eigen::Matrix<float, 4, 4> state_weight;
        };

        extern Optimizer optimizer1;
    }  // namespace AdaptiveMPC
}  // namespace MPC
}  // namespace module

#endif  // MODULE_OPTIMIZER_HPP


// BLAS level 2

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include "utils.h"

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

using std::cout;
using std::endl;

typedef ublas::vector<double> vct_t;
typedef ublas::matrix<double, ublas::row_major> rm_t;
typedef ublas::matrix<double, ublas::column_major> cm_t;

int main() {

  cout << endl;

  vct_t vx (2);
  vct_t vy (4);

  // row major matrix
  rm_t rm (4, 2);
  init_m (rm, kpp (1));
  print_m (rm, "row major matrix m");
  cout << endl;

  blas::set (1., vx);
  print_v (vx, "vx");
  cout << endl;

  // vy = m vx
  blas::gemv ( 1.0, rm, vx, 0.0, vy);
  print_v (vy, "vy = m vx");
  cout << endl;

  blas::set (1., vy);
  print_v (vy, "vy");
  cout << endl;

  // vx = m^T vy
  blas::gemv ( 1.0, bindings::trans(rm), vy, 0.0, vx);
  print_v (vx, "vx = m^T vy");
  cout << endl;

  cout << endl;

  // column major matrix
  cm_t cm (4, 2);
  init_m (cm, kpp (1));
  print_m (cm, "column major matrix m");
  cout << endl;

  blas::set (1., vx);
  print_v (vx, "vx");
  cout << endl;

  // vy = m vx
  blas::gemv (1.0, cm, vx, 0.0, vy);
  print_v (vy, "vy = m vx");
  cout << endl;

  blas::set (1., vy);
  print_v (vy, "vy");
  cout << endl;

  // vx = m^T vy
  blas::gemv ( 1.0, bindings::trans(cm), vy, 0.0, vx);
  print_v (vx, "vx = m^T vy");
  cout << endl;

}


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

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

 * See LICENSE for the license information

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

/**
 * @file   testPose2.cpp
 * @brief  Unit tests for Pose2 class
 */

#include <cmath>
#include <iostream>

#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>

using namespace gtsam;
using namespace std;

// #define SLOW_BUT_CORRECT_EXPMAP

GTSAM_CONCEPT_TESTABLE_INST(Pose2)
GTSAM_CONCEPT_LIE_INST(Pose2)

/* ************************************************************************* */
TEST(Pose2, constructors) {
  Point2 p;
  Pose2 pose(0,p);
  Pose2 origin;
  assert_equal(pose,origin);
	Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
	EXPECT(assert_equal(t,Pose2(t.matrix())));
}

/* ************************************************************************* */
TEST(Pose2, manifold) {
	Pose2 t1(M_PI/2.0, Point2(1, 2));
	Pose2 t2(M_PI/2.0+0.018, Point2(1.015, 2.01));
	Pose2 origin;
	Vector d12 = t1.localCoordinates(t2);
	EXPECT(assert_equal(t2, t1.retract(d12)));
	EXPECT(assert_equal(t2, t1*origin.retract(d12)));
	Vector d21 = t2.localCoordinates(t1);
	EXPECT(assert_equal(t1, t2.retract(d21)));
	EXPECT(assert_equal(t1, t2*origin.retract(d21)));
}

/* ************************************************************************* */
TEST(Pose2, retract) {
  Pose2 pose(M_PI/2.0, Point2(1, 2));
#ifdef SLOW_BUT_CORRECT_EXPMAP
  Pose2 expected(1.00811, 2.01528, 2.5608);
#else
  Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
#endif
  Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99));
  EXPECT(assert_equal(expected, actual, 1e-5));
}

/* ************************************************************************* */
TEST(Pose2, expmap) {
  Pose2 pose(M_PI/2.0, Point2(1, 2));
  Pose2 expected(1.00811, 2.01528, 2.5608);
  Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
  EXPECT(assert_equal(expected, actual, 1e-5));
}

/* ************************************************************************* */
TEST(Pose2, expmap2) {
  Pose2 pose(M_PI/2.0, Point2(1, 2));
  Pose2 expected(1.00811, 2.01528, 2.5608);
  Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
  EXPECT(assert_equal(expected, actual, 1e-5));
}

/* ************************************************************************* */
TEST(Pose2, expmap3) {
  // do an actual series exponential map
	// see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps
  Matrix A = Matrix_(3,3,
  		0.0, -0.99,  0.01,
  		0.99,  0.0, -0.015,
  		0.0,   0.0,  0.0);
  Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
  Matrix expected = eye(3) + A + A2 + A3 + A4;

  Vector v = Vector_(3, 0.01, -0.015, 0.99);
  Pose2 pose = Pose2::Expmap(v);
  Pose2 pose2(v);
  EXPECT(assert_equal(pose, pose2));
  Matrix actual = pose.matrix();
  //EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(Pose2, expmap0) {
  Pose2 pose(M_PI/2.0, Point2(1, 2));
//#ifdef SLOW_BUT_CORRECT_EXPMAP
  Pose2 expected(1.01491, 2.01013, 1.5888);
//#else
//  Pose2 expected(M_PI/2.0+0.018, Point2(1.015, 2.01));
//#endif
  Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
  EXPECT(assert_equal(expected, actual, 1e-5));
}

/* ************************************************************************* */
TEST(Pose2, expmap0_full) {
  Pose2 pose(M_PI/2.0, Point2(1, 2));
  Pose2 expected(1.01491, 2.01013, 1.5888);
  Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
  EXPECT(assert_equal(expected, actual, 1e-5));
}

/* ************************************************************************* */
TEST(Pose2, expmap0_full2) {
  Pose2 pose(M_PI/2.0, Point2(1, 2));
  Pose2 expected(1.01491, 2.01013, 1.5888);
  Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
  EXPECT(assert_equal(expected, actual, 1e-5));
}

#ifdef SLOW_BUT_CORRECT_EXPMAP
/* ************************************************************************* */
// test case for screw motion in the plane
namespace screw {
  double w=0.3;
	Vector xi = Vector_(3, 0.0, w, w);
	Rot2 expectedR = Rot2::fromAngle(w);
	Point2 expectedT(-0.0446635, 0.29552);
	Pose2 expected(expectedR, expectedT);
}

TEST(Pose3, expmap_c)
{
  EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
  EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
  EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
}
#endif

/* ************************************************************************* */
TEST(Pose2, expmap_c_full)
{
  double w=0.3;
	Vector xi = Vector_(3, 0.0, w, w);
	Rot2 expectedR = Rot2::fromAngle(w);
	Point2 expectedT(-0.0446635, 0.29552);
	Pose2 expected(expectedR, expectedT);
  EXPECT(assert_equal(expected, expm<Pose2>(xi),1e-6));
  EXPECT(assert_equal(expected, Pose2::Expmap(xi),1e-6));
  EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6));
}

/* ************************************************************************* */
TEST(Pose2, logmap) {
  Pose2 pose0(M_PI/2.0, Point2(1, 2));
  Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP
  Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
#else
  Vector expected = Vector_(3, 0.01, -0.015, 0.018);
#endif
  Vector actual = pose0.localCoordinates(pose);
  EXPECT(assert_equal(expected, actual, 1e-5));
}

/* ************************************************************************* */
TEST(Pose2, logmap_full) {
  Pose2 pose0(M_PI/2.0, Point2(1, 2));
  Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
  Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
  Vector actual = logmap_default<Pose2>(pose0, pose);
  EXPECT(assert_equal(expected, actual, 1e-5));
}

/* ************************************************************************* */
static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
	return pose.transform_to(point);
}

TEST( Pose2, transform_to )
{
  Pose2 pose(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
  Point2 point(-1,4);    // landmark at (-1,4)

  // expected
  Point2 expected(2,2);
  Matrix expectedH1 = Matrix_(2,3, -1.0, 0.0, 2.0,  0.0, -1.0, -2.0);
  Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0,  -1.0, 0.0);

  // actual
  Matrix actualH1, actualH2;
  Point2 actual = pose.transform_to(point, actualH1, actualH2);
  EXPECT(assert_equal(expected,actual));

  EXPECT(assert_equal(expectedH1,actualH1));
  Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point);
  EXPECT(assert_equal(numericalH1,actualH1));

  EXPECT(assert_equal(expectedH2,actualH2));
  Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point);
  EXPECT(assert_equal(numericalH2,actualH2));
}

/* ************************************************************************* */
static Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
	return pose.transform_from(point);
}

TEST (Pose2, transform_from)
{
	Pose2 pose(1., 0., M_PI/2.0);
	Point2 pt(2., 1.);
	Matrix H1, H2;
	Point2 actual = pose.transform_from(pt, H1, H2);

	Point2 expected(0., 2.);
	EXPECT(assert_equal(expected, actual));

	Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
	Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);

	Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt);
	EXPECT(assert_equal(H1_expected, H1));
	EXPECT(assert_equal(H1_expected, numericalH1));

	Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt);
	EXPECT(assert_equal(H2_expected, H2));
	EXPECT(assert_equal(H2_expected, numericalH2));
}

/* ************************************************************************* */
TEST(Pose2, compose_a)
{
  Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
  Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));

  Matrix actualDcompose1;
  Matrix actualDcompose2;
  Pose2 actual = pose1.compose(pose2, actualDcompose1, actualDcompose2);

  Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
  EXPECT(assert_equal(expected, actual));

  Matrix expectedH1 = Matrix_(3,3,
  		0.0, 1.0, 0.0,
  	   -1.0, 0.0, 2.0,
  		0.0, 0.0, 1.0
  );
  Matrix expectedH2 = eye(3);
  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
  EXPECT(assert_equal(expectedH1,actualDcompose1));
  EXPECT(assert_equal(numericalH1,actualDcompose1));
  EXPECT(assert_equal(expectedH2,actualDcompose2));
  EXPECT(assert_equal(numericalH2,actualDcompose2));

  Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
  Point2 expected_point(-1.0, -1.0);
  Point2 actual_point1 = (pose1 * pose2).transform_to(point);
  Point2 actual_point2 = pose2.transform_to(pose1.transform_to(point));
  EXPECT(assert_equal(expected_point, actual_point1));
  EXPECT(assert_equal(expected_point, actual_point2));
}

/* ************************************************************************* */
TEST(Pose2, compose_b)
{
  Pose2 pose1(Rot2::fromAngle(M_PI/10.0), Point2(.75, .5));
  Pose2 pose2(Rot2::fromAngle(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));

  Pose2 pose_expected(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 2.0));

  Pose2 pose_actual_op = pose1 * pose2;
  Matrix actualDcompose1, actualDcompose2;
  Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);

  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
  EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
  EXPECT(assert_equal(numericalH2,actualDcompose2));

  EXPECT(assert_equal(pose_expected, pose_actual_op));
  EXPECT(assert_equal(pose_expected, pose_actual_fcn));
}

/* ************************************************************************* */
TEST(Pose2, compose_c)
{
  Pose2 pose1(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 1.0));
  Pose2 pose2(Rot2::fromAngle(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));

  Pose2 pose_expected(Rot2::fromAngle(M_PI/2.0), Point2(1.0, 2.0));

  Pose2 pose_actual_op = pose1 * pose2;
  Matrix actualDcompose1, actualDcompose2;
  Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);

  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
  EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
  EXPECT(assert_equal(numericalH2,actualDcompose2));

  EXPECT(assert_equal(pose_expected, pose_actual_op));
  EXPECT(assert_equal(pose_expected, pose_actual_fcn));
}

/* ************************************************************************* */
TEST(Pose2, inverse )
{
	Point2 origin, t(1,2);
	Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y

	Pose2 identity, lTg = gTl.inverse();
	EXPECT(assert_equal(identity,lTg.compose(gTl)));
	EXPECT(assert_equal(identity,gTl.compose(lTg)));

	Point2 l(4,5), g(-4,6);
	EXPECT(assert_equal(g,gTl*l));
	EXPECT(assert_equal(l,lTg*g));

	// Check derivative
  Matrix numericalH = numericalDerivative11<Pose2,Pose2>(testing::inverse, lTg);
  Matrix actualDinverse;
  lTg.inverse(actualDinverse);
  EXPECT(assert_equal(numericalH,actualDinverse));
}

/* ************************************************************************* */
Vector homogeneous(const Point2& p) {
	return Vector_(3, p.x(), p.y(), 1.0);
}

/* ************************************************************************* */
Matrix matrix(const Pose2& gTl) {
	Matrix gRl = gTl.r().matrix();
	Point2 gt = gTl.t();
	return Matrix_(3, 3,
			gRl(0, 0), gRl(0, 1), gt.x(),
			gRl(1, 0), gRl(1, 1), gt.y(),
			      0.0,       0.0,   1.0);
}

/* ************************************************************************* */
TEST( Pose2, matrix )
{
	Point2 origin, t(1,2);
	Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
  Matrix gMl = matrix(gTl);
  EXPECT(assert_equal(Matrix_(3,3,
  		0.0, -1.0, 1.0,
  		1.0,  0.0, 2.0,
  		0.0,  0.0, 1.0),
  		gMl));
  Rot2 gR1 = gTl.r();
  EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
  Point2 x_axis(1,0), y_axis(0,1);
  EXPECT(assert_equal(Matrix_(2,2,
  		0.0, -1.0,
  		1.0,  0.0),
  		gR1.matrix()));
  EXPECT(assert_equal(Point2(0,1),gR1*x_axis));
  EXPECT(assert_equal(Point2(-1,0),gR1*y_axis));
  EXPECT(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis)));
  EXPECT(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis)));

  // check inverse pose
  Matrix lMg = matrix(gTl.inverse());
  EXPECT(assert_equal(Matrix_(3,3,
  		0.0,  1.0,-2.0,
  	 -1.0,  0.0, 1.0,
  		0.0,  0.0, 1.0),
  		lMg));
}

/* ************************************************************************* */
TEST( Pose2, compose_matrix )
{
  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
  Pose2 _1T2(M_PI, Point2(-1,4));  // local robot at (-1,4) loooking at negative x
  Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
  EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
}

/* ************************************************************************* */
TEST( Pose2, between )
{
  // <
  //
	//       ^
	//
	// *--0--*--*
  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
  Pose2 gT2(M_PI, Point2(-1,4));  // robot at (-1,4) loooking at negative x

  Matrix actualH1,actualH2;
  Pose2 expected(M_PI/2.0, Point2(2,2));
  Pose2 actual1 = gT1.between(gT2);
  Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
  EXPECT(assert_equal(expected,actual1));
  EXPECT(assert_equal(expected,actual2));

  Matrix expectedH1 = Matrix_(3,3,
      0.0,-1.0,-2.0,
      1.0, 0.0,-2.0,
      0.0, 0.0,-1.0
  );
  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
  EXPECT(assert_equal(expectedH1,actualH1));
  EXPECT(assert_equal(numericalH1,actualH1));
	// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
  EXPECT(assert_equal(-gT2.between(gT1).adjointMap(),actualH1));

  Matrix expectedH2 = Matrix_(3,3,
       1.0, 0.0, 0.0,
       0.0, 1.0, 0.0,
       0.0, 0.0, 1.0
  );
  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
  EXPECT(assert_equal(expectedH2,actualH2));
  EXPECT(assert_equal(numericalH2,actualH2));

}

/* ************************************************************************* */
// reverse situation for extra test
TEST( Pose2, between2 )
{
  Pose2 p2(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
  Pose2 p1(M_PI, Point2(-1,4));  // robot at (-1,4) loooking at negative x

  Matrix actualH1,actualH2;
  p1.between(p2,actualH1,actualH2);
  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
  EXPECT(assert_equal(numericalH1,actualH1));
  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
  EXPECT(assert_equal(numericalH2,actualH2));
}

/* ************************************************************************* */
TEST( Pose2, round_trip )
{
	Pose2 p1(1.23, 2.30, 0.2);
	Pose2 odo(0.53, 0.39, 0.15);
	Pose2 p2 = p1.compose(odo);
	EXPECT(assert_equal(odo, p1.between(p2)));
}

/* ************************************************************************* */
TEST(Pose2, members)
{
  Pose2 pose;
  EXPECT(pose.dim() == 3);
}

/* ************************************************************************* */
// some shared test values
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);

/* ************************************************************************* */
Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) {
	return pose.bearing(pt);
}

TEST( Pose2, bearing )
{
	Matrix expectedH1, actualH1, expectedH2, actualH2;

	// establish bearing is indeed zero
	EXPECT(assert_equal(Rot2(),x1.bearing(l1)));

	// establish bearing is indeed 45 degrees
	EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(l2)));

	// establish bearing is indeed 45 degrees even if shifted
	Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
	EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
	EXPECT(assert_equal(expectedH1,actualH1));
	expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
	EXPECT(assert_equal(expectedH1,actualH1));

	// establish bearing is indeed 45 degrees even if rotated
	Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
	EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
	expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
	EXPECT(assert_equal(expectedH1,actualH1));
	EXPECT(assert_equal(expectedH1,actualH1));
}

/* ************************************************************************* */
Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
	return pose.bearing(pt);
}

TEST( Pose2, bearing_pose )
{
	Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);

	Matrix expectedH1, actualH1, expectedH2, actualH2;

	// establish bearing is indeed zero
	EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));

	// establish bearing is indeed 45 degrees
	EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(xl2)));

	// establish bearing is indeed 45 degrees even if shifted
	Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2);
	EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
	expectedH2 = numericalDerivative22(bearing_pose_proxy, x2, xl3);
	EXPECT(assert_equal(expectedH1,actualH1));
	EXPECT(assert_equal(expectedH2,actualH2));

	// establish bearing is indeed 45 degrees even if rotated
	Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2);
	EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
	expectedH2 = numericalDerivative22(bearing_pose_proxy, x3, xl4);
	EXPECT(assert_equal(expectedH1,actualH1));
	EXPECT(assert_equal(expectedH2,actualH2));
}

/* ************************************************************************* */
LieVector range_proxy(const Pose2& pose, const Point2& point) {
	return LieVector(pose.range(point));
}
TEST( Pose2, range )
{
	Matrix expectedH1, actualH1, expectedH2, actualH2;

	// establish range is indeed zero
	EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);

	// establish range is indeed 45 degrees
	EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);

	// Another pair
	double actual23 = x2.range(l3, actualH1, actualH2);
	EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(range_proxy, x2, l3);
	expectedH2 = numericalDerivative22(range_proxy, x2, l3);
	EXPECT(assert_equal(expectedH1,actualH1));
	EXPECT(assert_equal(expectedH2,actualH2));

	// Another test
	double actual34 = x3.range(l4, actualH1, actualH2);
	EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(range_proxy, x3, l4);
	expectedH2 = numericalDerivative22(range_proxy, x3, l4);
	EXPECT(assert_equal(expectedH1,actualH1));
	EXPECT(assert_equal(expectedH2,actualH2));
}

/* ************************************************************************* */
LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
	return LieVector(pose.range(point));
}
TEST( Pose2, range_pose )
{
	Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);

	Matrix expectedH1, actualH1, expectedH2, actualH2;

	// establish range is indeed zero
	EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);

	// establish range is indeed 45 degrees
	EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);

	// Another pair
	double actual23 = x2.range(xl3, actualH1, actualH2);
	EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
	expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3);
	EXPECT(assert_equal(expectedH1,actualH1));
	EXPECT(assert_equal(expectedH2,actualH2));

	// Another test
	double actual34 = x3.range(xl4, actualH1, actualH2);
	EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);

	// Check numerical derivatives
	expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4);
	expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4);
	EXPECT(assert_equal(expectedH1,actualH1));
	EXPECT(assert_equal(expectedH2,actualH2));
}

/* ************************************************************************* */

TEST(Pose2, align_1) {
	Pose2 expected(Rot2::fromAngle(0), Point2(10,10));

	vector<Point2Pair> correspondences;
	Point2Pair pq1(make_pair(Point2(0,0), Point2(10,10)));
	Point2Pair pq2(make_pair(Point2(20,10), Point2(30,20)));
	correspondences += pq1, pq2;

  boost::optional<Pose2> actual = align(correspondences);
  EXPECT(assert_equal(expected, *actual));
}

TEST(Pose2, align_2) {
	Point2 t(20,10);
	Rot2 R = Rot2::fromAngle(M_PI/2.0);
	Pose2 expected(R, t);

	vector<Point2Pair> correspondences;
	Point2 p1(0,0), p2(10,0);
	Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2);
  EXPECT(assert_equal(Point2(20,10),q1));
  EXPECT(assert_equal(Point2(20,20),q2));
	Point2Pair pq1(make_pair(p1, q1));
	Point2Pair pq2(make_pair(p2, q2));
	correspondences += pq1, pq2;

  boost::optional<Pose2> actual = align(correspondences);
  EXPECT(assert_equal(expected, *actual));
}

namespace align_3 {
	Point2 t(10,10);
	Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
	Point2 p1(0,0), p2(10,0), p3(10,10);
	Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2), q3 = expected.transform_from(p3);
}

TEST(Pose2, align_3) {
	using namespace align_3;

	vector<Point2Pair> correspondences;
	Point2Pair pq1(make_pair(p1, q1));
	Point2Pair pq2(make_pair(p2, q2));
	Point2Pair pq3(make_pair(p3, q3));
	correspondences += pq1, pq2, pq3;

  boost::optional<Pose2> actual = align(correspondences);
  EXPECT(assert_equal(expected, *actual));
}

/* ************************************************************************* */
// Prototype code to align two triangles using a rigid transform
/* ************************************************************************* */
struct Triangle { size_t i_,j_,k_;};

boost::optional<Pose2> align(const vector<Point2>& ps, const vector<Point2>& qs,
		const pair<Triangle, Triangle>& trianglePair) {
	const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
	vector<Point2Pair> correspondences;
	correspondences += make_pair(ps[t1.i_],qs[t2.i_]), make_pair(ps[t1.j_],qs[t2.j_]), make_pair(ps[t1.k_],qs[t2.k_]);
	return align(correspondences);
}

TEST(Pose2, align_4) {
	using namespace align_3;

	vector<Point2> ps,qs;
	ps += p1, p2, p3;
	qs += q3, q1, q2; // note in 3,1,2 order !

	Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
	Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;

  boost::optional<Pose2> actual = align(ps, qs, make_pair(t1,t2));
  EXPECT(assert_equal(expected, *actual));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <stdint.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <helper_functions.h>
#include <nvidia/helper_cuda.h>

//#include <mmf/defines.h>
#include <mmf/sphereSimple.hpp>
#include <mmf/optimizationSO3.hpp>
//#include <mmf/timer.hpp>

using namespace Eigen;

namespace mmf{

class OptSO3vMF : public OptSO3
{
  public:
  OptSO3vMF(float sigma, float t_max = 5.0f, float dt = 0.05f, float
      *d_weights =NULL):
    OptSO3(sigma,t_max, dt, d_weights)
  {
//    t_max_ = 5.0f;
//    dt_ = 0.05f; // 0.1
  };

  virtual ~OptSO3vMF()
  { };

protected:

  virtual void conjugateGradientPostparation_impl(Matrix3f& R);
  virtual float conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N);
  /* evaluate cost function for a given assignment of npormals to axes */
  virtual float evalCostFunction(Matrix3f& R);
  /* compute Jacobian */
  virtual void computeJacobian(Matrix3f&J, Matrix3f& R, float N);

  virtual void init() {};
};

}


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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

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

#include <geometry_test_common.hpp>

#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/extensions/triangulation/triangulation.hpp>

namespace bg = boost::geometry;

template <typename P>
void test_all()
{
    typedef bg::model::triangulation<P, false> triangulation;
    std::vector<P> in;
    in.push_back(P(-1, -2));
    in.push_back(P(1, -2));
    in.push_back(P(-2, 0));
    in.push_back(P(2, 0));
    in.push_back(P(1, 2));
    in.push_back(P(-1, 2));
    in.push_back(P(0, 0));
    in.push_back(P(0, 1));
    triangulation t(8);
    bg::delaunay_triangulation(in, t);
    BOOST_CHECK( t.valid() );
}

int test_main(int, char* [])
{
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();
    return 0;
}


#include <ql/quantlib.hpp>
//#include <ql/userconfig.hpp>
#include <iostream>
#include <vector>
#include <boost/foreach.hpp>

using namespace std;
using namespace QuantLib;

void testingBlackScholesCalculator() {

    Real S0  = 100.0;
    Real K   = 105.0;
    Real rd  = 0.034;
    Real rf  = 0.021;
    Real tau = 0.5;
    Real vol = 0.177;

    Real domDisc = std::exp(-rd*tau);
    Real forDisc = std::exp(-rf*tau);
    Real stdDev = vol*std::sqrt(tau);

    boost::shared_ptr<PlainVanillaPayoff> vanillaPayoffPut(new PlainVanillaPayoff(Option::Put, K));
    boost::shared_ptr<AssetOrNothingPayoff> aonPayoffCall(new AssetOrNothingPayoff(Option::Call, K));

    BlackScholesCalculator vanillaPutPricer(vanillaPayoffPut, S0, forDisc, stdDev, domDisc);
    BlackScholesCalculator aonCallPricer(aonPayoffCall, S0, forDisc, stdDev, domDisc);

    cout << "------------- Vanilla VAlues -----------------" << endl;
    cout << "Value: " << vanillaPutPricer.value() << endl;

}

int main(void) {

    testingBlackScholesCalculator();
    return 0;
}


#ifndef CALIBRATOR_MODELS_SHORTRATE_TWOFACTORMODELS_GENERALG2_HPP
#define CALIBRATOR_MODELS_SHORTRATE_TWOFACTORMODELS_GENERALG2_HPP

#include <boost/make_shared.hpp>

#include <ql/models/shortrate/twofactormodel.hpp>
#include <ql/instruments/swaption.hpp>
#include <ql/math/solver1d.hpp>
#include <ql/math/integrals/kronrodintegral.hpp>

#include <calibrator/global.hpp>
#include <calibrator/processes/generalornsteinuhlenbeckprocess.hpp>
#include <calibrator/models/shortrate/dynamics/gaussianfactordynamics.hpp>

namespace HJCALIBRATOR
{
	//! Two-additive-factor gaussian model class
	/*! This class implements a two-additive-factor model defined by
	\f[
	dr_t = \varphi(t) + x_t + y_t
	\f]
	where \f$ x_t \f$ and \f$ y_t \f$ are defined by
	\f[
	dx_t = -a x_t dt + \sigma dW^1_t, x_0 = 0
	\f]
	\f[
	dy_t = -b y_t dt + \sigma dW^2_t, y_0 = 0
	\f]
	and \f$ dW^1_t dW^2_t = \rho dt \f$.

	\bug This class was not tested enough to guarantee
	its functionality.

	\todo Tree implementation

	\ingroup shortrate
	*/
	class GeneralizedG2 : public TwoFactorModel, public AffineModel, public TermStructureConsistentModel
	{
		class Dynamics;

		shared_ptr<Gaussian2FactorDynamics> dynamics_;

	public:
		GeneralizedG2( shared_ptr<Gaussian2FactorDynamics> dynamics,
					   Real integralSignificance = 10,
					   shared_ptr<Integrator> integrator = boost::make_shared<GaussKronrodAdaptive>( GaussKronrodAdaptive( 1.e-8, 10000 ) ) );
		virtual ~GeneralizedG2() {}

		// TwoFactorModel virtual override
		shared_ptr<Lattice> tree( const TimeGrid& grid ) const override
		{
			// todo
			return shared_ptr<Lattice>();
		};

		shared_ptr<ShortRateDynamics> dynamics() const;

		virtual DiscountFactor discount( Time t ) const override
		{
			return termStructure()->discount( t );
		}

		virtual Real discountBond( Time now,
								   Time maturity,
								   Array factors ) const;

		virtual Real discountBondOption( Option::Type type,
										 Real strike,
										 Time maturity,
										 Time bondMaturity ) const override;

		virtual Real discountBondOption( Option::Type type, Real strike,
										 Time maturity, Time bondStart,
										 Time bondMaturity ) const override;

		virtual Real swaption( const Swaption::arguments& arg, Real strike ) const;

		Parameter a() const { return a_; }
		Parameter b() const { return b_; }
		Parameter sigma() const { return sigma_; }
		Parameter eta() const { return eta_; }
		Parameter rho() const { return rho_; }

	protected:
		// CalibratedModel virtual override
		virtual void generateArguments() override;

		Real A( Time t, Time T ) const;

		Parameter& a_;
		Parameter& sigma_;
		Parameter& b_;
		Parameter& eta_;
		Parameter& rho_;

		Real integralSignificance_;

		shared_ptr<Integrator> integrator_;
	};

	//! Short-rate dynamics in the time-dependent Hull-White model
	/*! The short-rate follows an time-dependent Hull-White process */
	class GeneralizedG2::Dynamics : public TwoFactorModel::ShortRateDynamics {
	public:
		Dynamics( const shared_ptr<Gaussian2FactorDynamics> dynamics )
			: ShortRateDynamics( shared_ptr<StochasticProcess1D>( new GeneralizedOrnsteinUhlenbeckProcess( dynamics->a( 0 ), dynamics->sigma( 0 ) ) ),
								 shared_ptr<StochasticProcess1D>( new GeneralizedOrnsteinUhlenbeckProcess( dynamics->a( 1 ), dynamics->sigma( 1 ) ) ),
								 dynamics->rho( 0, 1 )(0.0) )
			, dynamics_( dynamics )
		{}

		virtual Real shortRate( Time t, Real x, Real y ) const {
			return x + y + dynamics_->phi( t );
		}

		shared_ptr<Gaussian2FactorDynamics> dynamics_;
	};

	// inline definitions
	inline shared_ptr<TwoFactorModel::ShortRateDynamics>	GeneralizedG2::dynamics() const
	{
		return shared_ptr<ShortRateDynamics>( new Dynamics( dynamics_ ) );
	}
}
#endif // !CALIBRATOR_MODELS_SHORTRATE_TWOFACTORMODELS_GENERALG2_HPP


//
// Created by Robert Schmoltzi on 19.05.21.
//

#include "SpiAlgo.h"
#include <boost/math/special_functions/factorials.hpp>

static GeneralizedRfAlgo::Scalar aSmallerB(const size_t a, const size_t b, const size_t n) {
	return GeneralizedRfAlgo::factorials.lg_unrooted_dbl_fact_fast(static_cast<long>(n)) -
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(b)) -
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(n - a)) +
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(b - a + 1));
}

// Input: |A1|, |A2|, |X|
// Called if splits overlap
// Output:
// if(S1 == S2):
//      - lg_root(|A1|) - lg_root(|B1|) + lg_unroot(|X|)      [ <=> h(S1)]
// else: // Let S1 be the greater split
//      - lg_root(|A1|) - lg_root(|B2|) + lg_root(|A1| - |A2| + 1) + lg_unroot(|X|)  [ <=> h(S1,S2)]
GeneralizedRfAlgo::Scalar SpiAlgo::one_overlap(const size_t a, const size_t b, const size_t n) {
	if (a == b) {
		return GeneralizedRfAlgo::factorials.lg_unrooted_dbl_fact_fast(static_cast<long>(n)) -
		       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(a)) -
		       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(n - a));
	}
	if (a < b) {
		return aSmallerB(a, b, n);
	}

	return GeneralizedRfAlgo::factorials.lg_unrooted_dbl_fact_fast(static_cast<long>(n)) -
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(a)) -
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(n - b)) +
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(a - b + 1));
}

// Input: |A1|, |B2|, |X|
// Called if splits overlap
// Output:
// if(S1 == S2):
//      - lg_root(|A2|) - lg_root(|B2|) + lg_unroot(|X|)      [ <=> h(S1)]
// else: // Let S1 be the greater split
//      - lg_root(|A1|) - lg_root(|B2|) + lg_root(|A1| - |A2| + 1) + lg_unroot(|X|)  [ <=> h(S1,S2)]
GeneralizedRfAlgo::Scalar
SpiAlgo::one_overlap_notb(const size_t a, const size_t n_minus_b, const size_t n) {
	const size_t b = n - n_minus_b;
	if (a == b) {
		return GeneralizedRfAlgo::factorials.lg_unrooted_dbl_fact_fast(static_cast<long>(n)) -
		       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(b)) -
		       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(n_minus_b));
	}
	if (a < b) {
		return aSmallerB(a, b, n);
	}
	return GeneralizedRfAlgo::factorials.lg_unrooted_dbl_fact_fast(static_cast<long>(n)) -
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(a)) -
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(n_minus_b)) +
	       GeneralizedRfAlgo::factorials.lg_rooted_dbl_fact_fast(static_cast<long>(a - b + 1));
}

// Based on the implementation by Martin R. Smith, found at
// https://github.com/ms609/TreeDist/blob/e05ab4c9e69c9548f066b2a7b256e35f7f92067d/src/tree_distance_functions.cpp
GeneralizedRfAlgo::Scalar SpiAlgo::calc_split_score(const PllSplit &S1, const PllSplit &S2) {
	{
		bool found = false;
		for (size_t i = 0; i != PllSplit::split_len; ++i) {
			// TODO: Time direct access vs vectorized Split-Methods (less iterations vs doing less
			// in each iteration)
			if ((~S1()[i] & S2()[i])) {
				found = true;
				break;
			}
		}
		if (!found)
			return one_overlap(S1.getPrecalcPopcnt(), S2.getPrecalcPopcnt(), taxa);
	}
	{
		bool found = false;

		for (size_t i = 0; i != PllSplit::split_len; ++i) {
			if ((S1()[i] & ~S2()[i])) {
				found = true;
				break;
			}
		}
		if (!found)
			return one_overlap(S1.getPrecalcPopcnt(), S2.getPrecalcPopcnt(), taxa);
	}
	const size_t bits_too_many = GeneralizedRfAlgo::bits_too_many(taxa);
	for (size_t i = 0; i != PllSplit::split_len; ++i) {
		pll_split_base_t test = ~(S1()[i] | S2()[i]);
		if (i == PllSplit::split_len - 1 && bits_too_many) {
			test &= (static_cast<pll_split_base_t>(~0) >> bits_too_many);
		}
		if (test) {
			return 0;
		}
	}
	return one_overlap_notb(S1.getPrecalcPopcnt(), S2.getPrecalcPopcnt(), taxa);
}
SpiAlgo::SpiAlgo(size_t split_len) : GeneralizedRfAlgo(split_len) {
}

RfAlgorithmInterface::Scalar SpiAlgo::calc_split_score(const PllSplit &S1) {
	return S1.getHInfoContent();
}

// AB | DEFC
// DEF | ABC --> ABC | DEF

// ABC | DE
// ADE | BC


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ASECPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ASECPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing asecpi capabilities

     inverse secant in \$f\pi\$f multiples: \f$(1/\pi) \arccos(1/x)\f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = asecpi(x);
    @endcode

    @see asec, asecd, cospi, acospi

  **/
  Value asecpi(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/asecpi.hpp>
#include <boost/simd/function/simd/asecpi.hpp>

#endif


#include<iostream>
#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"tpoperators.hpp"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include<iomanip>
#include <boost/program_options.hpp>

using namespace boost::program_options;
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
   // std::vector<size_t> ee(L, 0);
 using Mat= Operators::Mat;
  size_t M{};
  size_t L{};
  double t0{};
  double omega{};
  double gamma{};
  double T{};
  bool PB{};
  try
  {
    options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("M", value(&M)->default_value(2), "M")
      ("t", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(1.), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega")
      ("T", value(&T)->default_value(1.), "T")
    ("pb", value(&PB)->default_value(true), "PB");

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

    if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<size_t>() << '\n';

      }
     if (vm.count("M,m"))
      {
	std::cout << "M: " << vm["M"].as<size_t>() << '\n';

      }
      if (vm.count("t"))
      {
	std::cout << "t0: " << vm["t"].as<double>() << '\n';
      }
       if (vm.count("omg"))
      {
	std::cout << "omega: " << vm["omg"].as<double>() << '\n';
      }
       if (vm.count("gam"))
      {
	std::cout << "gamma: " << vm["gam"].as<double>() << '\n';
      }
         if (vm.count("T"))
      {
	std::cout << "T: " << vm["T"].as<double>() << '\n';
      }
              if (vm.count("pb"))
      {
	std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
      }
    }
  }
  catch (const error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }
   ElectronBasis e( L, 1);
   //   std::cout<< e<<std::endl;
      ElectronState e2( L, 0);
   //   std::cout<< e<<std::endl;

  PhononBasis ph(L, M);
  //  std::cout<< ph<<std::endl;
  HolsteinBasis TP(e, ph);
  //HolsteinBasis TP2(e2, ph);
  //  std::cout<< TP<<std::endl;
  e.insert(e2);
  std::cout<< e<< std::endl;
  //  std::cout<< TP2<< std::endl;

  std::cout<<"total dim "<< TP.dim << std::endl;
  std::cout<<std::endl;
        Mat E1=Operators::EKinOperatorL(TP, e, t0, PB);
      Mat Ebdag=Operators::NBosonCOperator(TP, ph, gamma, PB);
      Mat Eb=Operators::NBosonDOperator(TP, ph, gamma, PB);
      Mat Eph=Operators::NumberOperator(TP, ph, omega,  PB);

      Mat H=E1  +Ebdag + Eb+ Eph;
          Eigen::MatrixXcd HH=Eigen::MatrixXcd(H);

	     Eigen::VectorXd ev=Eigen::VectorXd(TP.dim);
	     diagMat(HH, ev);
	     return 0;
}


#include <Eigen/LU>
#include <elasty/fem.hpp>
#include <gtest/gtest.h>

namespace
{
    constexpr double k_youngs_modulus = 800.0;
    constexpr double k_poisson_ratio  = 0.40;

    constexpr double k_first_lame  = elasty::fem::calcFirstLame(k_youngs_modulus, k_poisson_ratio);
    constexpr double k_second_lame = elasty::fem::calcSecondLame(k_youngs_modulus, k_poisson_ratio);
} // namespace

TEST(FemTest, StVenantKirchhoff2d)
{
    auto calcPiolaStress = [](const Eigen::Matrix2d& F) -> Eigen::Matrix2d
    {
        return elasty::fem::calcStVenantKirchhoffPiolaStress(F, k_first_lame, k_second_lame);
    };
    auto calcEnergyDensity = [](const Eigen::Matrix2d& F) -> double
    {
        return elasty::fem::calcStVenantKirchhoffEnergyDensity(F, k_first_lame, k_second_lame);
    };

    Eigen::MatrixXd x_rest{2, 3};
    x_rest.col(0) = Eigen::Vector2d{0.0, 0.0};
    x_rest.col(1) = Eigen::Vector2d{1.0, 0.0};
    x_rest.col(2) = Eigen::Vector2d{0.0, 1.0};

    Eigen::MatrixXd x{2, 3};
    x.col(0) = Eigen::Vector2d{0.0, 0.5};
    x.col(1) = Eigen::Vector2d{2.0, 0.0};
    x.col(2) = Eigen::Vector2d{0.0, 1.5};

    // Calculate $\frac{\partial \Psi}{\partial \mathbf{x}}$ analytically
    const auto D_m      = elasty::fem::calc2dShapeMatrix(x_rest.col(0), x_rest.col(1), x_rest.col(2));
    const auto D_m_inv  = D_m.inverse();
    const auto F        = elasty::fem::calc2dTriangleDeformGrad(x.col(0), x.col(1), x.col(2), D_m_inv);
    const auto P        = calcPiolaStress(F);
    const auto vec_P    = Eigen::Map<const Eigen::Vector4d>{P.data(), P.size()};
    const auto vec_PFPx = elasty::fem::calcVecTrianglePartDeformGradPartPos(D_m_inv);
    const auto PPsiPx   = vec_PFPx.transpose() * vec_P;

    // Calculate $\frac{\partial \Psi}{\partial \mathbf{x}}$ numerically
    Eigen::MatrixXd x_temp = x;
    Eigen::VectorXd diff{6};
    for (size_t i = 0; i < 3; ++i)
    {
        for (size_t j = 0; j < 2; ++j)
        {
            constexpr double eps = 1e-06;

            x_temp(j, i) += eps;
            const auto F_p =
                elasty::fem::calc2dTriangleDeformGrad(x_temp.col(0), x_temp.col(1), x_temp.col(2), D_m_inv);
            const auto e_p = calcEnergyDensity(F_p);

            x_temp = x;

            x_temp(j, i) -= eps;
            const auto F_m =
                elasty::fem::calc2dTriangleDeformGrad(x_temp.col(0), x_temp.col(1), x_temp.col(2), D_m_inv);
            const auto e_m = calcEnergyDensity(F_m);

            x_temp = x;

            diff[i * 2 + j] = (e_p - e_m) / (2.0 * eps);
        }
    }

    EXPECT_TRUE((PPsiPx - diff).norm() < 1e-04);
}

TEST(FemTest, CoRotational2d)
{
    auto calcPiolaStress = [](const Eigen::Matrix2d& F) -> Eigen::Matrix2d
    {
        return elasty::fem::calcCoRotationalPiolaStress(F, k_first_lame, k_second_lame);
    };
    auto calcEnergyDensity = [](const Eigen::Matrix2d& F) -> double
    {
        return elasty::fem::calcCoRotationalEnergyDensity(F, k_first_lame, k_second_lame);
    };

    Eigen::MatrixXd x_rest{2, 3};
    x_rest.col(0) = Eigen::Vector2d{0.0, 0.0};
    x_rest.col(1) = Eigen::Vector2d{1.0, 0.0};
    x_rest.col(2) = Eigen::Vector2d{0.0, 1.0};

    Eigen::MatrixXd x{2, 3};
    x.col(0) = Eigen::Vector2d{0.0, 0.5};
    x.col(1) = Eigen::Vector2d{2.0, 0.0};
    x.col(2) = Eigen::Vector2d{0.0, 1.5};

    // Calculate $\frac{\partial \Psi}{\partial \mathbf{x}}$ analytically
    const auto D_m      = elasty::fem::calc2dShapeMatrix(x_rest.col(0), x_rest.col(1), x_rest.col(2));
    const auto D_m_inv  = D_m.inverse();
    const auto F        = elasty::fem::calc2dTriangleDeformGrad(x.col(0), x.col(1), x.col(2), D_m_inv);
    const auto P        = calcPiolaStress(F);
    const auto vec_P    = Eigen::Map<const Eigen::Vector4d>{P.data(), P.size()};
    const auto vec_PFPx = elasty::fem::calcVecTrianglePartDeformGradPartPos(D_m_inv);
    const auto PPsiPx   = vec_PFPx.transpose() * vec_P;

    // Calculate $\frac{\partial \Psi}{\partial \mathbf{x}}$ numerically
    Eigen::MatrixXd x_temp = x;
    Eigen::VectorXd diff{6};
    for (size_t i = 0; i < 3; ++i)
    {
        for (size_t j = 0; j < 2; ++j)
        {
            constexpr double eps = 1e-06;

            x_temp(j, i) += eps;
            const auto F_p =
                elasty::fem::calc2dTriangleDeformGrad(x_temp.col(0), x_temp.col(1), x_temp.col(2), D_m_inv);
            const auto e_p = calcEnergyDensity(F_p);

            x_temp = x;

            x_temp(j, i) -= eps;
            const auto F_m =
                elasty::fem::calc2dTriangleDeformGrad(x_temp.col(0), x_temp.col(1), x_temp.col(2), D_m_inv);
            const auto e_m = calcEnergyDensity(F_m);

            x_temp = x;

            diff[i * 2 + j] = (e_p - e_m) / (2.0 * eps);
        }
    }

    EXPECT_TRUE((PPsiPx - diff).norm() < 1e-04);
}

int main(int argc, char** argv)
{
    ::testing::InitGoogleTest(&argc, argv);

    return RUN_ALL_TESTS();
}


#include <crave/SystemC.hpp>
#include <crave/ConstrainedRandom.hpp>
#include <systemc.h>
#include <boost/timer.hpp>

using crave::rand_obj;
using crave::randv;
using sc_dt::sc_bv;
using sc_dt::sc_uint;

/**
 * ALU:
 * complete enumerated there are:
 * ADD 0x0: 136
 * SUB 0x1: 136
 * MUL 0x2:  76
 * DIV 0x3: 240
 * valid assignments.
 */
struct ALU4 : public rand_obj {
randv< sc_bv<2> >  op ;
  randv< sc_uint<4> > a, b ;

  ALU4(rand_obj* parent = 0)
  : rand_obj(parent), op(this), a(this), b(this)
  {
    constraint ( (op() != (unsigned char)0x0) || ( (unsigned char)15 >= a() + b() ) );
    constraint ( (op() != (unsigned char)0x1) || (((unsigned char)15 >= a() - b()) && (b() <= a()) ) );
    constraint ( (op() != (unsigned char)0x2) || ( (unsigned char)15 >= a() * b() ) );
    constraint ( (op() != (unsigned char)0x3) || ( b() != (unsigned char)0        ) );
  }

  friend std::ostream & operator<< (std::ostream & o, ALU4 const & alu)
  {
    o << alu.op
      << ' ' << alu.a
      << ' ' << alu.b
      ;
    return o;
  }
};

int sc_main (int argc, char** argv)
{
  crave::init("./crave.cfg");
  boost::timer timer;
  ALU4 c;
  c.next();
  std::cout << "first: " << timer.elapsed() << "\n";
  for (int i=0; i<1000; ++i) {
    c.next();
    //std::cout << i << ": " << c << std::endl;
  }
  std::cout << "complete: " << timer.elapsed() << "\n";
  return 0;
}


#define DEBUG 1
/**
 * File    : F2.cpp
 * Author  : Kazune Takahashi
 * Created : 10/7/2020, 7:10:56 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
// ----- AtCoder Library -----
#include <atcoder/all>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- mint -----
using mint = atcoder::modint1000000007;
// using mint = atcoder::modint998244353;
// using mint = atcoder::modint; // atcoder::modint::set_mod(xxx);
// using mint = atcoder::static_modint<1000000009>;
// using mint0 = dynamic_modint<xxx>;
// using mint1 = dynamic_modint<yyy>;
istream &operator>>(istream &is, mint &x)
{
  ll t;
  is >> t;
  x = t;
  return is;
}
ostream &operator<<(ostream &os, mint const &x)
{
  return os << x.val();
}
// ----- Combination -----
template <typename Mint = mint>
class Combination
{
public:
  constexpr static ll MAX_SIZE{3'000'010LL};
  // constexpr static ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
  vector<mint> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[Mint::mod() % i]) * (Mint::mod() / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using combination = Combination<mint>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- FenwickTree -----
template <typename T = ll>
class FenwickTree : public atcoder::fenwick_tree<T>
{
public:
  using atcoder::fenwick_tree<T>::fenwick_tree;

  FenwickTree(vector<T> const &v) : FenwickTree(static_cast<int>(v.size()))
  {
    for (auto i{size_t{0}}; i < v.size(); ++i)
    {
      atcoder::fenwick_tree<T>::add(i, v[i]);
    }
  }

  T operator[](int i)
  {
    return atcoder::fenwick_tree<T>::sum(i, i + 1);
  }
};
// ----- Math -----
using atcoder::floor_sum;
// ----- UnionFind -----
using UnionFind = atcoder::dsu;
// ----- MaxFlow -----
template <typename Cap = ll>
class MaxFlow : public atcoder::mf_graph<Cap>
{
public:
  using atcoder::mf_graph<Cap>::mf_graph;

  int add_edge(int from, int to)
  {
    return atcoder::mf_graph<Cap>::add_edge(from, to, Cap{1});
  }
};
ostream &operator<<(ostream &os, typename atcoder::mf_graph<ll>::edge const &edge)
{
  return os << "from: " << edge.from << ", to: " << edge.to << ", cap: " << edge.cap << ", flow: " << edge.flow;
}
// ----- MinCostFlow -----
template <typename Cap = ll, typename Cost = ll>
class MinCostFlow : public atcoder::mcf_graph<Cap, Cost>
{
private:
  Cost infty;

public:
  using atcoder::mcf_graph<Cap, Cost>::mcf_graph;

  MinCostFlow(int n, Cost infty = Cost{0}) : atcoder::mcf_graph<Cap, Cost>::mcf_graph(n), infty{infty} {}

  int add_edge(int from, int to, Cap cap)
  {
    return atcoder::mcf_graph<Cap, Cost>::add_edge(from, to, cap, Cost{0});
  }

  int add_edge(int from, int to, Cap cap, Cost cost)
  {
    return atcoder::mcf_graph<Cap, Cost>::add_edge(from, to, cap, cost + infty);
  }

  pair<Cap, Cost> flow(int s, int t)
  {
    return flow(s, t, std::numeric_limits<Cap>::max());
  }

  pair<Cap, Cost> flow(int s, int t, Cap flow_limit)
  {
    return slope(s, t, flow_limit).back();
  }

  vector<pair<Cap, Cost>> slope(int s, int t)
  {
    return slope(s, t, numeric_limits<Cap>::max());
  }

  vector<pair<Cap, Cost>> slope(int s, int t, Cap flow_limit)
  {
    auto res{atcoder::mcf_graph<Cap, Cost>::slope(s, t, flow_limit)};
    for (auto &[cap, cost] : res)
    {
      cost -= cap * infty;
    }
    return res;
  }

  typename atcoder::mcf_graph<Cap, Cost>::edge get_edge(int i)
  {
    auto res{atcoder::mcf_graph<Cap, Cost>::get_edge(i)};
    res.cost -= infty;
    return res;
  }

  vector<typename atcoder::mcf_graph<Cap, Cost>::edge> edges()
  {
    auto res{atcoder::mcf_graph<Cap, Cost>::edges()};
    for (auto &e : res)
    {
      e.cost -= infty;
    }
    return res;
  }
};
ostream &operator<<(ostream &os, typename atcoder::mcf_graph<ll, ll>::edge const &edge)
{
  return os << "from: " << edge.from << ", to: " << edge.to << ", cap: " << edge.cap << ", flow: " << edge.flow << ", cost: " << edge.cost;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Solve -----

class Solve
{

public:
  Solve()
  {
  }

  void flush()
  {
  }

private:
};

// ----- main() -----

/*
int main()
{
  Solve solve;
  solve.flush();
}
*/

combination C;

int main()
{
  ll k;
  string s;
  cin >> k >> s;
  mint ans{0};
  ll y{static_cast<ll>(s.size())};
  for (auto x{0}; x <= k; ++x)
  {
    ans += C(y + k - x - 1, y - 1) * mint{26}.pow(x) * mint{25}.pow(k - x);
  }
  cout << ans << endl;
}