#pragma once
#include "binomials.hpp"
#include <NTL/ZZ.h>
#include <NTL/RR.h>
#include <iomanip>

/***************************Classic ISDs***************************************/

double isd_log_cost_classic_BJMM_approx(const uint32_t n,
                                        const uint32_t k,
                                        const uint32_t t) {
    return ((double)t) * - log((1.0 - (double) k / (double) n)) / log(2);
}

// computes the probability of a random k * k being invertible
const NTL::RR log_probability_k_by_k_is_inv(const NTL::RR &k) {
    NTL::RR log_pinv = NTL::RR(0.5);
    for(long i = 2 ; i <=k ; i++){
        log_pinv = log_pinv * (NTL::RR(1) - NTL::power2_RR(-i));
    }
    return NTL::log(log_pinv);
}

const NTL::RR probability_k_by_k_is_inv(const NTL::RR &k) {
    NTL::RR log_pinv = NTL::RR(0.5);
    for(long i = 2 ; i <=k ; i++){
        log_pinv = log_pinv * (NTL::RR(1) - NTL::power2_RR(-i));
    }
    return log_pinv;
}

const NTL::RR classic_rref_red_cost(const NTL::RR &n, const NTL::RR & r){
    /* simple reduced row echelon form transform, as it is not likely to be the
     * bottleneck */
    NTL::RR k = n-r;
    return r*r*n/NTL::RR(2) +
           (n*r)/NTL::RR(2) -
           r*r*r / NTL::RR(6) +
           r*r +
           r / NTL::RR(6) - NTL::RR(1);
}

const NTL::RR classic_IS_candidate_cost(const NTL::RR &n, const NTL::RR & r){
    return classic_rref_red_cost(n,r)/probability_k_by_k_is_inv(r) + r*r;
}

const NTL::RR Fin_Send_rref_red_cost(const NTL::RR &n,
                                     const NTL::RR &r,
                                     const NTL::RR l){
    /* reduced size reduced row echelon form transformation, only yields an
     * (r-l) sized identity matrix */
    NTL::RR k = n-r;
    return  - l*l*l / NTL::RR(3)
            - l*l*n / NTL::RR(2)
            + l*l*r / NTL::RR(2)
            - 3*l*l / NTL::RR(2)
            - 3*l*n / NTL::RR(2)
            +   l*r / NTL::RR(2)
            -  13*l / NTL::RR(6)
            + n*r*r / NTL::RR(2)
            +   n*r / NTL::RR(2)
            - r*r*r / NTL::RR(6)
            +   r*r
            +     r / NTL::RR(6)
            - NTL::RR(1);
}

const NTL::RR Fin_Send_IS_candidate_cost(const NTL::RR &n,
                                         const NTL::RR &r,
                                         const NTL::RR &l){
    return  Fin_Send_rref_red_cost(n,r,l)/probability_k_by_k_is_inv(r-l) + r*r;
}

double isd_log_cost_classic_Prange(const uint32_t n,
                                   const uint32_t k,
                                   const uint32_t t) {
   NTL::RR n_real = NTL::RR(n);
   NTL::RR k_real = NTL::RR(k);
   NTL::RR t_real = NTL::RR(t);

   NTL::RR cost_iter = classic_IS_candidate_cost(n_real,n_real-k_real);
   NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                       NTL::to_RR(binomial_wrapper(n-k,t));

   NTL::RR log_cost = log2_RR(num_iter)+ log2_RR(cost_iter);
   return NTL::conv<double>( log_cost );
}

#define P_MAX_LB 20
double isd_log_cost_classic_LB(const uint32_t n,
                               const uint32_t k,
                               const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_p = 1;
    uint32_t constrained_max_p = P_MAX_LB > t ? t : P_MAX_LB;
    NTL::RR IS_candidate_cost;
    IS_candidate_cost = classic_IS_candidate_cost(n_real,n_real-k_real);
    for(uint32_t p = 1 ;p < constrained_max_p; p++ ){
       NTL::RR p_real = NTL::RR(p);
       NTL::RR cost_iter = IS_candidate_cost +
                           NTL::to_RR(binomial_wrapper(k,p)*p*(n-k));
       NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                           NTL::to_RR( binomial_wrapper(k,p) *
                                       binomial_wrapper(n-k,t-p) );
       log_cost = (NTL::log(num_iter)+NTL::log(cost_iter)) / NTL::log(NTL::RR(2));
       if(min_log_cost > log_cost){
           min_log_cost = log_cost;
           best_p=p;
       }
    }
    std::cerr << std::endl << "Lee-Brickell best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_Leon P_MAX_LB
#define L_MAX_Leon 200
double isd_log_cost_classic_Leon(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l=0,best_p=1, constrained_max_l, constrained_max_p;

    NTL::RR IS_candidate_cost;
    IS_candidate_cost = classic_IS_candidate_cost(n_real,n_real-k_real);
    constrained_max_p = P_MAX_Leon > t ? t : P_MAX_Leon;
    for(uint32_t p = 1; p < constrained_max_p; p++ ){
      constrained_max_l = ( L_MAX_Leon > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_Leon);
      NTL::RR p_real = NTL::RR(p);
      for(uint32_t l = 0; l < constrained_max_l; l++){
          NTL::RR KChooseP = NTL::to_RR( binomial_wrapper(k,p) );
          NTL::RR cost_iter = IS_candidate_cost +
                   KChooseP * p_real * NTL::to_RR(l) +
                   ( KChooseP / NTL::power2_RR(l))* NTL::RR(p * (n-k - l));
          NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( binomial_wrapper(k,p) *
                                          binomial_wrapper(n-k-l,t-p) );
          log_cost = ( NTL::log(num_iter) + NTL::log(cost_iter) ) / NTL::log(NTL::RR(2));
          if(min_log_cost > log_cost){
              min_log_cost = log_cost;
              best_l = l;
              best_p = p;
          }
       }
    }
    std::cerr << std::endl << "Leon Best l: " << best_l << " best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_Stern P_MAX_Leon
#define L_MAX_Stern L_MAX_Leon
double isd_log_cost_classic_Stern(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l = 0,best_p = 2, constrained_max_l, constrained_max_p;

    NTL::RR IS_candidate_cost;
    IS_candidate_cost = classic_IS_candidate_cost(n_real,n_real-k_real);

    constrained_max_p = P_MAX_Stern > t ? t : P_MAX_Stern;
    for(uint32_t p = 2; p < constrained_max_p; p = p+2 ){
      constrained_max_l = ( L_MAX_Stern > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_Stern);
      NTL::ZZ kHalfChoosePHalf;
      for(uint32_t  l = 0; l < constrained_max_l; l++){
          NTL::RR p_real = NTL::RR(p);
          kHalfChoosePHalf = binomial_wrapper(k/2,p/2);
          NTL::RR kHalfChoosePHalf_real = NTL::to_RR(kHalfChoosePHalf);

          NTL::RR cost_iter = IS_candidate_cost +
                  kHalfChoosePHalf_real *
                              ( NTL::to_RR(l)*p_real +
                                (kHalfChoosePHalf_real / NTL::power2_RR(l))  * NTL::RR(p * (n-k - l))
                              );
// #if LOG_COST_CRITERION == 1
          NTL::RR log_stern_list_size = kHalfChoosePHalf_real *
                                   ( p_real/NTL::RR(2) * NTL::log( k_real/NTL::RR(2))/NTL::log(NTL::RR(2) ) +NTL::to_RR(l));
                  log_stern_list_size = NTL::log(log_stern_list_size) / NTL::log(NTL::RR(2));
                  cost_iter = cost_iter*log_stern_list_size;
// #endif
          NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( kHalfChoosePHalf*kHalfChoosePHalf *
                                          binomial_wrapper(n-k-l,t-p) );
          log_cost = log2_RR(num_iter) + log2_RR(cost_iter);
          if(min_log_cost > log_cost){
              min_log_cost = log_cost;
              best_l = l;
              best_p = p;
          }
       }
    }

    std::cerr << std::endl << "Stern Best l: " << best_l << " best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_FS P_MAX_Stern
#define L_MAX_FS L_MAX_Stern
double isd_log_cost_classic_FS(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l = 0, best_p = 2,constrained_max_l, constrained_max_p;

    NTL::RR IS_candidate_cost;
    constrained_max_p = P_MAX_Stern > t ? t : P_MAX_Stern;
    for(uint32_t p = 2; p < constrained_max_p; p = p+2 ){
      constrained_max_l = ( L_MAX_Stern > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_Stern);
      NTL::RR p_real = NTL::RR(p);
      NTL::ZZ kPlusLHalfChoosePHalf;
      for(uint32_t  l = 0; l < constrained_max_l; l++){
       IS_candidate_cost = Fin_Send_IS_candidate_cost(n_real,n_real-k_real,NTL::RR(l));
          kPlusLHalfChoosePHalf = binomial_wrapper((k+l)/2,p/2);
          NTL::RR kPlusLHalfChoosePHalf_real = NTL::to_RR(kPlusLHalfChoosePHalf);
          NTL::RR cost_iter = IS_candidate_cost +
                  kPlusLHalfChoosePHalf_real *
                              ( NTL::to_RR(l)*p_real +
                                ( kPlusLHalfChoosePHalf_real / NTL::power2_RR(l)) *
                                  NTL::RR(p * (n-k - l))
                              );
// #if LOG_COST_CRITERION == 1
          NTL::RR l_real = NTL::to_RR(l);
          NTL::RR log_FS_list_size = kPlusLHalfChoosePHalf_real *
                                   ( p_real/NTL::RR(2) * NTL::log( (k_real+l_real)/NTL::RR(2))/NTL::log(NTL::RR(2) ) +l_real);
                  log_FS_list_size = log2_RR(log_FS_list_size);
                  cost_iter = cost_iter*log_FS_list_size;
// #endif
          NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( kPlusLHalfChoosePHalf * kPlusLHalfChoosePHalf *
                                          binomial_wrapper(n-k-l,t-p) );

          log_cost = log2_RR(num_iter) + log2_RR(cost_iter);
          if(min_log_cost > log_cost){
              min_log_cost = log_cost;
              best_l = l;
              best_p = p;
          }
       }
    }
    std::cerr << std::endl << "FS Best l: " << best_l << " best p: " << best_p << std::endl;
    return NTL::conv<double>( min_log_cost );
}

#define P_MAX_MMT (P_MAX_FS+25) // P_MAX_MMT
#define L_MAX_MMT 350 //L_MAX_MMT
#define L_MIN_MMT 2
double isd_log_cost_classic_MMT(const uint32_t  n,
                                 const uint32_t k,
                                 const uint32_t t) {
    uint32_t r = n-k;
    NTL::RR n_real = NTL::RR(n);
    NTL::RR r_real = NTL::RR(r);
    NTL::RR k_real = n_real-r_real;

    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost, log_mem_cost;
    uint32_t best_l= L_MIN_MMT, best_l1, best_p = 4,
             constrained_max_l = 0, constrained_max_p;

    NTL::RR FS_IS_candidate_cost;
    constrained_max_p = P_MAX_MMT > t ? t : P_MAX_MMT;
    /* p should be divisible by 4 in MMT */
    for(uint32_t p = 4; p <= constrained_max_p; p = p+4 ){
      constrained_max_l = ( L_MAX_MMT > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_MMT );
             for(uint32_t l = L_MIN_MMT; l <= constrained_max_l; l++){
                NTL::RR l_real = NTL::to_RR(l);
                NTL::ZZ kPlusLHalfChoosePHalf = binomial_wrapper((k+l)/2,p/2);
                NTL::RR num_iter  = NTL::to_RR(binomial_wrapper(n,t)) /
                              NTL::to_RR( kPlusLHalfChoosePHalf * kPlusLHalfChoosePHalf *
                                          binomial_wrapper(n-k-l,t-p) );
                FS_IS_candidate_cost = Fin_Send_IS_candidate_cost(n_real,r_real,l_real);
                NTL::ZZ  kPlusLHalfChoosePFourths = binomial_wrapper((k+l)/2,p/4);
                NTL::RR  kPlusLHalfChoosePFourths_real = NTL::to_RR(kPlusLHalfChoosePFourths);
                NTL::RR  minOperandRight, min;
                NTL::RR  PChoosePHalf = NTL::to_RR(binomial_wrapper(p,p/2));
                NTL::RR  kPlusLChoosePHalf = NTL::to_RR(binomial_wrapper((k+l),p/2));
                minOperandRight = NTL::to_RR(binomial_wrapper((k+l)/2,p/2)) / PChoosePHalf;
                min =  kPlusLHalfChoosePFourths_real > minOperandRight ? minOperandRight : kPlusLHalfChoosePFourths_real;

               /* hoist out anything not depending on l_1/l_2 split*/
#if defined(EXPLORE_REPRS)
               for(uint32_t l_1 = 1 ; l_1 <= l ; l_1++){
                  uint32_t l_2= l-l_1;
#else
                  uint32_t l_2 = NTL::conv<unsigned int>(log2_RR(kPlusLHalfChoosePFourths_real / NTL::to_RR(binomial_wrapper(p,p/2))));
                  /*clamp l_2 to a safe value , 0 < l_2 < l*/
                  l_2 = l_2 <= 0 ? 1 : l_2;
                  l_2 = l_2 >= l ? l-1 : l_2;

                  uint32_t l_1= l - l_2;
#endif
                  NTL::RR interm = kPlusLHalfChoosePFourths_real / NTL::power2_RR(l_2) *
                                         NTL::to_RR(p/2*l_1);

                  NTL::RR otherFactor = ( NTL::to_RR(p/4*l_2) + interm );
                  NTL::RR cost_iter = FS_IS_candidate_cost +
                                      min*otherFactor +
                                      kPlusLHalfChoosePFourths_real * NTL::to_RR(p/2*l_2);

                  NTL::RR lastAddend = otherFactor +
                                        kPlusLHalfChoosePFourths_real *
                                         kPlusLChoosePHalf * PChoosePHalf /
                                         NTL::power2_RR(l)   *
                                       NTL::to_RR( p*(r-l) );
                  lastAddend = lastAddend * kPlusLHalfChoosePFourths_real;
                  cost_iter += lastAddend;
// #if 0

          NTL::RR log_MMT_space = r_real*n_real +
                                  kPlusLHalfChoosePFourths_real *
                                       (NTL::to_RR(p/4)* log2_RR(NTL::to_RR(k+l/2))+ NTL::to_RR(l_2) )+
                                  NTL::to_RR(min) * (NTL::to_RR(p/2)* log2_RR(NTL::to_RR(k+l))+ NTL::to_RR(l) );
                  log_MMT_space = log2_RR(log_MMT_space);
                  cost_iter = cost_iter*log_MMT_space;
// #endif
                  log_cost = log2_RR(num_iter) + log2_RR(cost_iter);
                  if(min_log_cost > log_cost){
                      min_log_cost = log_cost;
                      best_l = l;
                      best_l1 = l_1;
                      best_p = p;
                      log_mem_cost = log_MMT_space;
                  }
#if defined(EXPLORE_REPRS)
               }
#endif
            }
    }
    std::cerr << std::endl << "MMT Best l: " << best_l
                           << " best p: "   << best_p
                           << " best l1: "  << best_l1
                           << std::endl;
   if(best_p == constrained_max_p){
          std::cerr << std::endl << "Warning: p on exploration edge! " << std::endl;
   }
   if(best_l == constrained_max_l){
          std::cerr << std::endl << "Warning: l on exploration edge! " << std::endl;
   }
   //std::cerr << log_mem_cost << " ";
   return NTL::conv<double>( min_log_cost );
}

#define P_MAX_BJMM 20 // P_MAX_MMT
#define L_MAX_BJMM 90 //L_MAX_MMT
#define Eps1_MAX_BJMM 4
#define Eps2_MAX_BJMM 4
double isd_log_cost_classic_BJMM(const uint32_t n,
                                 const uint32_t k,
                                 const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    uint32_t r = n-k;
    NTL::RR r_real = NTL::RR(r);

    NTL::RR min_log_cost = n_real; // unreachable upper bound
    NTL::RR log_cost;
    uint32_t best_l, best_p,
             best_eps_1, best_eps_2,
             constrained_max_l, constrained_max_p;

    NTL::RR FS_IS_candidate_cost;
    constrained_max_p = P_MAX_BJMM > t ? t : P_MAX_BJMM;
    /*p should be divisible by 2 in BJMM */
    for(uint32_t p = 2; p < constrained_max_p; p = p+2 ){
        /* sweep over all the valid eps1 knowing that p/2 + eps1 should be a
         * multiple of 4*/
        constrained_max_l = ( L_MAX_BJMM > (n-k-(t-p)) ? (n-k-(t-p)) : L_MAX_BJMM );
        for(uint32_t  l = 0; l < constrained_max_l; l++){
            for(uint32_t eps1 = 2+(p%2) ; eps1 < Eps1_MAX_BJMM; eps1 = eps1 + 2) {
                uint32_t p_1 = p/2 + eps1;
            /* sweep over all the valid eps2 knowing that p_1/2 + eps2 should
             * be even */
                for(uint32_t eps2 = (p_1%2) ; eps2 < Eps2_MAX_BJMM; eps2 = eps2 + 2){
                    uint32_t p_2 = p_1/2 + eps2;

                    /* Available parameters p, p_1,p_2,p_3, l */
                    NTL::RR l_real = NTL::RR(l);
                    FS_IS_candidate_cost = Fin_Send_IS_candidate_cost(n_real,n_real-k_real,l_real);
                    uint32_t p_3 = p_2/2;

                    NTL::ZZ L3_list_len = binomial_wrapper((k+l)/2,p_3);
                    NTL::RR L3_list_len_real = NTL::to_RR(L3_list_len);
                    /* the BJMM number of iterations depends only on L3 parameters
                    * precompute it */
                    NTL::RR num_iter  = NTL::to_RR( binomial_wrapper(n,t) ) /
                                        NTL::to_RR( binomial_wrapper((k+l),p) *
                                                    binomial_wrapper(r-l,t-p)
                                                );
                    NTL::RR P_invalid_splits = NTL::power(L3_list_len_real,2) /
                                            NTL::to_RR( binomial_wrapper(k+l,p_2));
                    num_iter = num_iter / NTL::power(P_invalid_splits,4);

                    /* lengths of lists 2 to 0 have to be divided by the number of repr.s*/
                    NTL::RR L2_list_len = NTL::to_RR(binomial_wrapper(k+l,p_2)) *
                                        NTL::power(P_invalid_splits,1);
                    NTL::RR L1_list_len = NTL::to_RR(binomial_wrapper(k+l,p_1)) *
                                        NTL::power(P_invalid_splits,2);
                    /* estimating the range for r_1 and r_2 requires to compute the
                    * number of representations rho_1 and rho_2 */

                    NTL::ZZ rho_2 = binomial_wrapper(p_1,p_1/2) *
                                    binomial_wrapper(k+l-p_1,eps2);
                    NTL::ZZ rho_1 = binomial_wrapper(p,p/2) *
                                    binomial_wrapper(k+l-p,eps1);
                    int min_r2 = NTL::conv<int>(NTL::log(NTL::to_RR(rho_2)) /
                                NTL::log(NTL::RR(2)));
                    int max_r1 = NTL::conv<int>(NTL::log(NTL::to_RR(rho_1)) /
                                NTL::log(NTL::RR(2)));

                    /*enumerate r_1 and r_2 over the suggested range
                    * log(rho_2) < r2 < r_1 < log(rho_1)*/
                    /* clamp to safe values */
                    min_r2 = min_r2 > 0 ? min_r2 : 1;
                    max_r1 = max_r1 < (int)l ? max_r1 : l-1;

                    NTL::RR p_real = NTL::RR(p);
                    for(int r_2 = min_r2 ; r_2 < max_r1 - 1; r_2++){
                        for(int r_1 = r_2+1; r_1 < max_r1 ; r_1++){

                            /*add the cost of building Layer 3 to cost_iter */
                            NTL::RR cost_iter = NTL::to_RR(4) *
                                                (k + l + 2*L3_list_len_real +
                                                r_2 +
                                                NTL::power(L3_list_len_real,2)*
                                                NTL::to_RR(2*p_3*r_2));

                             /* add the cost of building Layer 2 */
                            cost_iter +=    2 * (NTL::power((NTL::to_RR(rho_2) /
                                            (NTL::power2_RR(r_2)))*
                                            NTL::power(L3_list_len_real,2),2)
                                            * 2 * p_2 * (r_1-r_2));

                            /* add the cost of building Layer 1 */
                            cost_iter +=    NTL::power((NTL::to_RR(rho_1) /
                                            NTL::power2_RR(r_1)) *
                                            (NTL::to_RR(rho_2) /
                                            NTL::power2_RR(r_2))*
                                            NTL::power(L3_list_len_real,2),4) * 2 * p_1 * l;

                             /* add the cost of building L0 */
                            cost_iter +=    p * (r - l) *
                                            NTL::power((NTL::to_RR(rho_1) / NTL::power2_RR(r_1)) *
                                            (NTL::to_RR(rho_2) /
                                            NTL::power2_RR(r_2))*
                                            NTL::power(L3_list_len_real,2),4)
                                            / NTL::to_RR(l);

                            log_cost = log2_RR(num_iter) + log2_RR(cost_iter);

                            if(min_log_cost >  log_cost){
                                min_log_cost = log_cost;
                                best_l = l;
                                best_p = p;
                                best_eps_1 = eps1;
                                best_eps_2 = eps2;
                            }
                        }
                    }

                } /*end of iteration over l */
            /* to review up to to here */
            } /* end for over eps2 */
         } /* end for over eps1 */
    } /* end for over p*/
    std::cerr << std::endl << "BJMM Best l: " << best_l
                            << " best p: "   << best_p
                            << " best eps1: "  << best_eps_1
                            << " best eps2: "  << best_eps_2
                            << std::endl;
   return NTL::conv<double>( min_log_cost );
}

/***************************Quantum ISDs***************************************/

const NTL::RR quantum_gauss_red_cost(const NTL::RR &n,
                                     const NTL::RR & k) {
    return 0.5* NTL::power(n-k,3) + k*NTL::power((n-k),2);
}

double isd_log_cost_quantum_LB(const uint32_t n,
                               const uint32_t k,
                               const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR log_pi_fourths = NTL::log(pi*0.25);
    NTL::RR log_pinv = log_probability_k_by_k_is_inv(k_real);
    NTL::RR iteration_cost = quantum_gauss_red_cost(n_real,k_real) +
                             NTL::to_RR(binomial_wrapper(k,2) * 2 * (n-k));
    NTL::RR log_cost = (lnBinom(n_real,t_real) -
                        (lnBinom(k_real,NTL::RR(2)) +
                         lnBinom(n_real-k_real,t-NTL::RR(2)))
                       )*0.5 +
                       log_pi_fourths;
    log_cost += NTL::log(iteration_cost);
    log_cost = log_cost / NTL::log(NTL::RR(2));
    return NTL::conv<double>( log_cost );
}

#define MAX_M (t/2)

double isd_log_cost_quantum_stern(const uint32_t n,
                                  const uint32_t k,
                                  const uint32_t t) {
    NTL::RR n_real = NTL::RR(n);
    NTL::RR k_real = NTL::RR(k);
    NTL::RR t_real = NTL::RR(t);
    NTL::RR current_complexity, log_p_success, c_it, c_dec;

    // Start computing Stern's parameter invariant portions of complexity
    NTL::RR log_pi_fourths = NTL::log(pi*0.25);
    // compute the probability of a random k * k being invertible
    NTL::RR log_pinv = log_probability_k_by_k_is_inv(k_real);
    // compute the cost of inverting the matrix, in a quantum execution env.
    NTL::RR c_inv = quantum_gauss_red_cost(n_real,k_real);

    // optimize Stern's parameters :
    // m : the # of errors in half of the chosen dimensions
    // l : the length of the run of zeroes in the not chosen dimensions
    // done via exhaustive parameter space search, minimizing the total
    // complexity.
    // Initial value set to codeword bruteforce to ensure the minimum is found.
    NTL::RR min_stern_complexity = NTL::RR(n)*NTL::log(NTL::RR(2));

    for(long m = 1; m <= MAX_M; m++){
        NTL::RR m_real = NTL::RR(m);
        /* previous best complexity as a function of l alone.
         * initialize to bruteforce-equivalent,  break optimization loop as soon
         * as a minimum is found */
        NTL::RR prev_best_complexity = NTL::RR(t);
        for(long l = 0; l < (n-k-(t-2*m)); l++ ){

          NTL::RR  l_real = NTL::RR(l);
          log_p_success = lnBinom(t_real, 2*m_real) +
                   lnBinom(n_real-t_real, k_real-2*m_real) +
                   lnBinom(2*m_real,m_real) +
                   lnBinom(n_real-k_real-t_real+2*m_real,l_real);
          log_p_success = log_p_success - ( m_real*NTL::log(NTL::RR(4)) +
                              lnBinom(n_real,k_real) +
                              lnBinom(n_real -k_real, l_real));
         current_complexity = -(log_p_success+log_pinv)*0.5 + log_pi_fourths;
         /* to match specifications , the term should be
          * (n_real-k_real), as per in deVries, although
          * David Hobach thesis mentions it to be
          * (n_real-k_real-l_real), and it seems to match.
          * amend specs for the typo. */
         c_it = l_real +
            (n_real-k_real-l_real)* NTL::to_RR(binomial_wrapper(k/2,m)) /
            NTL::power2_RR(-l);

         c_it = c_it * 2*m_real * NTL::to_RR(binomial_wrapper(k/2,m));
#if IGNORE_DECODING_COST == 1
        c_dec = 0.0;
#elif IGNORE_DECODING_COST == 0
        /*cost of decoding estimated as per Golomb CWDEC
         * decoding an n-bit vector with weight k is
         * CWDEC_cost(k,n)=O(n^2 log_2(n))  and following deVries, where
         * c_dec = CWDEC_cost(n-k, n) + k + CWDEC_cost(l,n-k)*/
        c_dec = n_real*n_real*NTL::log(n_real) + k_real +
                (n_real-k_real)*(n_real-k_real)*NTL::log((n_real-k_real));
#endif
         current_complexity = current_complexity + NTL::log(c_it+c_inv+c_dec);
          if(current_complexity < prev_best_complexity){
             prev_best_complexity = current_complexity;
          } else{
             break;
          }
        }
       if(current_complexity < min_stern_complexity){
           min_stern_complexity = current_complexity;
       }
    }
    return NTL::conv<double>( min_stern_complexity / NTL::log(NTL::RR(2.0)) );
}

/***************************Aggregation ***************************************/

double c_isd_log_cost(const uint32_t n,
                      const uint32_t k,
                      const uint32_t t,
                      const uint32_t qc_order,
                      const uint32_t is_kra) {
    double min_cost = n, current_cost;
    /* for key recovery attacks the advantage from quasi-cyclicity is p,
     * for an ISD, the DOOM advantage is just sqrt(p) */
    double qc_red_factor= is_kra ? logl(qc_order) : logl(qc_order)/2.0;
    qc_red_factor = qc_red_factor/logl(2);

    std::cout << "Classic ";
    current_cost = isd_log_cost_classic_Prange(n,k,t) - qc_red_factor;
    std::cerr << "Classic Prange: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = current_cost;

    current_cost = isd_log_cost_classic_LB(n,k,t)- qc_red_factor;
    std::cerr << "Classic Lee-Brickell ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_Leon(n,k,t)- qc_red_factor;
     std::cerr << "Classic Leon ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_Stern(n,k,t)- qc_red_factor;
    std::cerr << "Classic Stern ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_FS(n,k,t)- qc_red_factor;
    std::cerr << "Classic Fin-Send ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

    current_cost = isd_log_cost_classic_MMT(n,k,t)- qc_red_factor;
    std::cerr << "Classic MMT ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;

#if SKIP_BJMM == 0
    current_cost = isd_log_cost_classic_BJMM(n,k,t)- qc_red_factor;
    std::cerr << "Classic BJMM ISD: " << std::setprecision(5) << current_cost << std::endl;
    std::cout << current_cost << " ";
    min_cost = min_cost > current_cost ? current_cost : min_cost;
#endif
    std::cout << std::endl;

    return min_cost;
}

double q_isd_log_cost(const uint32_t n,
                      const uint32_t k,
                      const uint32_t t,
                      const uint32_t qc_order,
                      const uint32_t is_kra) {
    double min_cost = n, current_cost;
    /* for key recovery attacks the advantage from quasi-cyclicity is p,
     * for an ISD, the DOOM advantage is just sqrt(p) */
    double qc_red_factor= is_kra ? logl(qc_order) : logl(qc_order)/2.0;
    qc_red_factor = qc_red_factor/logl(2);
    std::cout << "Quantum ";

    current_cost = isd_log_cost_quantum_LB(n,k,t)- qc_red_factor;
    std::cout << current_cost << " ";
//     std::cout << " Q-Lee-Brickell ISD: " << /**/current_cost << std::endl;
    min_cost = current_cost;

    current_cost = isd_log_cost_quantum_stern(n, k, t)- qc_red_factor;
    std::cout << current_cost << " ";
//     std::cout << ", Q-Stern ISD: " << current_cost << std::endl;
    min_cost = min_cost > current_cost ? current_cost : min_cost;
    std::cout << std::endl;

    return min_cost;
}


#include "bend_quadratic_forces.h"

#include <igl/sparse_cached.h>
#include <iostream>
#include <Eigen/Dense>
#include <igl/edges.h>
#include "../adjacency.h"
#include "../timer.h"

using namespace std;
using namespace Eigen;

typedef Eigen::Triplet<double> Tri;

// reference code for Discrete Quadratic Curvature Energies is here
// http://www.cs.columbia.edu/cg/quadratic/
// I took the functions cotTheta and Compute LocalStiffness and made them works with Eigen

//         x2
//         /\
//        /  \
//     e1/    \e3
//      /  t0  \
//     /        \
//    /    e0    \
//  x0------------x1
//    \          /
//     \   t1   /
//      \      /
//     e2\    /e4
//        \  /
//         \/
//         x3
//
// Edge orientation: e0,e1,e2 point away from x0
//                      e3,e4 point away from x1

Bend::Bend() {}

double Bend::cotTheta(const Vector3d v, const Vector3d w)
{
	//assert(finite(v.length()));
	//assert(finite(w.length()));
	//assert(v.length() > 0);
	//assert(w.length() > 0);
	const double cosTheta = v.dot(w);
	const double sinTheta = v.cross(w).norm();
	return (cosTheta / sinTheta);
}

// compute 4 by 4 local stiffness matrix Q(e0)
void Bend::ComputeLocalStiffness(
	const vector< Vector3d>& x,
	Matrix4d& Q)
{
	const Vector3d e0 = x[1] - x[0];
	const Vector3d e1 = x[2] - x[0];
	const Vector3d e2 = x[3] - x[0];
	const Vector3d e3 = x[2] - x[1];
	const Vector3d e4 = x[3] - x[1];

	const double c01 = cotTheta(e0, e1);
	const double c02 = cotTheta(e0, e2);
	const double c03 = cotTheta(-e0, e3);
	const double c04 = cotTheta(-e0, e4);

	const Vector4d K0 = Vector4d(c03 + c04, c01 + c02, -c01 - c03, -c02 - c04);

	const double A0 = e0.cross(e1).norm();
	const double A1 = e0.cross(e2).norm();

	const double coef = -3. / (A0 + A1);

	assert(finite(coef));
	assert(finite(c01));
	assert(finite(c02));
	assert(finite(c03));
	assert(finite(c04));

	// compute Q = coef times outer product of K0 and K0
	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < i; ++j) {
			Q(i, j) = Q(j, i) = coef * K0[i] * K0[j];
		}
		Q(i, i) = coef * K0[i] * K0[i];
	}
}

void Bend::init(
	const double k_bend,
	const double k_damping,
	const MatrixXd& X,			// in: vertex positions
	const MatrixXi& T			// in: mesh triangles
) {
	this->k_bend = k_bend;
	this->k_damping = k_damping;
	this->n = X.rows();

	// create list of 4 vertices for each face pair (each internal edge)
	createFacePairEdgeListWith4VerticeIDs(T, E4);

	// precompute the matrices Q, K and D
	K.resize(3 * n, 3 * n);
	D.resize(n * 3, n * 3);
	Q.resize(E4.rows());

    F_local.resize(E4.rows() * 4);

	this->precompute_rest_shape(X);
}

void Bend::precompute_rest_shape(const MatrixXd& X){

    if(!tripletsInitialized)
    {
        std::vector<Eigen::Triplet<double>> tri;

        for (int e = 0; e < E4.rows(); e++)
        {
            // add stiffness matrix
            for (int dim = 0; dim < 3; dim++) {
                for (int v1 = 0; v1 < 4; v1++) {
                    for (int v2 = 0; v2 < v1; v2++) {
                        tri.push_back(Tri(E4(e, v1) + dim * n, E4(e, v2) + dim * n, 0));
                        tri.push_back(Tri(E4(e, v2) + dim * n, E4(e, v1) + dim * n, 0));
                    }

                    tri.push_back(Tri(E4(e, v1) + dim * n, E4(e, v1) + dim * n, 0));
                }
            }
        }

        K = SparseMatrix<double>(3 * n, 3 * n);
        igl::sparse_cached_precompute(tri, K_data, K);
        D = K;

        triK.resize(tri.size());
        triD.resize(tri.size());

        tripletsInitialized = true;
    }

#pragma omp parallel for
    for (int e = 0; e < E4.rows(); e++) {
        vector< Vector3d > x(4);
        for (int i = 0; i < 4; i++)
            x[i] = X.row(E4(e, i));

        ComputeLocalStiffness(x, Q[e]);

        // --- second order derivatives ---
        // stiffness and damping matrix
        Matrix4d K_local = k_bend * Q[e];
        Matrix4d D_local = k_damping * Q[e];

        int offset = e * 48;

        // add stiffness matrix
        for (int dim = 0; dim < 3; dim++) {
            for (int v1 = 0; v1 < 4; v1++) {
                for (int v2 = 0; v2 < v1; v2++) {

                    triK[offset] = Tri(0, 0, K_local(v1, v2));
                    triK[offset+1] = Tri(0, 0, K_local(v2, v1));

                    triD[offset] = Tri(0, 0, D_local(v1, v2));
                    triD[offset+1] = Tri(0, 0, D_local(v2, v1));

                    offset += 2;
                }

                triK[offset] = Tri(0, 0,  K_local(v1, v1));
                triD[offset] = Tri(0, 0,  D_local(v1, v1));

                offset++;
            }
        }
    }

    igl::sparse_cached(triK, K_data, this->K);
    igl::sparse_cached(triD, K_data, this->D);
}

const Eigen::SparseMatrix<double>& Bend::getK() const
{
    return K;
}

const Eigen::SparseMatrix<double>& Bend::getD() const
{
    return D;
}

void Bend::compute_forces(
	const MatrixXd& X,			// in: vertex positions
	const VectorXd& V,			// in: velocities
	VectorXd& F)				// out: forces
{
    //#pragma omp parallel for
	for (int e = 0; e < E4.rows(); e++) {

        // get necessary positions and velocities for this edge
		vector< Vector4d > x(3);				// position: 3 vectors: x_coords, y_coords and z_coords - each with 4 entries for 4 vertices
		vector< Vector4d > v(3);				// velocity

        for (int i = 0; i < 4; i++) {
			int vertex = E4(e, i);
            for (int j = 0; j < 3; j++) {
				x[j](i) = X(vertex, j);
				v[j](i) = V(vertex + j * n);
			}
		}

        const auto qe = Q[e];

        F_local[3 * e + 0] = qe * (k_bend * x[0] + k_damping * v[0]);
        F_local[3 * e + 1] = qe * (k_bend * x[1] + k_damping * v[1]);
        F_local[3 * e + 2] = qe * (k_bend * x[2] + k_damping * v[2]);
	}

    for (int e = 0; e < E4.rows(); e++) {
        for (int i = 0; i < 4; i++) {            // force on vertex i
            const int vertex = E4(e, i);
            for (int j = 0; j < 3; j++) {        // put each force dimension into the right place of the total F
                F(vertex + j * n) += F_local[3 * e + j](i);
            }
        }
    }
}


#include "eigen-runtime.h"
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/Sparse>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/SymEigsShiftSolver.h>
#include <Spectra/SymGEigsSolver.h>
#include <Spectra/MatOp/DenseSymMatProd.h>
#include <Spectra/MatOp/SparseGenRealShiftSolve.h>
#include <Spectra/MatOp/SparseSymShiftSolve.h>
#include <Spectra/MatOp/SparseCholesky.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/Util/GEigsMode.h>

using namespace Spectra;
using namespace Eigen;

extern "C" RET eigen_eig(
    void* d, void* v,
    const void* p, int n)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map< Matrix<T3,Dynamic,Dynamic> > MapComplexMatrix;
    MapMatrix M((T1*)p, n, n);
    MapComplexMatrix D((T3*)d, n, 1);
    MapComplexMatrix V((T3*)v, n, n);
    EigenSolver<MatrixXd> es(M);
    D = es.eigenvalues();
    V = es.eigenvectors();
}

extern "C" const int spectral_eigs(
    const int k,   // number of eigenvectors to return
    void* d, void* v,  // pointer to the results
    const void* p, const int n,  // pointer to the input and dimensionality
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map< Matrix<T3,Dynamic,Dynamic> > MapComplexMatrix;
    MapMatrix M((T1*)p, n, n);
    MapComplexMatrix D((T3*)d, k, 1);
    MapComplexMatrix V((T3*)v, n, k);

    if(mode == 0) {
        DenseGenMatProd<double> op(M);
        GenEigsSolver<DenseGenMatProd<double>> eigs(op, k, ncv);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    } else {
        DenseGenRealShiftSolve<double> op(M);
        GenEigsRealShiftSolver<DenseGenRealShiftSolve<double>> eigs(op, k, ncv, sigma);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    }
}

extern "C" const int spectral_eigsh(
    int k,
    void* d, void* v,
    const void* p, int n,
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    MapMatrix M((T1*)p, n, n);
    MapMatrix D((T1*)d, k, 1);
    MapMatrix V((T1*)v, n, k);

    if(mode == 0) {
        DenseGenMatProd<double> op(M);
        SymEigsSolver<DenseGenMatProd<double>> eigsh(op, k, ncv);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    } else {
        DenseSymShiftSolve<double> op(M);
        SymEigsShiftSolver<DenseSymShiftSolve<double>> eigsh(op, k, ncv, sigma);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    }
}

extern "C" const int spectral_seigs(
    int k,
    void* d, void* v,
    const void* values, const void* outerIndexPtr, const void* innerIndices, int n, int s,
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T3,Dynamic,Dynamic> > MapComplexMatrix;
    typedef Map<const SparseMatrix<T1> > MapSparseMatrix;
    MapSparseMatrix M(n, n, s, (int*)outerIndexPtr, (int*)innerIndices, (T1*)values);
    MapComplexMatrix D((T3*)d, k, 1);
    MapComplexMatrix V((T3*)v, n, k);

    if(mode == 0) {
        SparseGenMatProd<double> op(M);
        GenEigsSolver<SparseGenMatProd<double>> eigs(op, k, ncv);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    } else {
        SparseGenRealShiftSolve<double> op(M);
        GenEigsRealShiftSolver<SparseGenRealShiftSolve<double>> eigs(op, k, ncv, sigma);
        eigs.init();
        int nconv = eigs.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigs.info() == CompInfo::Successful) {
            D = eigs.eigenvalues();
            V = eigs.eigenvectors();
        }
        return static_cast<int>(eigs.info());
    }
}

extern "C" const int spectral_seigsh(
    int k,
    void* d, void* v,
    const void* values, const void* outerIndexPtr, const void* innerIndices, int n, int s,
    const int ncv, int maxit, double tol,
    const int mode, const double sigma,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map<const SparseMatrix<T1> > MapSparseMatrix;
    MapSparseMatrix M(n, n, s, (int*)outerIndexPtr, (int*)innerIndices, (T1*)values);
    MapMatrix D((T1*)d, k, 1);
    MapMatrix V((T1*)v, n, k);

    if(mode == 0) {
        SparseGenMatProd<double> op(M);
        SymEigsSolver<SparseGenMatProd<double>> eigsh(op, k, ncv);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    } else {
        SparseSymShiftSolve<double> op(M);
        SymEigsShiftSolver<SparseSymShiftSolve<double>> eigsh(op, k, ncv, 0);
        eigsh.init();
        int nconv = eigsh.compute(static_cast<SortRule>(select), maxit, tol);
        if(eigsh.info() == CompInfo::Successful) {
            D = eigsh.eigenvalues();
            V = eigsh.eigenvectors();
        }
        return static_cast<int>(eigsh.info());
    }
}

// Generalized eigen solver for real symmetric matrices
extern "C" const int spectral_geigsh(
    int k,
    void* d, void* v,
    const void* a, int n,
    const void* values, const void* outerIndexPtr, const void* innerIndices, int s,
    const int ncv, int maxit, double tol,
    const int select)
{
    typedef Map< Matrix<T1,Dynamic,Dynamic> > MapMatrix;
    typedef Map<const SparseMatrix<T1> > MapSparseMatrix;
    MapMatrix A((T1*)a, n, n);
    MapSparseMatrix B(n, n, s, (int*)outerIndexPtr, (int*)innerIndices, (T1*)values);
    MapMatrix D((T1*)d, k, 1);
    MapMatrix V((T1*)v, n, k);

    DenseSymMatProd<double> op(A);
    SparseCholesky<double> Bop(B);

    SymGEigsSolver<DenseSymMatProd<double>, SparseCholesky<double>, GEigsMode::Cholesky> geigs(op, Bop, k, ncv);

    geigs.init();
    int nconv = geigs.compute(static_cast<SortRule>(select), maxit, tol);
    if(geigs.info() == CompInfo::Successful) {
        D = geigs.eigenvalues();
        V = geigs.eigenvectors();
    }
    return static_cast<int>(geigs.info());
}

template <class T>
RET cholesky(void* px, const void* pa, int n)
{
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix x((T*)px, n, n);
    MapMatrix A((T*)pa, n, n);
    x = A.llt().matrixL();
    return 0;
}
API(cholesky, (int code,
    void* px, const void* pa, int n), (px,pa,n));

template <class T, class TT>
RET bdcsvd(
    void* pu, void* ps, void* pv,
    const void* px, int r, int c)
{
    int m = r < c ? r : c;
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    typedef Map< Matrix<TT,Dynamic,Dynamic> > MapMatrix2;
    MapMatrix A((T*)px, r, c);
    MapMatrix U((T*)pu, r, m);
    MapMatrix2 s((TT*)ps, m, 1);
    MapMatrix V((T*)pv, c, m);
    BDCSVD< Matrix<T,Dynamic,Dynamic>> svd(A, ComputeThinU|ComputeThinV);
    U = svd.matrixU();
    V = svd.matrixV();
    s = svd.singularValues();
    return 0;
}
API2(bdcsvd, (int code,
    void* pu, void* ps, void* pv, const void* px, int r, int c), (pu,ps,pv,px,r,c));


/**
 * @file    gaussNewton.hpp
 * @brief   This provides the Gauss-Newton algorithm for Non-Linear Least Squares
 * @author  Shubham Shrivastava
 */

#ifndef GAUSS_NEWTON_H_
#define GAUSS_NEWTON_H_

#include <Eigen/Dense>
#include "tapl/common/common.hpp"

namespace tapl {
    namespace optim {

        /**< Gauss-Newton Optimizer */
        class GaussNewtonOptimizer {
        public:
            /**
             * @brief Compute reprojection error of an estimated 3d point in multiple cameras
             *
             * @param[in] point3d estimated 3d point
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             * @param[out] errors reprojection error in 'n' cameras (n x 2)
             */
            Eigen::MatrixXd reprojectionError(
                                    const Eigen::MatrixXd &point3d,
                                    const std::vector<tapl::Point2d> &points2d,
                                    const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // reprojection errors
                Eigen::MatrixXd errors(projectionMatrices.size()*2,1);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    // project to camera plane
                    Eigen::MatrixXd point2d_homogeneous(1,3);
                    point2d_homogeneous = (*it) * point3d_homogeneous;
                    Eigen::MatrixXd point2d_euclidean(2,1);
                    point2d_euclidean << (point2d_homogeneous(0,0) / point2d_homogeneous(2,0)),
                                         (point2d_homogeneous(1,0) / point2d_homogeneous(2,0));
                    // compute reprojection error
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    Eigen::MatrixXd reprojection_err(2,1);
                    Eigen::MatrixXd points2d_eigen(2,1);
                    points2d_eigen << points2d[cam_idx].x, points2d[cam_idx].y;
                    reprojection_err << (point2d_euclidean - points2d_eigen);
                    errors.block<2,1>(2*cam_idx,0) = reprojection_err;
                }
                // return reprojection errors
                return errors;
            }

            /**
             * @brief Given a 3D point and its corresponding points in the image
             *         planes, compute the associated Jacobian
             *
             * @param[in] point3d estimated 3d point
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             *
             * @return J Jacobian Matrix
             */
            const Eigen::MatrixXd jacobian(
                            const Eigen::MatrixXd &point3d,
                            const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // jacobian matrix
                Eigen::MatrixXd J(projectionMatrices.size()*2,3);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    double m1P_hat = ((*it).block<1,4>(0,0) * point3d_homogeneous)(0,0);
                    double m2P_hat = ((*it).block<1,4>(1,0) * point3d_homogeneous)(0,0);
                    double m3P_hat = ((*it).block<1,4>(2,0) * point3d_homogeneous)(0,0);

                    // build Jacobian
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    J.block<1,3>(2*cam_idx,0) << (((*it)(0,0)*m3P_hat - (*it)(2,0)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 (((*it)(0,1)*m3P_hat - (*it)(2,1)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 (((*it)(0,2)*m3P_hat - (*it)(2,2)*m1P_hat) / (m3P_hat*m3P_hat));
                    J.block<1,3>((2*cam_idx)+1,0) << (((*it)(1,0)*m3P_hat - (*it)(2,0)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     (((*it)(1,1)*m3P_hat - (*it)(2,1)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     (((*it)(1,2)*m3P_hat - (*it)(2,2)*m2P_hat) / (m3P_hat*m3P_hat));
                }

                // return the Jacobian Matrix
                return J;
            }

            /**
             * @brief Compute L2 reprojection error of an estimated 3d point in multiple cameras
             *
             * @param[in] point3d estimated 3d point
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             * @param[out] errors reprojection error in 'n' cameras (n x 2)
             */
            Eigen::MatrixXd reprojectionErrorL2(
                                    const Eigen::MatrixXd &point3d,
                                    const std::vector<tapl::Point2d> &points2d,
                                    const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // reprojection errors
                Eigen::MatrixXd errors(projectionMatrices.size()*2,1);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    // project to camera plane
                    Eigen::MatrixXd point2d_homogeneous(1,3);
                    point2d_homogeneous = (*it) * point3d_homogeneous;
                    Eigen::MatrixXd point2d_euclidean(2,1);
                    point2d_euclidean << (point2d_homogeneous(0,0) / point2d_homogeneous(2,0)),
                                         (point2d_homogeneous(1,0) / point2d_homogeneous(2,0));
                    // compute reprojection error
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    Eigen::MatrixXd reprojection_err(2,1);
                    Eigen::MatrixXd points2d_eigen(2,1);
                    points2d_eigen << points2d[cam_idx].x, points2d[cam_idx].y;
                    reprojection_err << (point2d_euclidean - points2d_eigen)*(point2d_euclidean - points2d_eigen);
                    errors.block<2,1>(2*cam_idx,0) = reprojection_err;
                }
                // return reprojection errors
                return errors;
            }

            /**
             * @brief Given a 3D point and its corresponding points in the image
             *         planes, compute the associated Jacobian for L2 reprojection errors
             *
             * @param[in] point3d estimated 3d point
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             *
             * @return J Jacobian Matrix
             */
            const Eigen::MatrixXd jacobianL2(
                            const Eigen::MatrixXd &point3d,
                            const std::vector<tapl::Point2d> &points2d,
                            const std::vector<Eigen::MatrixXd> &projectionMatrices) {

                // build homogeneous point
                Eigen::MatrixXd point3d_homogeneous(4,1);
                point3d_homogeneous.block<3,1>(0,0) << point3d;
                point3d_homogeneous.block<1,1>(3,0) << 1.0;

                // jacobian matrix
                Eigen::MatrixXd J(projectionMatrices.size()*2,3);
                // compute L1 reprojection error
                Eigen::MatrixXd reprErr = reprojectionError(point3d, points2d, projectionMatrices);
                // go through each camera
                for (auto it=projectionMatrices.begin();
                          it!=projectionMatrices.end();
                          ++it) {
                    double m1P_hat = ((*it).block<1,4>(0,0) * point3d_homogeneous)(0,0);
                    double m2P_hat = ((*it).block<1,4>(1,0) * point3d_homogeneous)(0,0);
                    double m3P_hat = ((*it).block<1,4>(2,0) * point3d_homogeneous)(0,0);

                    // build Jacobian
                    auto cam_idx = std::distance(projectionMatrices.begin(), it);
                    J.block<1,3>(2*cam_idx,0) << 2*reprErr(2*cam_idx,0)*(((*it)(0,0)*m3P_hat - (*it)(2,0)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 2*reprErr(2*cam_idx,0)*(((*it)(0,1)*m3P_hat - (*it)(2,1)*m1P_hat) / (m3P_hat*m3P_hat)),
                                                 2*reprErr(2*cam_idx,0)*(((*it)(0,2)*m3P_hat - (*it)(2,2)*m1P_hat) / (m3P_hat*m3P_hat));
                    J.block<1,3>((2*cam_idx)+1,0) << 2*reprErr(2*cam_idx+1,0)*(((*it)(1,0)*m3P_hat - (*it)(2,0)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     2*reprErr(2*cam_idx+1,0)*(((*it)(1,1)*m3P_hat - (*it)(2,1)*m2P_hat) / (m3P_hat*m3P_hat)),
                                                     2*reprErr(2*cam_idx+1,0)*(((*it)(1,2)*m3P_hat - (*it)(2,2)*m2P_hat) / (m3P_hat*m3P_hat));
                }

                // return the Jacobian Matrix
                return J;
            }

            /**
             * @brief Given a 3D point and its corresponding points in the image
             *         planes, compute the associated Jacobian
             *
             * @param[in] point3d initial estimate of the 3d point
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras (n x 3 x 4)
             * @param[in] nIterations projection matrices of 'n' cameras (n x 3 x 4)
             * @param[out] optimPoint3d optimized point 3d
             *
             * @return pair of pre-optimization and post-optimization reprojection error norms
             */
             std::pair<std::vector<float>,std::vector<float>> optimize(
                        const tapl::Point3d &point3d,
                        const std::vector<tapl::Point2d> &points2d,
                        const std::vector<Eigen::MatrixXd> &projectionMatrices,
                        const uint16_t nIterations,
                        const float reprErrorThresh,
                        tapl::Point3d &optimPoint3d) {

                Eigen::MatrixXd point3d_eigen(3,1);
                point3d_eigen << point3d.x, point3d.y, point3d.z;
                std::vector<float> preOptimReprErrNorm(points2d.size());
                std::vector<float> postOptimReprErrNorm(points2d.size());
                // log reprojection errors
                Eigen::MatrixXd errPreOptim = reprojectionErrorL2(point3d_eigen, points2d, projectionMatrices);
                for (auto i=0; i<points2d.size(); ++i) {
                    preOptimReprErrNorm.at(i) = sqrt( errPreOptim(2*i,0)*errPreOptim(2*i,0) +
                                                      errPreOptim(2*i+1,0)*errPreOptim(2*i+1,0));
                }
                // start optimization
                for (auto n=0; n<nIterations; ++n) {
                    errPreOptim =
                        reprojectionErrorL2(point3d_eigen, points2d, projectionMatrices);
                    auto errPreOptimNorm = sqrt( errPreOptim(0,0)*errPreOptim(0,0) +
                                                 errPreOptim(1,0)*errPreOptim(1,0));
                    if (errPreOptimNorm < reprErrorThresh) {break;}
                    auto J = jacobianL2(point3d_eigen, points2d, projectionMatrices);
                    point3d_eigen = point3d_eigen - (((J.transpose() * J).inverse()) * J.transpose() * errPreOptim);
                }
                Eigen::MatrixXd errPostOptim = reprojectionErrorL2(point3d_eigen, points2d, projectionMatrices);
                for (auto i=0; i<points2d.size(); ++i) {
                    postOptimReprErrNorm.at(i) = sqrt( errPostOptim(2*i,0)*errPostOptim(2*i,0) +
                                                       errPostOptim(2*i+1,0)*errPostOptim(2*i+1,0));
                }

                // optimized point
                optimPoint3d = *(new tapl::Point3d(point3d_eigen(0), point3d_eigen(1), point3d_eigen(2)));

                // return optimization errors
                return std::make_pair(preOptimReprErrNorm, postOptimReprErrNorm);
            }
        };
    }
}

#endif /* GAUSS_NEWTON_H_ */


// simple feed-forward neural network
#pragma once
#include <Eigen/Core>

using Eigen::Matrix;

// MatrixXd Sigmoid(MatrixXd const& x) {
//   return 1.0 / (1.0 + (-x).array().exp());
// }

inline float relu(float x) {
  return (x > 0) ? x : 0;
}

// measured 2x overall slowdown when using MatrixXf instead of template
template <int n_inputs, int n_hidden, int n_outputs>
class SmallNN {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Matrix<float, n_hidden, n_inputs> w0;
  Matrix<float, n_hidden, 1> b0;
  Matrix<float, n_outputs, n_hidden> w1;
  Matrix<float, n_outputs, 1> b1;

  Matrix<float, n_outputs, 1> predict(Matrix<float, n_inputs, 1> inputs) {
    Matrix<float, n_hidden, 1> a1 = w0 * inputs + b0;
    for (int i=0; i<n_hidden; i++) a1(i) = relu(a1(i));  // faster than unaryExpr
    Matrix<float, n_outputs, 1> a2 = w1 * a1 + b1;

    // somewhat hacky residual connection, but it measurably helps
    a2 *= 0.1;
    static_assert(n_outputs <= n_hidden, "n_hidden must be >= n_outputs");
    for (int i=0; i<n_outputs; i++) {
      a2(i) += a1(i);
    }
    a2 *= 1.0 / (1.0 + 0.1);

    return a2;
  }
};


#ifndef CANNON_ML_WOLFE_CONDITIONS_H
#define CANNON_ML_WOLFE_CONDITIONS_H

/*!
 * \file cannon/ml/wolfe_conditions.hpp
 * \brief File containing utility functions relating to line search and
 * sufficient decrease conditions.
 */

#include <functional>

#include <Eigen/Dense>

using namespace Eigen;

using RealFunc = std::function<double(const VectorXd&)>;
using MultiFunc = std::function<VectorXd(const VectorXd&)>;

namespace cannon {
  namespace ml {

    /*!
     * \brief Compute whether the input step along the input direction
     * satisfies the Armijo sufficient decrease condition with respect to the
     * function under consideration.
     *
     * \param f The function being optimized
     * \param f_grad Gradient of the function being optimized
     * \param x Current state from which to compute sufficient decrease
     * \param step Step along direction from x
     * \param direction Optimization direction
     * \param c_1 Sufficient decrease parameter
     *
     * \returns Whether the sufficient decrease condition is satisfied.
     */
    bool sufficient_decrease_condition(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, double step, const VectorXd& direction, double c_1);

    /*!
     * \brief Compute whether the input step along the input direction
     * satisfies the curvature condition.
     *
     * \param f The function being optimized
     * \param f_grad Gradient of the function being optimized
     * \param x Current state from which to compute sufficient decrease
     * \param step Step along direction from x
     * \param direction Optimization direction
     * \param c_2 Curvature parameter
     */
    bool curvature_condition(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, double step, const VectorXd& direction, double c_2);

    /*!
     * \brief Compute whether both Wolfe conditions are satisfied by the input
     * step along the input direction.
     *
     * \param f The function being optimized
     * \param f_grad Gradient of the function being optimized
     * \param x Current state from which to compute sufficient decrease
     * \param step Step along direction from x
     * \param direction Optimization direction
     * \param c_1 Sufficient decrease parameter
     * \param c_2 Curvature parameter
     */
    bool wolfe_conditions(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, double step, const VectorXd& direction, double c_1,
        double c_2);

    /*!
     * \brief Compute the optimal line search step between the input minimum
     * and maximum along the input direction.  Note that this is the minimizing
     * step.
     *
     * \param f The function to be minimized.
     * \param f_grad Gradient function for the function to be minimized.
     * \param x State to conduct line search from
     * \param direction Direction for line search
     * \param start_low Initial minimal step
     * \param start_high Initial maximal step
     * \param c_1 Sufficient decrease parameter
     * \param c_2 Curvature parameter
     * \param iterations Maximum number of iterations.
     */
    double line_search_zoom(RealFunc f, MultiFunc f_grad, const VectorXd& x,
        const VectorXd& direction, double start_low, double start_high,
        double c_1, double c_2, unsigned int iterations=100);

    /*!
     * \brief Compute the optimal line search step along the input direction,
     * moving as far as possible while satisfying the Wolfe conditions.  Note
     * that this is the minimizing step.
     *
     * \param f The function to be minimized.
     * \param f_grad Gradient function for the function to be minimized.
     * \param x State to conduct line search from
     * \param direction Direction for line search
     * \param step_1 Initial step to consider
     * \param c_1 Sufficient decrease parameter
     * \param c_2 Curvature parameter
     * \param iterations Maximum number of iterations.
     */
    // Returns optimal step. Note that this is the minimizing step, not the maximizing.
    double wolfe_condition_line_search(RealFunc f, MultiFunc f_grad, const
        VectorXd& x, const VectorXd& direction, double step_1 = 1.0, double c_1 = 1e-4,
        double c_2 = 0.5, unsigned int iterations=100);

  } // namespace ml
} // namespace cannon

#endif /* ifndef CANNON_ML_WOLFE_CONDITIONS_H */


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::det;
using arma::log_det;
using arma::trace;
using arma::diagmat;
using arma::symmatu;
using arma::symmatl;
using arma::trimatu;
using arma::trimatl;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedSquMat : public Expected {
    public:
      ExpectedSquMat() {
        cout << "Compute ExpectedSquMat(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::SquMat
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _squMat = *static_cast<Mat<double>*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            expectedArmaDet();
            expectedArmaLog_det();
            expectedArmaTrace();
            expectedArmaDiagmat();
            expectedArmaSymmatu();
            expectedArmaSymmatl();
            expectedArmaTrimatu();
            expectedArmaTrimatl();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _squMat;

      void expectedArmaDet() {
        cout << "- Compute expectedArmaDet() ... ";
        save<double>("Arma.det", Mat<double>({det(_squMat)}));
        cout << "done." << endl;
      }

      void expectedArmaLog_det() {
        cout << "- Compute expectedArmaLog_det() ... ";

        double val, sign;

        log_det(val, sign, _squMat);

        save<double>("Arma.log_detVal", Mat<double>({val}));
        save<double>("Arma.log_detSign", Mat<double>({sign}));

        cout << "done." << endl;
      }

      void expectedArmaTrace() {
        cout << "- Compute expectedArmaTrace() ... ";
        save<double>("Arma.trace", Mat<double>({trace(_squMat)}));
        cout << "done." << endl;
      }

      void expectedArmaDiagmat() {
        cout << "- Compute expectedArmaDiagmat() ... ";
        save<double>("Arma.diagmat", diagmat(_squMat));
        cout << "done." << endl;
      }

      void expectedArmaSymmatu() {
        cout << "- Compute expectedArmaSymmatu() ... ";
        save<double>("Arma.symmatu", symmatu(_squMat));
        cout << "done." << endl;
      }

      void expectedArmaSymmatl() {
        cout << "- Compute expectedArmaSymmatl() ... ";
        save<double>("Arma.symmatl", symmatl(_squMat));
        cout << "done." << endl;
      }

      void expectedArmaTrimatu() {
        cout << "- Compute expectedArmaTrimatu() ... ";
        save<double>("Arma.trimatu", trimatu(_squMat));
        cout << "done." << endl;
      }

      void expectedArmaTrimatl() {
        cout << "- Compute expectedArmaTrimatl() ... ";
        save<double>("Arma.trimatl", trimatl(_squMat));
        cout << "done." << endl;
      }
  };
}


#include "RBGL.hpp"
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

extern "C"
{

    SEXP BGL_KMST_D( SEXP num_verts_in, SEXP num_edges_in,
                     SEXP R_edges_in, SEXP R_weights_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);
        property_map < Graph_dd, edge_weight_t >::type weight = get(edge_weight, g);

        std::vector < Edge > spanning_tree;

        kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

        SEXP ansList, ans, answt;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(ans = allocMatrix(INTSXP,2,spanning_tree.size()));
        PROTECT(answt = allocMatrix(REALSXP,1,spanning_tree.size()));
        int k = 0, j = 0;

        for (std::vector < Edge >::iterator ei = spanning_tree.begin();
                ei != spanning_tree.end(); ++ei)
        {
            INTEGER(ans)[k++] = source(*ei,g);
            INTEGER(ans)[k++] = target(*ei,g);
            REAL(answt)[j++] = weight[*ei];
        }

        SET_VECTOR_ELT(ansList,0,ans);
        SET_VECTOR_ELT(ansList,1,answt);
        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_KMST_U( SEXP num_verts_in, SEXP num_edges_in,
                     SEXP R_edges_in, SEXP R_weights_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);
        property_map < Graph_ud, edge_weight_t >::type weight = get(edge_weight, g);

        std::vector < Edge > spanning_tree;

        kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

        SEXP ansList, ans, answt;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(ans = allocMatrix(INTSXP,2,spanning_tree.size()));
        PROTECT(answt = allocMatrix(REALSXP,1,spanning_tree.size()));

        int k = 0, j = 0;
        for (std::vector < Edge >::iterator ei = spanning_tree.begin();
                ei != spanning_tree.end(); ++ei)
        {
            INTEGER(ans)[k++] = source(*ei,g);
            INTEGER(ans)[k++] = target(*ei,g);
            REAL(answt)[j++] = weight[*ei];
        }

        SET_VECTOR_ELT(ansList,0,ans);
        SET_VECTOR_ELT(ansList,1,answt);
        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_PRIM_U( SEXP num_verts_in, SEXP num_edges_in,
                     SEXP R_edges_in, SEXP R_weights_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

	int NV = asInteger(num_verts_in);
        std::vector <Vertex> parent(NV);

        prim_minimum_spanning_tree(g, &parent[0]);

        property_map<Graph_ud, edge_weight_t>::type weight = get(edge_weight, g);

        SEXP ansList, ans, answt;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(ans = allocMatrix(INTSXP,2,NV));
        PROTECT(answt = allocMatrix(REALSXP,1,NV));

        int k = 0, j = 0;
        for (unsigned int v = 0; v < num_vertices(g); ++v)
	{
            INTEGER(ans)[k++] = parent[v];
            INTEGER(ans)[k++] = v;
            REAL(answt)[j++] = ( parent[v] == v) ?
			 0 : get(weight, edge(parent[v], v, g).first);
	}

        SET_VECTOR_ELT(ansList,0,ans);
        SET_VECTOR_ELT(ansList,1,answt);
        UNPROTECT(3);
        return(ansList);
    }

}


#include <gtest/gtest.h>
#include <stdexcept>
#include <cmath>
#include <Eigen/Dense>

// testing following API
#include "probability/sampling.h"

using namespace Eigen;
using namespace probability;

namespace SamplingTest
{
  void sampleNormalAndApproximate(VectorXd mean, MatrixXd covariance)
  {
    int N = 10000;

    // sum up for approximating the mean and variance
    VectorXd meanApprox = VectorXd::Zero(mean.size());
    MatrixXd covarianceApprox = MatrixXd::Zero(covariance.rows(), covariance.cols());

    // create N samples
    for (int i = 0; i < N; i++)
    {
      VectorXd sample = sampleNormalDistribution(mean, covariance);

      // accumulate for mean
      meanApprox = meanApprox + sample;

      // accumulate for variance (take true mean for calculation
      // otherwise we would have to save the samples)
      covarianceApprox = covarianceApprox + (sample - mean)*((sample - mean).transpose());
    }

    meanApprox = meanApprox / N;
    covarianceApprox = covarianceApprox / N;

    for (int i = 0; i < mean.size(); i++)
    {
      EXPECT_NEAR(mean[i], meanApprox[i], 0.1);
      for (int j = 0; j < covariance.cols(); j++)
    	EXPECT_NEAR(covariance(i,j), covarianceApprox(i,j), 0.2);
    }
  }

  VectorXd meanOfUniform(VectorXd a, VectorXd b)
  {
    int N = 1000;

    // sum up for approximating the mean and variance
    VectorXd meanApprox = VectorXd::Zero(a.size());

    // create N samples
    for (int i = 0; i < N; i++)
    {
      VectorXd sample = sampleUniformDistribution(a, b);
      meanApprox = meanApprox + sample;	// accumulate for mean
    }

    meanApprox = meanApprox / N;

    return meanApprox;
  }

  // -----------------------------------------
  // tests
  // -----------------------------------------
  TEST(SamplingTest, sampleNormalDistribution)
  {
    // random vector: size = 1 (like a random variable) ---------------
    VectorXd mean1(1);
    mean1 << 0;
    MatrixXd variance1(1,1);
    variance1 << 1;
    EXPECT_NO_THROW(sampleNormalAndApproximate(mean1, variance1));

    // random vector: size = 3 ----------------------------------------
    VectorXd mean2(3);
    mean2 << -1, 0, 1;
    MatrixXd covariance2(3,3);
    covariance2 <<
      0.5, 0, 0,
        0, 1, 0,
        0, 0, 2;
    EXPECT_NO_THROW(sampleNormalAndApproximate(mean2, covariance2));

    // random vector: size = 3, non-diagonal --------------------------
    VectorXd mean3(3);
    mean3 << -5, 1, 100;
    MatrixXd covariance3(3,3);
    covariance3 <<
        3, 0.1, 0.1,
      0.1,   1, 0.9,
      0.1, 0.9,   2;
    EXPECT_NO_THROW(sampleNormalAndApproximate(mean3, covariance3));

    // random vector: size = 3, non-diagonal, NOT positive definite! --
    VectorXd mean4(2);
    mean4 << -1000, 0;
    MatrixXd covariance4(2,2);
    covariance4 <<
      1, 2,
      2, 1;
    // throws error (cholesky decomposition failed)
    EXPECT_ANY_THROW(sampleNormalAndApproximate(mean4, covariance4));
  }

  TEST(SamplingTest, sampleUniformDistribution)
  {
    // random vector: size = 1 (like a random variable) ---------------
    VectorXd a1(1);
    a1 << 0;
    VectorXd b1(1);
    b1 << 1;
    VectorXd meanApprox;
    EXPECT_NO_THROW(meanApprox = meanOfUniform(a1,b1));
    for (int i = 0; i < meanApprox.size(); i++)
      EXPECT_NEAR((a1[i]+b1[i])/2, meanApprox[i], 0.2);

    // random vector: size = 2 ----------------------------------------
    VectorXd a2(2);
    a2 << -3, 0;
    VectorXd b2(2);
    b2 << 3, 2;
    EXPECT_NO_THROW(meanApprox = meanOfUniform(a2, b2));
    for (int i = 0; i < meanApprox.size(); i++)
      EXPECT_NEAR((a2[i]+b2[i])/2, meanApprox[i], 0.2);

    // check if really random!
    VectorXd meanApprox1 = meanOfUniform(a2, b2);
    VectorXd meanApprox2 = meanOfUniform(a2, b2);
    EXPECT_GT(fabs(meanApprox1[0] - meanApprox2[0]), 0.000001);
  }
}


//==================================================
// newton
//==================================================
#include <iostream>
#include <valarray>
#include <boost/function.hpp>
/*
 ◆開発環境
 [OS]: macOS 10.14~
 [c++] gcc version 9.2.0
 [boost] 1.71.0

 */
// テスト用パラメータークラス
class Param
{
    public:
        double t=0;
        double x=0;
        double y=0;
};

class Newton
{
    private:
        double f( double t, double ox, boost::function<double(double, Param)> method, Param _param )
        {
            double x;
            double _dif;
            x = method(t, _param);
            _dif = ox - x;
            return _dif;
        }

        double df( double t, double ox, boost::function<double(double, Param)> method, Param _param )
        {
            double x;
            double h;
            double dx;
            h = 0.00001;
            x = this->f(t, ox, method, _param);
            dx = this->f(t+h, ox, method, _param);
            return (dx-x)/h;
        }

    public:
        double newton( double initval, double ox, boost::function<double(double, Param)> method, Param _param , int count=1000000000, double ep=0.0001)
        {
            /*
             oxに最も近づくt値を近似する。epは収束時の許容誤差
             */
            double t2;
            double rf;
            double rdf;
            double t=initval;
            for (int i=0; i<count; i++)
                {
                    rf = this->f(t, ox, method, _param);
                    rdf = this->df(t, ox, method, _param);
                    std::cout<<"t:  " << t <<",   ox: "<< ox << ",  rf: " << rf << ",    rdf: " << rdf << std::endl;
                    if (rf ==0.0 || rdf==0.0)
                        {
                            return 0;
                        }
                    t2 = t - rf / rdf;
                    if (std::abs(t2-t) < ep)
                        {
                            t=t2;
                            break;
                        }else{
                            t=t2;
                            continue;
                        }
                        return t2;
                }
         return t2;
        }
};

// 動作テスト用
double testMethod(double t, Param _param)
{
    double result;
    result = (_param.x*_param.x*t / (_param.y/2)) *(t*t);
    return result;
};

int main()
{
    double t;
    Param test_param;
    test_param.t = 0.0;
    test_param.x = 3.0;
    test_param.y = 4.0;
    Newton nt;
    t = nt.newton(0.001, 200.34, testMethod, test_param, 1000000000, 0.0001);
    std::cout<<"result:  " <<testMethod(t, test_param)<< std::endl;
    std::cout<<"t:  " <<t<< std::endl;
    return 0;
}


#include <iostream>
#include <numeric>
#include <type.hpp>

#include <Eigen/Eigen>

#include "FEM/FEM.hpp"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"

static Float LinearInnerProduct(Float xmin, Float x_mid, Float xmax)
{
	return ((-2 + x_mid * x_mid + xmax - x_mid * (1 + xmax)) * cos(x_mid) + 2 * cos(xmax) + (1 - 2 * x_mid + xmax) * sin(x_mid) + (-1 + xmax) * sin(xmax))
		/ (x_mid - xmax) + 1 / (x_mid - xmin) * ((2 + x_mid - x_mid * x_mid + (-1 + x_mid) * xmin) * cos(x_mid) - 2 * cos(xmin) - (1 - 2 * x_mid + xmin) * sin(x_mid) + sin(xmin) - xmin * sin(xmin));
}

static Float LinearFunc(Float x, Float x0, Float x_left, Float x_right)
{
	if (x<x_left || x>x_right)
	{
		return 0;
	}
	if (x < x0)
	{
		return (x - x_left) / (x0 - x_left);
	}
	else return (x - x_right) / (x0 - x_right);
}

class LinearBaseFEM1D :public StaticFEM1D
{
public:
	LinearBaseFEM1D(size_t size) : StaticFEM1D(), size(size)
	{
		mesh.resize(size);
		//A default devision is provided
		Float h = 1.0 / (size + 1);
		for (int i = 0; i < size; ++i)
		{
			mesh[i] = (i + 1) * h;
		}
	}

	std::vector<Float> mesh;

	Float Value(Float x) override
	{
		Float ret = 0;
		for (int i = 0; i < mat_size; ++i)
		{
			ret += LinearFunc(x, MeshValue(i), MeshValue(i - 1), MeshValue(i + 1)) * coeff_()(i);
		}
		return ret;
	}

private:

	//return the indices of the functions related to the current function
	std::vector<int> RelatedFuncIdx(int idx) override
	{
		std::vector<int> ret;
		ret.push_back(idx);
		int left = idx - 1, right = idx + 1;
		if (0 <= left && left < mat_size)
		{
			ret.push_back(left);
		}
		if (0 <= right && right < mat_size)
		{
			ret.push_back(right);
		}
		return ret;
	}

	Float MeshValue(int index)
	{
		if (index < 0)
		{
			return 0;
		}
		if (index >= mat_size)
		{
			return  1.0;
		}
		return mesh[index];
	}

	Float GradientInnerProduct(int i, int j) override
	{
		if (i == j)
		{
			return 1.0 / (MeshValue(i) - MeshValue(i - 1)) + 1.0 / (MeshValue(i + 1) - MeshValue(i));
		}
		else if (abs(i - j) == 1)
		{
			return -1.0 / abs(MeshValue(i) - MeshValue(j));
		}
		else return 0;//For robustness
	}

	Float RHSInnerProduct(int i) override
	{
		return LinearInnerProduct(MeshValue(i - 1), MeshValue(i), MeshValue(i + 1));
	}

protected:

	int size;

	Float SelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	Float GradientSelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	void SetMatSize() override
	{
		mat_size = size;
	}
};

static Float Quadratic(Float x, Float x_left, Float x_middle, Float x_right)
{
	if (x <= x_left || x >= x_right)
	{
		return 0.0;
	}

	return   (x - x_left) * (x - x_right) / (x_middle - x_left) / (x_middle - x_right);
}

static Float QuadraticInnerProduct(Float xmin, Float xmid, Float xmax)
{
	//return (2. * (1. - 2. * xmax + xmin) * cos(xmax) - 2. * (1. + xmax - 2. * xmin) * cos(xmin) + 6. * (sin(xmax) - sin(xmin)) - (xmax - xmin) * (xmax*sin(xmax) - sin(xmax) + xmin*sin(xmin) - sin(xmin))) / (xmax - xmid) / (xmid - xmin);
	return  (2 * cos(xmax) - 4 * xmax * cos(xmax) + 2 * xmin * cos(xmax) - 2 * cos(xmin) - 2 * xmax * cos(xmin) +
		4 * xmin * cos(xmin) + 6 * sin(xmax) + xmax * sin(xmax) - (xmax * xmax) * sin(xmax) -
		xmin * sin(xmax) + xmax * xmin * sin(xmax) - 6 * sin(xmin) + xmax * sin(xmin) -
		xmin * sin(xmin) - xmax * xmin * sin(xmin) + (xmin * xmin) * sin(xmin)) /
		((xmax - xmid) * (xmid - xmin));
}

static Float QuadraticGradientInnerProduct(Float xmin, Float xmid, Float xmax, Float xmin2, Float xmid2, Float xmax2)
{
	auto left = std::max(xmin, xmin2);
	auto right = std::min(xmax, xmax2);

	if (left > right)
	{
		return  0;
	}
	Float ret = -((left - right) * (4 * (left * left) + 4 * left * right + 4 * (right * right) - 3 * left * xmax -
		3 * right * xmax - 3 * left * xmax2 - 3 * right * xmax2 + 3 * xmax * xmax2 - 3 * left * xmin -
		3 * right * xmin + 3 * xmax2 * xmin - 3 * left * xmin2 - 3 * right * xmin2 + 3 * xmax * xmin2 +
		3 * xmin * xmin2)) / 3. / (xmax - xmid) / (xmax2 - xmid2) / (xmid - xmin) / (xmid2 - xmin2);

	return ret;
}

class QuadraticBaseFEM1D :public StaticFEM1D
{
public:
	QuadraticBaseFEM1D(size_t size) : StaticFEM1D(), mesh_size(size), mat_size_(2 * size + 1)
	{
		mesh.resize(size);
		//A default division is provided
		Float h = 1.0 / (size + 1);
		for (int i = 0; i < size; ++i)
		{
			mesh[i] = (i + 1) * h;
		}
	}

	std::vector<Float> mesh;
	size_t mesh_size;
	size_t mat_size_;

	Float Value(Float x) override
	{
		Float ret = 0;
		for (int i = 0; i < mat_size; ++i)
		{
			Float xmin, xmid, xmax;
			idx_to_mesh(i, xmin, xmid, xmax);

			ret += Quadratic(x, xmin, xmid, xmax) * coeff_()(i);
		}
		return ret;
	}

private:

	//return the indices of the functions related to the current function
	std::vector<int> RelatedFuncIdx(int idx) override
	{
		std::vector<int> ret;
		if (idx < mesh_size)
		{
			ret.push_back(idx);
			int left = idx - 1, right = idx + 1;
			if (0 <= left && left < mesh_size)
			{
				ret.push_back(left);
			}
			if (0 <= right && right < mesh_size)
			{
				ret.push_back(right);
			}
			ret.push_back(idx + mesh_size);

			ret.push_back(idx + mesh_size + 1);
		}
		else
		{
			ret.push_back(idx);
			int left = idx - mesh_size - 1, right = idx - mesh_size;

			if (0 <= left && left < mesh_size)
			{
				ret.push_back(left);
			}
			if (0 <= right && right < mesh_size)
			{
				ret.push_back(right);
			}
		}
		return ret;
	}

	Float MeshValue(int index)
	{
		if (index < 0)
		{
			return 0;
		}
		if (index >= mesh_size)
		{
			return  1.0;
		}
		return mesh[index];
	}

	void idx_to_mesh(int idx, Float& xmin, Float& xmid, Float& xmax)
	{
		if (idx < mesh_size)
		{
			xmin = MeshValue(idx - 1);	xmid = MeshValue(idx);	xmax = MeshValue(idx + 1);
		}
		else
		{
			xmin = MeshValue(idx - 1 - mesh_size);
			xmid = (MeshValue(idx - mesh_size) + MeshValue(idx - 1 - mesh_size)) / 2.0;
			xmax = MeshValue(idx - mesh_size);
		}
	}

	Float GradientInnerProduct(int i, int j) override
	{
		Float xmin, xmid, xmax;
		Float xmin2, xmid2, xmax2;

		idx_to_mesh(i, xmin, xmid, xmax);
		idx_to_mesh(j, xmin2, xmid2, xmax2);

		return QuadraticGradientInnerProduct(xmin, xmid, xmax, xmin2, xmid2, xmax2);
	}

	Float RHSInnerProduct(int i) override
	{
		Float xmin, xmid, xmax;

		idx_to_mesh(i, xmin, xmid, xmax);
		return QuadraticInnerProduct(xmin, xmid, xmax);
	}

protected:
	Float SelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	Float GradientSelfInnerProduct(int i, int j) override
	{
		return 0;
	}

	void SetMatSize() override
	{
		mat_size = mat_size_;
	}
};

class FEM1DVisualizer :public Visualizer
{
protected:

	void evaluate()
	{
		int segement_ = segemnt;
		LinearBaseFEM1D linear(segement_);
		QuadraticBaseFEM1D quadratic(segement_ / 2);
		linear.evaluate();
		quadratic.evaluate();
		for (int i = 0; i < Length; ++i)
		{
			quadratic_val[i] = quadratic.Value(1.0 / (Length - 1) * i);
			linear_val[i] = linear.Value(1.0 / (Length - 1) * i);

			quadratic_diff[i] = quadratic_val[i] - precise_val[i];
			linear_diff[i] = linear_val[i] - precise_val[i];
		}
	}

	void AddPoint();
	void DragPoint();
	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	bool updated = true;
	int segemnt = 1;

	void error(std::vector<Float>& ref, std::vector<Float>& eval, Float& L_1, Float& L_2, Float& L_inf)
	{
		assert(ref.size() == eval.size());

		std::vector<Float> minus(ref.size());

		for (int i = 0; i < ref.size(); ++i)
		{
			minus[i] = abs(ref[i] - eval[i]);
		}

		L_inf = *std::max_element(minus.begin(), minus.end(), [](Float a, Float b) {return a < b; });
		L_2 = sqrt(std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a * a; }) / Float(ref.size()));
		L_1 = std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a; }) / Float(ref.size());
	}

public:
	FEM1DVisualizer() {
		Float h = 1.0 / (Length - 1);
		for (int i = 0; i < Length; ++i)
		{
			xs[i] = i * h;
			rhs_f[i] = (xs[i] - 1) * sin(xs[i]);
			precise_val[i] = -(2 - 2 * xs[i] + 2 * xs[i] * cos(1) - 2 * cos(xs[i]) + sin(xs[i]) - xs[i] * sin(xs[i]));
		}
		evaluate();

		Float L1, L2, L_inf;

		segemnt = 1;
		do
		{
			evaluate();

			error(precise_val, linear_val, L1, L2, L_inf);

			pointcount.push_back(segemnt);
			linear_vec_L_1.push_back(L1);
			linear_vec_L_2.push_back(L2);
			linear_vec_L_inf.push_back(L_inf);
			error(precise_val, quadratic_val, L1, L2, L_inf);

			quadratic_vec_L_1.push_back(L1);
			quadratic_vec_L_2.push_back(L2);
			quadratic_vec_L_inf.push_back(L_inf);

			segemnt *= 2;
		} while (segemnt != 1024);
		segemnt = 1;
	}
	const size_t Length = 2001;

	std::vector<Float> xs = std::vector<Float>(Length);
	std::vector<Float> rhs_f = std::vector<Float>(Length);
	std::vector<Float> precise_val = std::vector<Float>(Length);
	std::vector<Float> quadratic_val = std::vector<Float>(Length);
	std::vector<Float> linear_val = std::vector<Float>(Length);
	std::vector<Float> quadratic_diff = std::vector<Float>(Length);
	std::vector<Float> linear_diff = std::vector<Float>(Length);

	std::vector<Float> pointcount;
	std::vector<Float> linear_vec_L_1;
	std::vector<Float> linear_vec_L_2;
	std::vector<Float> linear_vec_L_inf;

	std::vector<Float> quadratic_vec_L_1;
	std::vector<Float> quadratic_vec_L_2;
	std::vector<Float> quadratic_vec_L_inf;
};

void FEM1DVisualizer::AddPoint()
{
	if (ImGui::IsMouseClicked(ImGuiMouseButton_Right))
	{
		auto pos = ImPlot::GetPlotMousePos();
		points.emplace_back(pos.x, pos.y);
		updated = true;
	}
}

void FEM1DVisualizer::DragPoint()
{
	for (int i = 0; i < points.size(); ++i)
	{
		auto& point = points[i];
		updated |= ImPlot::DragPoint(("Point " + std::to_string(i)).c_str(), &point.x(), &point.y());
	}
}
static inline ImVec2 operator-(const ImVec2& lhs, const ImVec2& rhs) { return ImVec2(lhs.x - rhs.x, lhs.y - rhs.y); }
void FEM1DVisualizer::draw(bool* p_open)
{
	if (ImGui::BeginTabBar("Homework 1")) {
		if (ImGui::BeginTabItem("FEM1D"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				//ImPlot::PlotLine("u", &xs[0], &ys1[0], Length);
				ImPlot::PlotLine("Precise solution", &xs[0], &precise_val[0], Length);
				//ImPlot::PlotLine("FEM Result", &xs[0], &ys3[0], Length);
				ImPlot::PlotLine("FEM Quadratic", &xs[0], &quadratic_val[0], Length);
				ImPlot::PlotLine("FEM Linear", &xs[0], &linear_val[0], Length);

				ImPlot::EndPlot();
			}
			if (ImGui::SliderInt("Number of segments", &segemnt, 1, 512))
			{
				evaluate();
			}
			ImGui::EndTabItem();
		}
		if (ImGui::BeginTabItem("FEM1D difference"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				//ImPlot::PlotLine("u", &xs[0], &ys1[0], Length);
				//ImPlot::PlotLine("FEM Result", &xs[0], &ys3[0], Length);
				ImPlot::PlotLine("FEM diff Linear", &xs[0], &linear_diff[0], Length);
				ImPlot::PlotLine("FEM diff Quadratic", &xs[0], &quadratic_diff[0], Length);

				ImPlot::EndPlot();
			}
			if (ImGui::SliderInt("Number of segments", &segemnt, 1, 512))
			{
				segemnt = segemnt < 1 ? 1 : segemnt;
				evaluate();
			}
			ImGui::EndTabItem();
		}

		if (ImGui::BeginTabItem("Error Plot"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus, ImPlotAxisFlags_LogScale, ImPlotAxisFlags_LogScale)) {
				//ImPlot::PlotLine("u", &xs[0], &ys1[0], Length);
				//ImPlot::PlotLine("FEM Result", &xs[0], &ys3[0], Length);
				ImPlot::PlotLine("Linear L1    Error", &pointcount[0], &linear_vec_L_1[0], pointcount.size());
				ImPlot::PlotLine("Linear L2    Error", &pointcount[0], &linear_vec_L_2[0], pointcount.size());
				ImPlot::PlotLine("Linear L_inf Error", &pointcount[0], &linear_vec_L_inf[0], pointcount.size());

				ImPlot::PlotLine("Quadratic L1    Error", &pointcount[0], &quadratic_vec_L_1[0], pointcount.size());
				ImPlot::PlotLine("Quadratic L2    Error", &pointcount[0], &quadratic_vec_L_2[0], pointcount.size());
				ImPlot::PlotLine("Quadratic L_inf Error", &pointcount[0], &quadratic_vec_L_inf[0], pointcount.size());

				ImPlot::EndPlot();
			}
			if (ImGui::SliderInt("Number of segments", &segemnt, 1, 100))
			{
				evaluate();
			}
			ImGui::EndTabItem();
		}
		ImGui::EndTabBar();
	}

	ImGui::End();
}

int main()
{
	FEM1DVisualizer visualizer;
	visualizer.RenderLoop();
}


#include <iostream>
#include <list>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <kv/lp.hpp>

#ifdef TEST_DD
#include <kv/dd.hpp>
#include <kv/rdd.hpp>
typedef kv::dd fl;
#else
typedef double fl;
#endif

namespace ub = boost::numeric::ublas;

int main()
{
	fl r;
	kv::interval<fl> ri;

	ub::vector<fl> objfunc;
	std::list< ub::vector<fl> > constraints;
	ub::vector<fl> tmp;

#ifdef TEST_DD
	std::cout.precision(33);
#else
	std::cout.precision(17);
#endif

	// simple problem

	objfunc.resize(3);
	tmp.resize(3);

	objfunc(0) = 0.;
	objfunc(1) = -2.;
	objfunc(2) = -1.;

	tmp(0) = -5.;
	tmp(1) = 1.;
	tmp(2) = -1.;
	constraints.push_back(tmp);

	tmp(0) = -10.;
	tmp(1) = 1.;
	tmp(2) = 2.;
	constraints.push_back(tmp);

	r = kv::lp_minimize(objfunc, constraints);
	std::cout << r << "\n";

	ri.lower() = kv::lp_minimize_verified(objfunc, constraints, -1);
	ri.upper() = kv::lp_minimize_verified(objfunc, constraints, 1);
	std::cout << ri << "\n";

	// simple problem by akky

	constraints.clear();
	objfunc.resize(4);
	tmp.resize(4);

	objfunc(0) = 0.;
	objfunc(1) = 1.;
	objfunc(2) = 0.;
	objfunc(3) = -1.;

	tmp(0) = -2.;
	tmp(1) = 1.;
	tmp(2) = 0.;
	tmp(3) = 0.;
	constraints.push_back(tmp);

	tmp(0) = 0.;
	tmp(1) = -2.;
	tmp(2) = 1.;
	tmp(3) = 0.;
	constraints.push_back(tmp);

	tmp(0) = -4.;
	tmp(1) = 4.;
	tmp(2) = -1.;
	tmp(3) = 0.;
	constraints.push_back(tmp);

	tmp(0) = 0.;
	tmp(1) = -2.;
	tmp(2) = 0.;
	tmp(3) = 1.;
	constraints.push_back(tmp);

	tmp(0) = 0.;
	tmp(1) = -4.;
	tmp(2) = 2.;
	tmp(3) = 1.;
	constraints.push_back(tmp);

	tmp(0) = -4.;
	tmp(1) = 6.;
	tmp(2) = -2.;
	tmp(3) = -1.;
	constraints.push_back(tmp);

	r = kv::lp_minimize(objfunc, constraints);
	std::cout << r << "\n";

	ri.lower() = kv::lp_minimize_verified(objfunc, constraints, -1);
	ri.upper() = kv::lp_minimize_verified(objfunc, constraints, 1);
	std::cout << ri << "\n";
}


#include "BSpline.h"

__pragma(warning(push, 0))
#include <Eigen/Eigen>
__pragma(warning(pop))

#include <iostream>

#include <omp.h>

namespace Chaf
{
	BSpline::BSpline()
	{
		m_T = { 0, 0, 0 };
	}

	double BSpline::genBasis(const std::vector<double>& T, double t, size_t i, size_t k)
	{
		if (k == 1)
		{
			if ((t >= T[i] && t < T[i + 1]) || (t >= T[i] && t <= T[i + 1] && T[i + 1] == T.back()))
			{
				return 1.0;
			}
			else
			{
				return 0.0;
			}
		}
		double left = 0.0;
		left = T[i + k - 1] - T[i] == 0.0 ? 0.0 : (t - T[i]) / (T[i + k - 1] - T[i]) * genBasis(T, t, i, k - 1);

		double right = 0.0;
		right = T[i + k] - T[i + 1] == 0.0 ? 0.0 : (T[i + k] - t) / (T[i + k] - T[i + 1]) * genBasis(T, t, i + 1, k - 1);

		return  left + right;
	}

	void BSpline::addPoint(const ImPlotPoint& p, PointType type)
	{
		Types.push_back(type);
		Control_Points_X.push_back(p.x);
		Control_Points_Y.push_back(p.y);
	}

	void BSpline::genDeBoorPoints(std::vector<double>& x, std::vector<double>& y)
	{
		std::vector<size_t> indices = { 1 };
		std::vector<double> s;
		for (size_t i = 0; i < Types.size(); i++)
		{
			indices.push_back(Types[i] + indices.back());
			s.push_back(static_cast<double>(i));
		}

		m_T.clear();
		m_T.push_back(0);
		m_T.push_back(0);
		m_T.push_back(0);
		for (size_t i = 0; i < Types.size(); i++)
		{
			for (size_t k = 0; k < Types[i]; k++)
			{
				m_T.push_back(static_cast<double>(i));
			}
		}
		m_T.push_back(static_cast<double>(Types.size() - 1));
		m_T.push_back(static_cast<double>(Types.size() - 1));
		m_T.push_back(static_cast<double>(Types.size() - 1));

		size_t n = m_T.size() - 4;

		x.resize(n);
		y.resize(n);
		Eigen::MatrixXd A(n, n);
		Eigen::MatrixXd b(n, 2);

		A.setZero();
		b.setZero();

		// Begin
		switch (Types[0])
		{
		case PointType::C2:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];
			break;
		case PointType::Line:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			A(2, 2) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];

			b(2, 0) = Control_Points_X[0];
			b(2, 1) = Control_Points_Y[0];
			break;
		case PointType::Sharp:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			A(2, 2) = 1;

			A(3, 3) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];

			b(2, 0) = Control_Points_X[0];
			b(2, 1) = Control_Points_Y[0];

			b(3, 0) = Control_Points_X[0];
			b(3, 1) = Control_Points_Y[0];
			break;
		default:
			break;
		}

		// Inner
#pragma omp parallel for
		for (int i = 1; i < Types.size() - 1; i++)
		{
			size_t index = indices[i];

			if (Types[i] == PointType::C2)
			{
				A(index, index - 1) = genBasis(m_T, s[i], index - 1, 4);
				A(index, index) = genBasis(m_T, s[i], index, 4);
				A(index, index + 1) = genBasis(m_T, s[i], index + 1, 4);
				b(index, 0) = Control_Points_X[i];
				b(index, 1) = Control_Points_Y[i];
			}
			else if (Types[i] == PointType::Line)
			{
				if (Types[i - 1] != PointType::Line)
				{
					A(index, index) = 2;
					A(index, index + 1) = -3;
					A(index, index + 2) = 1;

					A(index + 1, index + 1) = 1;

					b(index + 1, 0) = Control_Points_X[i];
					b(index + 1, 1) = Control_Points_Y[i];
				}
				else if (Types[i + 1] != PointType::Line)
				{
					A(index, index) = 1;

					A(index + 1, index - 1) = 1;
					A(index + 1, index) = -3;
					A(index + 1, index + 1) = 2;

					b(index, 0) = Control_Points_X[i];
					b(index, 1) = Control_Points_Y[i];
				}
				else
				{
					A(index, index) = 1;

					A(index + 1, index + 1) = 1;

					b(index, 0) = Control_Points_X[i];
					b(index, 1) = Control_Points_Y[i];

					b(index + 1, 0) = Control_Points_X[i];
					b(index + 1, 1) = Control_Points_Y[i];
				}
			}
			else if (Types[i] == PointType::Sharp)
			{
				A(index, index - 1) = 2;
				A(index, index) = -3;
				A(index, index + 1) = 1;

				A(index + 1, index + 1) = 1;

				A(index + 2, index + 1) = 1;
				A(index + 2, index + 2) = -3;
				A(index + 2, index + 3) = 2;

				b(index + 1, 0) = Control_Points_X[i];
				b(index + 1, 1) = Control_Points_Y[i];
			}
		}

		// End
		switch (Types.back())
		{
		case PointType::C2:
			A(n - 2, n - 3) = 1;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 1) = 2;

			A(n - 1, n - 1) = 1;

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
			break;
		case PointType::Line:
			A(n - 3, n - 3) = 1;

			A(n - 2, n - 3) = 1;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 1) = 2;

			A(n - 1, n - 1) = 1;

			b(n - 3, 0) = Control_Points_X.back();
			b(n - 3, 1) = Control_Points_Y.back();

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
			break;
		case PointType::Sharp:
			A(n - 4, n - 4) = 1;

			A(n - 3, n - 3) = 1;

			A(n - 2, n - 1) = 2;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 3) = 1;

			A(n - 1, n - 1) = 1;

			b(n - 4, 0) = Control_Points_X.back();
			b(n - 4, 1) = Control_Points_Y.back();

			b(n - 3, 0) = Control_Points_X.back();
			b(n - 3, 1) = Control_Points_Y.back();

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
		default:
			break;
		}

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

		Eigen::MatrixXd res = A.colPivHouseholderQr().solve(b);

		for (size_t i = 0; i < x.size(); i++)
		{
			x[i] = res(i, 0);
			y[i] = res(i, 1);
		}
	}

	void BSpline::genBSpline(const std::vector<double>& px, const std::vector<double>& py, std::vector<double>& x, std::vector<double>& y, size_t sample)
	{
		x.resize(sample+1);
		y.resize(sample+1);

#pragma omp parallel for
		for (int i = 0; i <= sample; i++)
		{
			double t = m_T.back() / static_cast<double>(sample) * static_cast<double>(i);
			auto p = genBSpline(px, py, t);
			x[i] = p.x;
			y[i] = p.y;
		}
	}

	void BSpline::clear()
	{
		m_T.clear();
		Control_Points_X.clear();
		Control_Points_Y.clear();
		Types.clear();
	}

	ImPlotPoint BSpline::genBSpline(const std::vector<double>& px, const std::vector<double>& py, double t)
	{
		ImPlotPoint result = { 0.0, 0.0 };
		for (size_t i = 0; i < px.size(); i++)
		{
			auto basis = genBasis(m_T, t, i, 4);
			result.x += basis * px[i];
			result.y += basis * py[i];
		}

		return result;
	}

}


#ifndef HYPSYS1D_POLYNOMIAL_BASIS_HPP
#define HYPSYS1D_POLYNOMIAL_BASIS_HPP

#include <Eigen/Dense>

/// Legendre polynomial basis
class PolynomialBasis {
  public:

    PolynomialBasis(int q) : p(q) {
        set_scaling_factor(1.0);
    }

    PolynomialBasis(int q, double scaling_factor_) : p(q) {
        set_scaling_factor(scaling_factor_);
    }

    void set_scaling_factor(double scaling_factor_) const {
        scaling_factor = scaling_factor_;
    }

    /// Computes the Legendre polynomial basis
    /// at a given reference point xi \in [0,1]
    Eigen::VectorXd operator() (double xi) const;

    /// Computes the derivative of Legendre polynomial basis
    /// at a given reference point xi \in [0,1]
    Eigen::VectorXd deriv (double xi) const;

    int get_degree() const {
        return p;
    }

  private:

    int p;
    mutable double scaling_factor;
};

#endif // HYPSYS1D_POLYNOMIAL_BASIS_HPP