// [[Rcpp::depends(BH)]]
// [[Rcpp::depends(RcppEigen)]]

#include "distribution.h"
#include <boost/math/distributions/logistic.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/cauchy.hpp>
#include <boost/math/distributions/extreme_value.hpp>
#include <boost/math/distributions/students_t.hpp>

using namespace boost::math;
using namespace std;
using namespace Rcpp ;

distribution::distribution(void) {
  // Rcout << "Distribution is being created" << endl;
}

std::string distribution::concatenate(std::string x, std::string level)
{
  return (x + " " +level);
}

// ORDINAL DATA SELECTION

DataFrame my_transpose(DataFrame dat_in){
  Environment base_base("package:base");
  Function my_transpose1 = base_base["t"];
  DataFrame data_out = my_transpose1(dat_in);
  return data_out;
}

NumericMatrix to_dummy1(NumericVector A, CharacterVector levels)
{
  NumericVector cha_to_fact = A;
  CharacterVector levs1 = levels;
  int var_lev = levs1.length();

  // Rcout << var_lev << std::endl;

  NumericMatrix B_Ma(cha_to_fact.length(), var_lev);
  for (int i_1 = 0; i_1 < cha_to_fact.length(); ++i_1){
    int col_ind = cha_to_fact[i_1] - 1;
    B_Ma(i_1, col_ind) = 1;
  }

  // print(B_Ma);

  B_Ma = B_Ma( _ , Range(0,var_lev-2) );

  // if (var_lev != 2){
  //   B_Ma = B_Ma( _ , Range(0,var_lev-2) );
  // } else {
  //   B_Ma = B_Ma( _ , Range(1,var_lev-1) );
  // }
  return B_Ma;
}

List Model_Matrix_or(DataFrame data, Formula formula) {
  Environment stats_env("package:stats");
  Function model_frame = stats_env["model.frame"];
  Function model_matrix = stats_env["model.matrix"];
  DataFrame df_new1 = model_frame(Rcpp::_["formula"] = formula, _["data"] = data);
  SEXP Response = df_new1[0];
  NumericMatrix df_new = model_matrix(df_new1, _["data"] = data);

  return List::create(
    Named("df_new") = df_new,
    Named("Response") = Response
  );

}

List Cat_ref_order(CharacterVector categories_order, SEXP response_categories){
  // Environment base_env("package:base");
  // Function my_unique = base_env["unique"];

  CharacterVector response_categories1 = response_categories;
  // CharacterVector levels = my_unique(response_categories1);

  IntegerVector num_categories_order = seq_len(categories_order.length());

  DataFrame response_neworder = DataFrame::create(  _["num_categories_order"] = num_categories_order );

  response_neworder = my_transpose(response_neworder);
  response_neworder.names() = categories_order;

  String a1;
  CharacterVector a2;
  for(int i = 0; i <response_categories1.length(); i++){
    a1 = response_categories1[i];
    a2 = response_neworder[a1];
    response_categories1[i] = a2[0];
  }

  // print(response_categories1);

  return List::create(
    Named("response_neworder") = response_neworder,
    Named("response_categories2") = response_categories1,
    Named("levels") = categories_order
  );
}

List distribution::All_pre_data_or(Formula formula, DataFrame input_data,
                                   CharacterVector categories_order,
                                   CharacterVector proportional_effect,
                                   std::string threshold){

  Environment base_env("package:base");
  Function my_asnumeric = base_env["as.numeric"];
  Function my_cbind = base_env["cbind"];
  Function my_order = base_env["order"];

  List M_matrix = Model_Matrix_or(input_data, formula);
  List Cat_ref_or_L = Cat_ref_order(categories_order, M_matrix["Response"]);
  NumericMatrix Design = M_matrix["df_new"];

  CharacterVector a1 = Cat_ref_or_L["response_categories2"];

  NumericVector Num_res = my_asnumeric(a1);

  Design = my_cbind(Num_res, Design);

  // Now order dataset with respect to the repsonse variables in the given order
  DataFrame df_tans = my_transpose(Design);
  DataFrame df_tans_2 = Design ;
  NumericVector order_var_sel = my_order(df_tans_2[0]);
  order_var_sel = order_var_sel - 1 ;
  df_tans = df_tans[order_var_sel];

  df_tans_2 = my_transpose(df_tans);

  CharacterVector Levels = Cat_ref_or_L["levels"];

  int N_cats = Levels.length();

  LogicalVector any_alternative_specific = !is_na(proportional_effect); // TRUE IF THERE ARE
  CharacterVector colnames_final_m = df_tans_2.names();

  NumericVector x(colnames_final_m.length());
  if (any_alternative_specific[0]) {
    // x(colnames_final_m.length());
    for(int indi = 0 ; indi < proportional_effect.length(); indi++){
      String var_1 = proportional_effect[indi];
      int indi_var = df_tans_2.findName(var_1);
      x[indi_var] = indi_var;
    }
    colnames_final_m = colnames_final_m[x==0]; // Case where there no proportional effects
  }

  // Now extend
  // Y EXTEND
  NumericVector Response = df_tans_2[0];
  NumericMatrix Response_EXT = to_dummy1(Response, Levels);

  // X EXTEND

  // X COMPLETE

  DataFrame DF_complete_effect = df_tans_2[colnames_final_m];
  NumericMatrix Pre_Design1 = internal::convert_using_rfunction(DF_complete_effect, "as.matrix");
  Eigen::Map<Eigen::MatrixXd> Pre_Design = as<Eigen::Map<Eigen::MatrixXd> >(Pre_Design1);
  Eigen::MatrixXd Iden_Q1 = Eigen::MatrixXd::Identity(N_cats-1,N_cats-1);

  Eigen::MatrixXd X_EXT_COMPLETE;

  if (threshold == "equidistant"){ // eRASE THE LAST TWO COLUMNS ONE CORRESPONDING TO DR AND OTHER TO THE INTERCEPT
    X_EXT_COMPLETE = Eigen::kroneckerProduct(Pre_Design.rightCols(DF_complete_effect.cols() - 2), Iden_Q1).eval();
    colnames_final_m.erase(0);
    colnames_final_m.erase(0);
  } else {
    X_EXT_COMPLETE = Eigen::kroneckerProduct(Pre_Design.rightCols(DF_complete_effect.cols() - 1), Iden_Q1).eval();
    colnames_final_m.erase(0);
  }

  Eigen::MatrixXd Design_Matrix;

  if (any_alternative_specific[0]) {

    // PONER ESA PARTE ACA

    DataFrame DF_proportional_effect = df_tans_2[proportional_effect];
    NumericMatrix Pre_DF_proportional1 = internal::convert_using_rfunction(DF_proportional_effect, "as.matrix");
    Eigen::Map<Eigen::MatrixXd> Pre_DF_proportional2 = as<Eigen::Map<Eigen::MatrixXd> >(Pre_DF_proportional1);

    Eigen::MatrixXd Pre_DF_proportional = Pre_DF_proportional2;

    Eigen::VectorXd Ones = Eigen::VectorXd::Ones(N_cats-1);
    Eigen::MatrixXd X_EXT_PROPORTIONAL = Eigen::kroneckerProduct(Pre_DF_proportional, Ones).eval();

    // TENGO QUE PONER EL IF ACA

    if (threshold == "equidistant"){
      NumericMatrix tJac = my_cbind(1, seq_len(categories_order.length() -1 )-1 );
      Eigen::Map<Eigen::MatrixXd> tJac2 = as<Eigen::Map<Eigen::MatrixXd> >(tJac);
      Eigen::VectorXd Ones1 = Eigen::VectorXd::Ones(Response_EXT.rows());
      Eigen::MatrixXd Threshold_M = Eigen::kroneckerProduct(Ones1, tJac2).eval();
      X_EXT_PROPORTIONAL.conservativeResize(X_EXT_PROPORTIONAL.rows(),X_EXT_PROPORTIONAL.cols()+2);
      X_EXT_PROPORTIONAL.block(0,X_EXT_PROPORTIONAL.cols()-2, X_EXT_PROPORTIONAL.rows(),2) = Threshold_M;
    }

    Design_Matrix.conservativeResize(X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()+X_EXT_PROPORTIONAL.cols());
    Design_Matrix.block(0,0,X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()) = X_EXT_COMPLETE;
    Design_Matrix.block(0,X_EXT_COMPLETE.cols(),X_EXT_COMPLETE.rows(),X_EXT_PROPORTIONAL.cols()) = X_EXT_PROPORTIONAL;

  }else{Design_Matrix = X_EXT_COMPLETE;}

  return List::create(
    Named("Design_Matrix") = Design_Matrix,
    Named("Response_EXT") = Response_EXT,
    Named("Levels") = Levels,
    Named("Complete_effects") = colnames_final_m,
    Named("N_cats") = N_cats
  );
}

// [[Rcpp::export]]
List All_pre_data_or2(Formula formula, DataFrame input_data,
                      CharacterVector categories_order,
                      CharacterVector proportional_effect){

  Environment base_env("package:base");
  Function my_asnumeric = base_env["as.numeric"];
  Function my_cbind = base_env["cbind"];
  Function my_order = base_env["order"];

  List M_matrix = Model_Matrix_or(input_data, formula);
  List Cat_ref_or_L = Cat_ref_order(categories_order, M_matrix["Response"]);
  NumericMatrix Design = M_matrix["df_new"];

  CharacterVector a1 = Cat_ref_or_L["response_categories2"];
  NumericVector Num_res = my_asnumeric(a1);

  Design = my_cbind(Num_res, Design);

  // Now order dataset with respect to the repsonse variables in the given order
  DataFrame df_tans = my_transpose(Design);
  DataFrame df_tans_2 = Design ;
  NumericVector order_var_sel = my_order(df_tans_2[0]);
  order_var_sel = order_var_sel - 1 ;
  df_tans = df_tans[order_var_sel];

  df_tans_2 = my_transpose(df_tans);

  CharacterVector Levels = Cat_ref_or_L["levels"];
  int N_cats = Levels.length();

  LogicalVector any_alternative_specific = !is_na(proportional_effect); // TRUE IF THERE ARE
  CharacterVector colnames_final_m = df_tans_2.names();

  NumericVector x(colnames_final_m.length());
  if (any_alternative_specific[0]) {
    // x(colnames_final_m.length());
    for(int indi = 0 ; indi < proportional_effect.length(); indi++){
      String var_1 = proportional_effect[indi];
      int indi_var = df_tans_2.findName(var_1);
      x[indi_var] = indi_var;
    }
    colnames_final_m = colnames_final_m[x==0]; // Case where there no proportional effects
  }

  // Now extend

  // X COMPLETE
  DataFrame DF_complete_effect = df_tans_2[colnames_final_m];
  NumericMatrix Pre_Design1 = internal::convert_using_rfunction(DF_complete_effect, "as.matrix");
  Eigen::Map<Eigen::MatrixXd> Pre_Design = as<Eigen::Map<Eigen::MatrixXd> >(Pre_Design1);
  Eigen::MatrixXd Iden_Q1 = Eigen::MatrixXd::Identity(N_cats-1,N_cats-1);
  Eigen::MatrixXd X_EXT_COMPLETE = Eigen::kroneckerProduct(Pre_Design.rightCols(DF_complete_effect.cols() - 1), Iden_Q1).eval();

  // X PROPOTIONAL

  Eigen::MatrixXd Design_Matrix;

  // Y EXTEND
  NumericVector Response = df_tans_2[0];
  NumericMatrix Response_EXT = to_dummy1(Response, Levels);

  NumericMatrix tJac = my_cbind(1, seq_len(categories_order.length() -1 )-1 );
  Rcout << tJac << endl;

  NumericMatrix Response_EXT1 = internal::convert_using_rfunction(Response_EXT, "as.matrix");
  NumericMatrix tJac1 = internal::convert_using_rfunction(tJac, "as.matrix");

  Eigen::Map<Eigen::MatrixXd> Response_EXT2 = as<Eigen::Map<Eigen::MatrixXd> >(Response_EXT1);
  Eigen::Map<Eigen::MatrixXd> tJac2 = as<Eigen::Map<Eigen::MatrixXd> >(tJac1);

  // Eigen::MatrixXd MAR = Response_EXT2 * tJac2;

  Eigen::VectorXd Ones1 = Eigen::VectorXd::Ones(Response_EXT2.rows());
  Eigen::MatrixXd MAR = Eigen::kroneckerProduct(Ones1, tJac2).eval();

  if (any_alternative_specific[0]) {

    DataFrame DF_proportional_effect = df_tans_2[proportional_effect];
    NumericMatrix Pre_DF_proportional1 = internal::convert_using_rfunction(DF_proportional_effect, "as.matrix");
    Eigen::Map<Eigen::MatrixXd> Pre_DF_proportional2 = as<Eigen::Map<Eigen::MatrixXd> >(Pre_DF_proportional1);

    Eigen::MatrixXd Pre_DF_proportional = Pre_DF_proportional2;

    // Pre_DF_proportional.conservativeResize(Pre_DF_proportional.rows(),Pre_DF_proportional.cols()+2);
    // Pre_DF_proportional.block(0,Pre_DF_proportional2.cols(),Pre_DF_proportional.rows(),2) = MAR;

    Eigen::VectorXd Ones = Eigen::VectorXd::Ones(N_cats-1);
    Eigen::MatrixXd X_EXT_PROPORTIONAL = Eigen::kroneckerProduct(Pre_DF_proportional, Ones).eval();

    X_EXT_PROPORTIONAL.conservativeResize(X_EXT_PROPORTIONAL.rows(),X_EXT_PROPORTIONAL.cols()+2);
    X_EXT_PROPORTIONAL.block(0,X_EXT_PROPORTIONAL.cols()-2,X_EXT_PROPORTIONAL.rows(),2) = MAR;

    Design_Matrix.conservativeResize(X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()+X_EXT_PROPORTIONAL.cols());
    Design_Matrix.block(0,0,X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()) = X_EXT_COMPLETE;
    Design_Matrix.block(0,X_EXT_COMPLETE.cols(),X_EXT_COMPLETE.rows(),X_EXT_PROPORTIONAL.cols()) = X_EXT_PROPORTIONAL;

  }else{Design_Matrix = X_EXT_COMPLETE;}

  colnames_final_m.erase(0);

  return List::create(
    Named("Design_Matrix") = Design_Matrix,
    Named("Response_EXT") = Response_EXT,
    Named("Levels") = Levels,
    Named("Complete_effects") = colnames_final_m,
    Named("MAR2") = MAR,
    Named("N_cats") = N_cats
  );
}

List distribution::All_pre_data_NEWDATA(Formula formula,
                                        DataFrame NEWDATA,
                                        CharacterVector categories_order,
                                        CharacterVector proportional_effect,
                                        int N_cats){

  Environment base_env("package:base");
  Function my_asnumeric = base_env["as.numeric"];
  Function my_cbind = base_env["cbind"];
  Function my_order = base_env["order"];

  List M_matrix = Model_Matrix_or(NEWDATA, formula);
  // List Cat_ref_or_L = Cat_ref_order(categories_order, M_matrix["Response"]);
  NumericMatrix Design = M_matrix["df_new"];

  // CharacterVector a1 = Cat_ref_or_L["response_categories2"];
  // NumericVector Num_res = my_asnumeric(a1);
  //
  // Design = my_cbind(Num_res, Design);

  // Now order dataset with respect to the repsonse variables in the given order
  // DataFrame df_tans = my_transpose(Design);
  DataFrame df_tans_2 = Design ;

  // NumericVector order_var_sel = my_order(df_tans_2[0]);
  // order_var_sel = order_var_sel - 1 ;
  // df_tans = df_tans[order_var_sel];

  // df_tans_2 = my_transpose(df_tans);

  // CharacterVector Levels = Cat_ref_or_L["levels"];
  // int N_cats = Levels.length();

  LogicalVector any_alternative_specific = !is_na(proportional_effect); // TRUE IF THERE ARE
  CharacterVector colnames_final_m = df_tans_2.names();

  NumericVector x(colnames_final_m.length());
  if (any_alternative_specific[0]) {
    // x(colnames_final_m.length());
    for(int indi = 0 ; indi < proportional_effect.length(); indi++){
      String var_1 = proportional_effect[indi];
      int indi_var = df_tans_2.findName(var_1);
      x[indi_var] = indi_var;
    }
    colnames_final_m = colnames_final_m[x==0]; // Case where there no proportional effects
  }

  // X EXTEND

  // X COMPLETE

  DataFrame DF_complete_effect = df_tans_2[colnames_final_m];

  NumericMatrix Pre_Design1 = internal::convert_using_rfunction(DF_complete_effect, "as.matrix");
  Eigen::Map<Eigen::MatrixXd> Pre_Design = as<Eigen::Map<Eigen::MatrixXd> >(Pre_Design1);
  Eigen::MatrixXd Iden_Q1 = Eigen::MatrixXd::Identity(N_cats-1,N_cats-1);

  Eigen::MatrixXd X_EXT_COMPLETE;

  // if (threshold == "equidistant"){ // eRASE THE LAST TWO COLUMNS ONE CORRESPONDING TO DR AND OTHER TO THE INTERCEPT
  //   X_EXT_COMPLETE = Eigen::kroneckerProduct(Pre_Design.rightCols(DF_complete_effect.cols() - 2), Iden_Q1).eval();
  //   colnames_final_m.erase(0);
  //   colnames_final_m.erase(0);
  // } else {
  X_EXT_COMPLETE = Eigen::kroneckerProduct(Pre_Design, Iden_Q1).eval();

  // colnames_final_m.erase(0);
  // }

  Eigen::MatrixXd Design_Matrix;

  if (any_alternative_specific[0]) {

    // PONER ESA PARTE ACA

    DataFrame DF_proportional_effect = df_tans_2[proportional_effect];
    NumericMatrix Pre_DF_proportional1 = internal::convert_using_rfunction(DF_proportional_effect, "as.matrix");
    Eigen::Map<Eigen::MatrixXd> Pre_DF_proportional2 = as<Eigen::Map<Eigen::MatrixXd> >(Pre_DF_proportional1);

    Eigen::MatrixXd Pre_DF_proportional = Pre_DF_proportional2;

    Eigen::VectorXd Ones = Eigen::VectorXd::Ones(N_cats-1);
    Eigen::MatrixXd X_EXT_PROPORTIONAL = Eigen::kroneckerProduct(Pre_DF_proportional, Ones).eval();

    // TENGO QUE PONER EL IF ACA
    //
    // if (threshold == "equidistant"){
    //   NumericMatrix tJac = my_cbind(1, seq_len(categories_order.length() -1 )-1 );
    //   Eigen::Map<Eigen::MatrixXd> tJac2 = as<Eigen::Map<Eigen::MatrixXd> >(tJac);
    //   Eigen::VectorXd Ones1 = Eigen::VectorXd::Ones(Response_EXT.rows());
    //   Eigen::MatrixXd Threshold_M = Eigen::kroneckerProduct(Ones1, tJac2).eval();
    //   X_EXT_PROPORTIONAL.conservativeResize(X_EXT_PROPORTIONAL.rows(),X_EXT_PROPORTIONAL.cols()+2);
    //   X_EXT_PROPORTIONAL.block(0,X_EXT_PROPORTIONAL.cols()-2, X_EXT_PROPORTIONAL.rows(),2) = Threshold_M;
    // }

    Design_Matrix.conservativeResize(X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()+X_EXT_PROPORTIONAL.cols());
    Design_Matrix.block(0,0,X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()) = X_EXT_COMPLETE;
    Design_Matrix.block(0,X_EXT_COMPLETE.cols(),X_EXT_COMPLETE.rows(),X_EXT_PROPORTIONAL.cols()) = X_EXT_PROPORTIONAL;

  }else{Design_Matrix = X_EXT_COMPLETE;}

  // Now extend

  // // X COMPLETE
  // DataFrame DF_complete_effect = df_tans_2[colnames_final_m];
  // NumericMatrix Pre_Design1 = internal::convert_using_rfunction(DF_complete_effect, "as.matrix");
  // Eigen::Map<Eigen::MatrixXd> Pre_Design = as<Eigen::Map<Eigen::MatrixXd> >(Pre_Design1);
  // // print(colnames_final_m);
  // Eigen::MatrixXd Iden_Q1 = Eigen::MatrixXd::Identity(N_cats-1,N_cats-1);
  //
  // Eigen::MatrixXd X_EXT_COMPLETE = Eigen::kroneckerProduct(Pre_Design.rightCols(DF_complete_effect.cols() - 1), Iden_Q1).eval();
  //
  // // X PROPOTIONAL
  //
  // Eigen::MatrixXd Design_Matrix;
  //
  // if (any_alternative_specific[0]) {
  //
  //   DataFrame DF_proportional_effect = df_tans_2[proportional_effect];
  //   NumericMatrix Pre_DF_proportional1 = internal::convert_using_rfunction(DF_proportional_effect, "as.matrix");
  //   Eigen::Map<Eigen::MatrixXd> Pre_DF_proportional = as<Eigen::Map<Eigen::MatrixXd> >(Pre_DF_proportional1);
  //   Eigen::VectorXd Ones = Eigen::VectorXd::Ones(N_cats-1);
  //   Eigen::MatrixXd X_EXT_PROPORTIONAL = Eigen::kroneckerProduct(Pre_DF_proportional, Ones).eval();
  //
  //   Design_Matrix.conservativeResize(X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()+X_EXT_PROPORTIONAL.cols());
  //   Design_Matrix.block(0,0,X_EXT_COMPLETE.rows(),X_EXT_COMPLETE.cols()) = X_EXT_COMPLETE;
  //   Design_Matrix.block(0,X_EXT_COMPLETE.cols(),X_EXT_COMPLETE.rows(),X_EXT_PROPORTIONAL.cols()) = X_EXT_PROPORTIONAL;
  //
  // }else{Design_Matrix = X_EXT_COMPLETE;}

  return List::create(
    Named("Design") = Design,
    Named("Design_Matrix") = Design_Matrix
  );
}

CharacterVector Var_Not_In(DataFrame final_matrix, CharacterVector alternative_specific){

  LogicalVector any_alternative_specific = !is_na(alternative_specific);

  CharacterVector colnames_final_m = final_matrix.names();

  if (any_alternative_specific[0]) {
    NumericVector x(colnames_final_m.length());
    for(int indi = 0 ; indi < alternative_specific.length(); indi++){
      String var_1 = alternative_specific[indi];
      int indi_var = final_matrix.findName(var_1);
      x[indi_var] = indi_var;
    }
    colnames_final_m = colnames_final_m[x==0]; // Case where there no alternative specific variables
  }
  colnames_final_m.erase(0, 4); // Eliminate the first 4 information variables

  return colnames_final_m;
}

List formula_entry(Formula formula1){
  Environment base_base("package:base");
  Function my_strsplit = base_base["strsplit"];
  Function my_format = base_base["format"];
  Function my_paste = base_base["paste"];
  Function my_sub = base_base["sub"];
  Function my_rev = base_base["rev"];
  Function my_trimws = base_base["trimws"];

  Environment base_stats("package:stats");
  Function my_as_formula = base_stats["as.formula"];

  CharacterVector st1 = my_format(formula1);
  String str_for = my_paste(st1,  _["collapse"] = "");
  List list1 = (my_strsplit(str_for, "~"));
  CharacterVector vars = list1[0];
  String vars_string = vars[1];
  String res1 = vars[0];
  String res = my_trimws(res1);
  List list_vars = my_strsplit(vars_string, "[+]");
  CharacterVector char_vars = list_vars(0);

  String vars_for = my_paste(my_sub("\\[[^()]*\\]", "", char_vars),  _["collapse"] = " + ");
  CharacterVector form = my_paste(res, vars_for, _["sep"] = "~");
  Formula formula2 = my_as_formula(form);

  String firs_col;
  List list_no_spaces, list_var_rev, list_cat_rev;
  StringVector Vars(char_vars.length()), Alternatives(char_vars.length());

  for (int i = 0; i < char_vars.length() ; i++) {
    firs_col = char_vars[i];
    String no_spaces = my_trimws(firs_col);
    list_no_spaces = my_strsplit(no_spaces, "");
    String rev_string = my_paste(my_rev(list_no_spaces[0]), _["collapse"] = "");

    String var = my_sub(".*\\[", "", rev_string);
    list_var_rev = my_strsplit(var, "");
    String var1 = my_paste(my_rev(list_var_rev[0]), _["collapse"] = "");

    String cat = my_sub(".*\\]", "", my_sub("\\[.*", "", rev_string));
    list_cat_rev = my_strsplit(cat, "");
    String cat1 = my_paste(my_rev(list_cat_rev[0]), _["collapse"] = "");

    if (var1 != cat1){ Alternatives[i] = cat1; }
    if (var1 == "1"){ var1 = "(Intercept)"; }
    Vars[i] = var1;
  }

  DataFrame Var_alt = DataFrame::create(Named("Alternatives") = Alternatives);
  Var_alt = my_transpose(Var_alt);
  Var_alt.names() = Vars;

  DataFrame Var_alt1 = Var_alt;

  return List::create(
    Named("Var_alt") = Var_alt,
    Named("Response") = res,
    Named("formula_model") = formula2
  );
}

NumericVector Cat_ref(CharacterVector alternatives, String Ref_cat){
  NumericVector Ref_vec(alternatives.length());
  for (int i = 0; i < alternatives.length(); i++){
    if(alternatives(i) == Ref_cat){
      Ref_vec(i) = 1;
    }else{Ref_vec(i) = 0;}
  }
  return Ref_vec;
}

DataFrame Sort_DataFrame(DataFrame ModelMatrix,
                         DataFrame InputData,
                         CharacterVector names,
                         String Choice_vec,
                         String Ref_cat) {
  // Order DATAFRAME ACCORDING TO VARIABLES GIVEN IN VECTOR NAMES
  // CBIND OF DATA SETS AND THEN ORDER ACCORDING TO VARIABLES
  Environment base_env("package:base");
  Function my_order = base_env["order"];
  Function my_cbind = base_env["cbind"];
  Function my_asnumeric = base_env["as.numeric"];

  String alt = names[0];
  String id_case_0 = names[2];

  NumericVector Cat_ref_vec = Cat_ref(InputData[alt], Ref_cat);

  DataFrame A2 = my_cbind( _["alternatives"] = InputData[alt],
                           _["Cat_ref_vec"] = Cat_ref_vec,
                           _["id_case"] = InputData[id_case_0],
                                                   _["choice"] = my_asnumeric(InputData[Choice_vec]),
                                                   ModelMatrix);

  CharacterVector names1 = {"alternatives", "Cat_ref_vec", "id_case"};

  DataFrame df_tans = my_transpose(A2);
  DataFrame df_tans_2 = A2 ;

  for (int i = 0; i < names1.length() ; i++) {
    String var = names1(i);
    NumericVector order_var_sel = my_order(df_tans_2[var]);
    order_var_sel = order_var_sel - 1 ;
    df_tans = df_tans[order_var_sel];
    df_tans_2 = my_transpose(df_tans);
  }
  return  df_tans_2;
}

DataFrame my_AsNumericMatrix(DataFrame dat_in){
  Environment base_env("package:base");
  Function my_asnumeric = base_env["as.numeric"];
  Function my_ascharacter = base_env["as.character"];
  Function my_cbind = base_env["cbind"];
  DataFrame data_out = dat_in ;

  for (int i = 4; i < dat_in.length() ; i++) {
    NumericVector vec = my_asnumeric(my_ascharacter(dat_in[i]));
    data_out[i] = vec;
  }
  return data_out;
}

NumericMatrix Model_Matrix(DataFrame data, Formula formula) {
  Environment stats_env("package:stats");
  Function model_frame = stats_env["model.frame"];
  Function model_matrix = stats_env["model.matrix"];
  DataFrame df_new1 = model_frame(Rcpp::_["formula"] = formula, _["data"] = data);
  NumericMatrix df_new = model_matrix(df_new1, _["data"] = data);
  return  df_new;
}

DataFrame All_pre_data(Formula formula, DataFrame input_data, CharacterVector var_informatives, String choice, String Ref_cat){
  DataFrame data_output = my_AsNumericMatrix(Sort_DataFrame(
    Model_Matrix(input_data,
                 formula_entry(formula)["formula_model"]),
                 input_data,
                 var_informatives,
                 choice,
                 Ref_cat));
  return data_output;
}

Eigen::MatrixXd Extend_alt_specific(DataFrame alt_specific, int N_cats, int N_ind, CharacterVector var_alt_specific){
  // cat_index = 1;

  Eigen::VectorXd Ones1 = Eigen::VectorXd::Ones(N_cats-1);
  Eigen::MatrixXd Iden_Q = Eigen::MatrixXd::Identity(N_cats,N_cats);
  Iden_Q.conservativeResize(Iden_Q.rows() - 1, Iden_Q.cols());
  Iden_Q.col(N_cats-1) = -Ones1;
  NumericMatrix alt_specific_num = internal::convert_using_rfunction(alt_specific[var_alt_specific], "as.matrix");
  Eigen::Map<Eigen::MatrixXd> M_alt_specific = as<Eigen::Map<Eigen::MatrixXd> >(alt_specific_num);
  Eigen::MatrixXd Matrix_trans((N_cats-1)*N_ind,var_alt_specific.length());
  for(int indi = 1 ; indi <= N_ind ; indi++)
  {
    Eigen::MatrixXd Block_ind =  M_alt_specific.block((indi-1) * N_cats, 0, N_cats, var_alt_specific.length());
    Eigen::MatrixXd Block_RES = Block_ind.transpose() * Iden_Q.transpose();
    Matrix_trans.block((indi-1) * (N_cats-1), 0, N_cats-1, var_alt_specific.length()) = Block_RES.transpose();
  }

  return Matrix_trans;
}

Eigen::MatrixXd Extend_case_specific(DataFrame case_specific, int N_cats, int N_ind,
                                     CharacterVector var_alt_specific,  DataFrame Var_alt){

  CharacterVector var_case_specific = Var_Not_In(case_specific, var_alt_specific);
  DataFrame effect_specific_for1 = Var_alt[var_case_specific];
  effect_specific_for1 = my_transpose(effect_specific_for1);
  CharacterVector effect_specific_for2 = effect_specific_for1[0];

  Eigen::MatrixXd Iden_Q1 = Eigen::MatrixXd::Identity(N_cats-1,N_cats-1);
  NumericMatrix case_specific_num = internal::convert_using_rfunction(case_specific[var_case_specific], "as.matrix");
  Eigen::Map<Eigen::MatrixXd> M_case_specific = as<Eigen::Map<Eigen::MatrixXd> >(case_specific_num);

  Eigen::MatrixXd Matrix_trans((N_cats-1)*N_ind,(N_cats-1)*var_case_specific.length());

  for(int indi = 1 ; indi <= N_ind ; indi++)
  {
    Eigen::MatrixXd Block_ind =  M_case_specific.block((indi-1) * N_cats, 0, 1, var_case_specific.length());
    Eigen::MatrixXd Block_RES = Eigen::kroneckerProduct(Block_ind, Iden_Q1).eval();
    Matrix_trans.block((indi-1) * (N_cats-1), 0, N_cats-1, (N_cats-1)*var_case_specific.length()) = Block_RES;
  }
  // Rcout << Matrix_trans << std::endl;
  // Create vector relation between category and order in dataset
  CharacterVector ordered_cat = case_specific["alternatives"];
  String cat_1 = ordered_cat[0];
  NumericVector cat_index = NumericVector::create(Named(cat_1 , 0));

  for(int j_1 = 1; j_1 < N_cats ; j_1++){
    cat_1 = ordered_cat[j_1];
    cat_index.push_back(j_1, cat_1);
  }
  int count_re_var = 0;
  for(int ind_b_var = 0; ind_b_var < var_case_specific.length() ; ind_b_var++){

    Eigen::MatrixXd Block_cat = Matrix_trans.block(0,ind_b_var*(N_cats-1),Matrix_trans.rows(), N_cats-1);

    if( effect_specific_for2[ind_b_var] != "" ){
      String cat_loop = effect_specific_for2[ind_b_var];
      int Var_to_keep = cat_index[cat_loop];
      Matrix_trans.block(0,count_re_var,Matrix_trans.rows(), 1) = Block_cat.block(0, Var_to_keep, Matrix_trans.rows(),1);
      count_re_var = count_re_var+1;

    }else{

      Matrix_trans.block(0,count_re_var,Matrix_trans.rows(), N_cats-1) = Block_cat;
      count_re_var = count_re_var+N_cats-1;

    }
  }
  Eigen::MatrixXd Matrix_trans1 = Matrix_trans.block(0,0,Matrix_trans.rows(), count_re_var);
  return Matrix_trans1;
}

Eigen::MatrixXd Extend_All_design(DataFrame Final_mat, DataFrame Var_alt, CharacterVector var_alt_specific,
                                  int N_ind, int N_cats){

  LogicalVector any_alternative_specific = !is_na(var_alt_specific); // TRUE IF ANY

  CharacterVector var_case_specific = Var_Not_In(Final_mat, var_alt_specific);
  Eigen::MatrixXd Ex_case_M = Extend_case_specific(Final_mat, N_cats, N_ind, var_alt_specific, Var_alt );

  Eigen::MatrixXd Design_Matrix;

  if (any_alternative_specific[0]) {
    Eigen::MatrixXd Ex_alt_M = Extend_alt_specific(Final_mat, N_cats, N_ind, var_alt_specific);
    int rows_t = Ex_case_M.rows();
    Design_Matrix.conservativeResize(rows_t,Ex_case_M.cols()+Ex_alt_M.cols());
    Design_Matrix.block(0,0,rows_t,Ex_case_M.cols()) = Ex_case_M;
    Design_Matrix.block(0,Ex_case_M.cols(),rows_t,Ex_alt_M.cols()) = Ex_alt_M;
  }else{Design_Matrix = Ex_case_M;}

  return Design_Matrix;
}

List Extend_Response(DataFrame Final_mat ){

  Environment base_env("package:base");
  Function my_unique = base_env["unique"];
  Function my_length = base_env["length"];
  Function my_matrix = base_env["matrix"];
  Function my_asnumeric = base_env["as.numeric"];

  SEXP N_cat_1 = my_length(my_unique(Final_mat["alternatives"]));
  int N_cat = Rcpp::as<int>(N_cat_1);

  NumericVector y11 = my_asnumeric(Final_mat["choice"]) ;
  DataFrame Y_Ext = my_transpose(my_matrix(y11 ,  _["nrow"] = N_cat));
  NumericMatrix Y_Ext1 = internal::convert_using_rfunction(Y_Ext, "as.matrix");

  Eigen::MatrixXd Y_n2 = as<Eigen::Map<Eigen::MatrixXd> >(Y_Ext1-1);
  Y_n2.conservativeResize(Y_n2.rows(), Y_n2.cols() - 1);

  return List::create(
    Named("N_cat") = N_cat,
    Named("Y_Ext") = Y_n2
  );
}

List distribution::select_data_nested(Formula formula,
                                      String individuals,
                                      String Alternatives,
                                      SEXP ref_cat,
                                      CharacterVector var_alt_specific,
                                      DataFrame input_data
) {

  List Formula_l = formula_entry(formula);
  SEXP Var_spe_alt1 = Formula_l["Var_alt"];
  String Response = Formula_l["Response"];
  DataFrame Var_spe_alt = Rcpp::as<DataFrame>(Var_spe_alt1);

  CharacterVector var_informatives = {"Alternatives", "Cat_ref_vec", "individuals"};
  var_informatives[0] = Alternatives;
  var_informatives[2] = individuals;

  DataFrame Final_mat1 = All_pre_data(Formula_l["formula_model"],
                                      input_data,
                                      var_informatives,
                                      Response,
                                      ref_cat);

  List Response_L = Extend_Response(Final_mat1);
  Eigen::MatrixXd Response_M = Response_L["Y_Ext"];

  Eigen::MatrixXd Design_Matrix = Extend_All_design(Final_mat1,
                                                    Var_spe_alt,
                                                    var_alt_specific,
                                                    Response_M.rows(),
                                                    Response_L["N_cat"]);

  return List::create(
    _["Design_Matrix"] = Design_Matrix,
    _["Response_M"] = Response_M
  );
}

Eigen::VectorXd Logistic::in_open_corner(const Eigen::VectorXd& p) const
{
  Eigen::VectorXd pi = p;
  int J = pi.size() + 1;
  for(int j=0; j<J-1; ++j)
  { pi[j] = std::max(_epsilon_0, std::min(pi[j], 1-_epsilon_1)); }
  double sum = pi.sum();
  if(sum > 1-_epsilon_1)
  {
    for(int j=0; j<J-1; ++j)
    { pi[j] *= (1.-_epsilon_1)/sum;  }
  }
  return pi;
}

Logistic::Logistic(void) {
}
double Logistic::cdf_logit(const double& value) const
{
  boost::math::logistic dist(0., 1.);
  return boost::math::cdf(dist, value);
}
double Logistic::pdf_logit(const double& value) const
{
  boost::math::logistic dist(0., 1.);
  return boost::math::pdf(dist, value);
}
Eigen::VectorXd Logistic::InverseLinkQuantileFunction(Eigen::VectorXd vector ){
  boost::math::logistic dist(0., 1.);
  for (int i = 0; i<=vector.size()-1; i++)
    vector(i) = quantile(dist, vector(i));
  return vector;
}

Normal::Normal(void) {
}
double Normal::cdf_normal(const double& value) const
{
  boost::math::normal norm;
  return boost::math::cdf(norm, value);
}
double Normal::pdf_normal(const double& value) const
{
  boost::math::normal norm;
  return boost::math::pdf(norm, value);
}
Eigen::VectorXd Normal::InverseLinkQuantileFunction(Eigen::VectorXd vector ){
  boost::math::normal norm;
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = quantile(norm, vector(i));
  return vector;
}

Cauchit::Cauchit(void) {
}

double Cauchit::cdf_cauchit(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  return cdf(extreme_value, value);
}
double Cauchit::pdf_cauchit(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  return pdf(extreme_value, value);
}
Eigen::VectorXd Cauchit::InverseLinkQuantileFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = quantile(extreme_value, vector(i));
  return vector;
}

Student::Student(void) {
}

double Student::cdf_student(const double& value, const double& freedom_degrees) const
{
  double z;
  if(freedom_degrees < 2 * pow(value, 2) )
  { z = boost::math::ibeta(freedom_degrees * 0.5, 0.5, freedom_degrees / (freedom_degrees + pow(value, 2))) * 0.5; }
  else
  { z = boost::math::ibetac(0.5, freedom_degrees * 0.5, pow(value, 2) / (freedom_degrees + pow(value, 2))) * 0.5; }
  if(value > 0)
  { return 1-z; }
  else
  {return z; }
}

double Student::pdf_student(const double& value, const double& freedom_degrees) const
{ return pow( freedom_degrees/(freedom_degrees + pow(value, 2)) , (1+freedom_degrees) * 0.5 ) / ( pow(freedom_degrees,0.5) * boost::math::beta(freedom_degrees*0.5, 0.5) ); }

Gumbel::Gumbel(void) {
  // Rcout << "Gumbel is being created" << endl;
}
double Gumbel::cdf_gumbel(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  return cdf(extreme_value, value);
}
double Gumbel::pdf_gumbel(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  return pdf(extreme_value, value);
}

Gompertz::Gompertz(void) {
  // Rcout << "Gompertz is being created" << endl;
}

double Gompertz::pdf_gompertz(const double& value) const
{ double _mu = 0.0;
  double _sigma = 1.0;

  return (exp((value - _mu)/ _sigma) *  exp( - exp ((value - _mu)/ _sigma) ) ) / _sigma ; }

double Gompertz::cdf_gompertz(const double& value) const
{ double _mu = 0.0;
  double _sigma = 1.0;
  return  1 - exp( - exp((value - _mu) / _sigma) ); }

RCPP_MODULE(exportmod){
  using namespace Rcpp ;
  class_<distribution>("distribution")
    .constructor()
  ;
  class_<Student>("Student")
    .derives<distribution>("distribution")
    .constructor()
    .method( "cdf_student", &Student::cdf_student )
    .method( "pdf_student", &Student::pdf_student )
  ;
}

// RCPP_MODULE(exportmoddev){
//   using namespace Rcpp ;
//   class_<distribution>("distribution")
//     .constructor()
//   ;
//   class_<Logistic>("Logistic")
//     .derives<distribution>("distribution")
//     .constructor()
//     .method( "InverseLinkCumulativeFunction", &Logistic::InverseLinkCumulativeFunction )
//   ;
// }


//
//  E_step.cpp
//  synthetic_data_helper
//
//  Created by Cristián Garay on 11/16/16.
//  Copyright © 2016 Cristian Garay. All rights reserved.
//

#define NPY_NO_DEPRECATED_API NPY_1_11_API_VERSION

#include <iostream>
#include <stdint.h>
#include <alloca.h>
#include <Eigen/Core>
#include <omp.h>
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/python/ptr.hpp>
#include <Python.h>
#include <numpy/ndarrayobject.h>

using namespace Eigen;
using namespace std;
using namespace boost::python;

#if PY_VERSION_HEX >= 0x03000000
void *
#else
void
#endif
init_numpy(){
    //Py_Initialize;
    import_array();
}

//original comment:
//"TODO if we aren't outputting gamma, don't need to write it to memory (just
//need t and t+1), so we can save the stack array for each HMM at the cost of
//a branch"

/*struct double_to_python_float
{
    static PyObject* convert(double const& d)
      {
        return boost::python::incref(
          boost::python::object(d).ptr());
      }
};*/

//numpy scalar converters.
template <typename T, NPY_TYPES NumPyScalarType>
struct enable_numpy_scalar_converter
{
  enable_numpy_scalar_converter()
  {
    // Required NumPy call in order to use the NumPy C API within another
    // extension module.
    // import_array();
    init_numpy();

    boost::python::converter::registry::push_back(
      &convertible,
      &construct,
      boost::python::type_id<T>());
  }

  static void* convertible(PyObject* object)
  {
    // The object is convertible if all of the following are true:
    // - is a valid object.
    // - is a numpy array scalar.
    // - its descriptor type matches the type for this converter.
    return (
      object &&                                                    // Valid
      PyArray_CheckScalar(object) &&                               // Scalar
      PyArray_DescrFromScalar(object)->type_num == NumPyScalarType // Match
    )
      ? object // The Python object can be converted.
      : NULL;
  }

  static void construct(
    PyObject* object,
    boost::python::converter::rvalue_from_python_stage1_data* data)
  {
    // Obtain a handle to the memory block that the converter has allocated
    // for the C++ type.
    namespace python = boost::python;
    typedef python::converter::rvalue_from_python_storage<T> storage_type;
    void* storage = reinterpret_cast<storage_type*>(data)->storage.bytes;

    // Extract the array scalar type directly into the storage.
    PyArray_ScalarAsCtype(object, storage);

    // Set convertible to indicate success.
    data->convertible = storage;
  }
};

//dict create_synthetic_data(dict& model, numeric::array& starts, numeric::array& lengths, numeric::array& resources)
dict run(dict& data, dict& model, numeric::array& trans_softcounts, numeric::array& emission_softcounts, numeric::array& init_softcounts, int num_outputs){
    //TODO: check if parameters are null.
    //TODO: check that dicts have the required members.
    //TODO: check that all parameters have the right sizes.
    //TODO: i'm not sending any error messages.

    IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");

    numeric::array alldata = extract<numeric::array>(data["data"]); //multidimensional array, so i need to keep extracting arrays.
    int bigT = len(alldata[0]); //this should be the number of columns in the alldata object. i'm assuming is 2d array.
    int num_subparts = len(alldata);

    numeric::array allresources = extract<numeric::array>(data["resources"]);

    numeric::array starts = extract<numeric::array>(data["starts"]);

    int num_sequences = len(starts);

    numeric::array lengths = extract<numeric::array>(data["lengths"]);

    numeric::array learns = extract<numeric::array>(model["learns"]);
    int num_resources = len(learns);

    numeric::array forgets = extract<numeric::array>(model["forgets"]);

    numeric::array guesses = extract<numeric::array>(model["guesses"]);

    numeric::array slips = extract<numeric::array>(model["slips"]);

    double prior = extract<double>(model["prior"]);

    bool normalizeLengths = false;
    //then the original code goes to find the optional parameters.

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        double learn = extract<double>(learns[n]);
        double forget = extract<double>(forgets[n]);
        As.col(2*n) << 1-learn, learn;
        As.col(2*n+1) << forget, 1-forget;
    }

    Array2Xd Bn(2,2*num_subparts);
    for (int n=0; n<num_subparts; n++) {
        double guess = extract<double>(guesses[n]);
        double slip = extract<double>(slips[n]);
        Bn.col(2*n) << 1-guess, slip; // incorrect
        Bn.col(2*n+1) << guess, 1-slip; // correct
    }

    //// outputs

    //TODO: NEED TO FIX THIS I'M CREATING NEW ARRAYS AND I NEED TO USE THE ARGUMENTS!!!
    //TODO: FIX THIS!!!
    /*Map<ArrayXXd,Aligned> all_trans_softcounts(trans_softcounts,2,2*num_resources);
    all_trans_softcounts.setZero();
    Map<Array2Xd,Aligned> all_emission_softcounts(emission_softcounts,2,2*num_subparts);
    all_emission_softcounts.setZero();
    Map<Array2d,Aligned> all_initial_softcounts(init_softcounts);
    all_initial_softcounts.setZero();*/
    //TODO: I replaced the pointers to the arguments for new Eigen arrays.
    /*ArrayXXd all_trans_softcounts(2,2*num_resources);
    all_trans_softcounts.setZero(); //why is he setting all these to zero???
    Array2Xd all_emission_softcounts(2,2*num_subparts);
    all_emission_softcounts.setZero();
    Array2d all_initial_softcounts(2, 1); //should i use these dimensions? the same as the original vector??
    all_initial_softcounts.setZero();*/
    //cout << "all_trans_softcounts" << all_trans_softcounts << endl;
    //cout << "all_emission_softcounts" << all_emission_softcounts << endl;
    //cout << "all_initial_softcounts" << all_initial_softcounts << endl;
    double r_trans_softcounts[2*2*num_resources];
    double r_emission_softcounts[2*2*num_subparts];
    double r_init_softcounts[2*1];
    Map<ArrayXXd,Aligned> all_trans_softcounts(r_trans_softcounts,2,2*num_resources);
    all_trans_softcounts.setZero();
    Map<Array2Xd,Aligned> all_emission_softcounts(r_emission_softcounts,2,2*num_subparts);
    all_emission_softcounts.setZero();
    Map<Array2d,Aligned> all_initial_softcounts(r_init_softcounts);
    all_initial_softcounts.setZero();

    //TODO: FIX THIS!!! I'll replace all these weird arrays for zeroes ones.
    //Array2Xd likelihoods_out(2,bigT);
    //likelihoods_out.setZero();
    //Array2Xd gamma_out(2,bigT);
    //gamma_out.setZero();
    //Array2Xd alpha_out(2,bigT);
    //alpha_out.setZero();
    Map<Array2Xd,Aligned> likelihoods_out(NULL,2,bigT);
    Map<Array2Xd,Aligned> gamma_out(NULL,2,bigT);
    Map<Array2Xd,Aligned> alpha_out(NULL,2,bigT);
    double s_total_loglike = 0;
    double *total_loglike = &s_total_loglike;
    //cout << "likelihoods_out" << likelihoods_out << endl;
    //cout << "gamma_out" << gamma_out << endl;
    //cout << "alpha_out" << alpha_out << endl;
    //cout << "s_total_loglike " << s_total_loglike << endl;
    //cout << "total_loglike " << total_loglike << endl;

    //TODO: FIX THIS!!! why is he doing this??
    /* switch (num_outputs)
    {
        case 4:
            plhs[3] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&likelihoods_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[3]),2,bigT);
        case 3:
            plhs[2] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&gamma_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[2]),2,bigT);
        case 2:
            plhs[1] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&alpha_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[1]),2,bigT);
        case 1:
            plhs[0] = mxCreateDoubleScalar(0.);
            total_loglike = mxGetPr(plhs[0]);
    }*/
    double r_likelihoods_out[2*bigT];
    double r_gamma_out[2*bigT];
    double r_alpha_out[2*bigT];

    new (&likelihoods_out) Map<Array2Xd,Aligned>(r_likelihoods_out,2,bigT);
    new (&gamma_out) Map<Array2Xd,Aligned>(r_gamma_out,2,bigT);
    new (&alpha_out) Map<Array2Xd,Aligned>(r_alpha_out,2,bigT);

    /* COMPUTATION */
    Eigen::initParallel();
    /* omp_set_dynamic(0); */
    /* omp_set_num_threads(6); */
    #pragma omp parallel
    {
        double s_trans_softcounts[2*2*num_resources] __attribute__((aligned(16)));
        double s_emission_softcounts[2*2*num_subparts] __attribute__((aligned(16)));
        Map<ArrayXXd,Aligned> trans_softcounts_temp(s_trans_softcounts,2,2*num_resources);
        Map<ArrayXXd,Aligned> emission_softcounts_temp(s_emission_softcounts,2,2*num_subparts);
        Array2d init_softcounts_temp;
        double loglike;

        trans_softcounts_temp.setZero();
        emission_softcounts_temp.setZero();
        init_softcounts_temp.setZero();
        loglike = 0;
        int num_threads = omp_get_num_threads();
        int blocklen = 1 + ((num_sequences - 1) / num_threads);
        int sequence_idx_start = blocklen * omp_get_thread_num();
        int sequence_idx_end = min(sequence_idx_start+blocklen,num_sequences);
        //mexPrintf("start:%d   end:%d\n", sequence_idx_start, sequence_idx_end);
        //cout << "start: " << sequence_idx_start << " end: " << sequence_idx_end << endl;

        for (int sequence_index=sequence_idx_start; sequence_index < sequence_idx_end; sequence_index++) {

            // NOTE: -1 because Matlab indexing starts at 1
            int64_t sequence_start = extract<int64_t>(starts[sequence_index]) - 1;

            int64_t T = extract<int64_t>(lengths[sequence_index]);

            //// likelihoods
            double s_likelihoods[2*T];
            Map<Array2Xd,Aligned> likelihoods(s_likelihoods,2,T);

            likelihoods.setOnes();
             for (int t=0; t<T; t++) {
                 for (int n=0; n<num_subparts; n++) {
                    int32_t data_temp = extract<int32_t>(alldata[n][sequence_start+t]);
                     if (data_temp != 0) {
                         likelihoods.col(t) *= Bn.col(2*n + (data_temp == 2));
                     }
                 }
             }

            //// forward messages
            double norm;
            double s_alpha[2*T] __attribute__((aligned(16)));
            double contribution;
            Map<MatrixXd,Aligned> alpha(s_alpha,2,T);
            alpha.col(0) = initial_distn * likelihoods.col(0);
            norm = alpha.col(0).sum();
            //cout << "norm: " << norm << endl;
            alpha.col(0) /= norm;
            contribution = log(norm);
            //cout << "contribution " << contribution << endl;
            if(normalizeLengths) {
                contribution = contribution / T;
            }
            loglike += contribution;
            //cout << "loglike2 " << loglike << endl;

            for (int t=0; t<T-1; t++) {
                int64_t resources_temp = extract<int64_t>(allresources[sequence_start+t]);
                alpha.col(t+1) = (As.block(0,2*(resources_temp-1),2,2) * alpha.col(t)).array()
                    * likelihoods.col(t+1);
                //cout << "likelihoods.col(t+1) " << likelihoods.col(t+1) << endl;
                norm = alpha.col(t+1).sum();
                //cout << "norm: " << norm << endl;
                alpha.col(t+1) /= norm;
                contribution = log(norm);
                //cout << "contribution: " << contribution << endl;
                if(normalizeLengths) {
                    contribution = contribution / T;
                }
                loglike += contribution;
                //cout << "loglike " << loglike << endl;
            }

            //// backward messages and statistic counting

            double s_gamma[2*T] __attribute__((aligned(16)));
            Map<Array2Xd,Aligned> gamma(s_gamma,2,T);
            gamma.col(T-1) = alpha.col(T-1);
            for (int n=0; n<num_subparts; n++) {
                int32_t data_temp = extract<int32_t>(alldata[n][sequence_start+(T-1)]);
                if (data_temp != 0) {
                    emission_softcounts_temp.col(2*n + (data_temp == 2)) += gamma.col(T-1);
                }
            }

            for (int t=T-2; t>=0; t--) {

				int64_t resources_temp = extract<int64_t>(allresources[sequence_start+t]);
                Matrix2d A = As.block(0,2*(resources_temp-1),2,2);
                Array22d pair = A.array();
                pair.rowwise() *= alpha.col(t).transpose().array();
                pair.colwise() *= gamma.col(t+1);
                pair.colwise() /= (A*alpha.col(t)).array();
                pair = (pair != pair).select(0.,pair); // NOTE: replace NaNs
                trans_softcounts_temp.block(0,2*(resources_temp-1),2,2) += pair;

                gamma.col(t) = pair.colwise().sum().transpose();
                // NOTE: we have to touch the data again here
                for (int n=0; n<num_subparts; n++) {
                    int32_t data_temp = extract<int32_t>(alldata[n][sequence_start+t]);
                    if (data_temp != 0) {
                        emission_softcounts_temp.col(2*n + (data_temp == 2)) += gamma.col(t);
                    }
                }
            }
            init_softcounts_temp += gamma.col(0);

            //TODO: FIX THIS!!!
            /* switch (nlhs)
            {
                case 4:
                    likelihoods_out.block(0,sequence_start,2,T) = likelihoods;
                case 3:
                    gamma_out.block(0,sequence_start,2,T) = gamma;
                case 2:
                    alpha_out.block(0,sequence_start,2,T) = alpha;
            } */
            likelihoods_out.block(0,sequence_start,2,T) = likelihoods;
            gamma_out.block(0,sequence_start,2,T) = gamma;
            alpha_out.block(0,sequence_start,2,T) = alpha;
        }

        #pragma omp critical
        {
            all_trans_softcounts += trans_softcounts_temp;
            all_emission_softcounts += emission_softcounts_temp;
            all_initial_softcounts += init_softcounts_temp;
            //cout << "loglike " << loglike << endl;
            *total_loglike += loglike;
        }
    }

    dict result;
    result["total_loglike"] = *total_loglike;

    //cout << "r_trans_softcounts " << r_trans_softcounts << endl;

    npy_intp all_trans_softcounts_dims[3] = {num_resources,2,2}; //TODO: just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_trans_softcounts_pyObj = PyArray_New(&PyArray_Type, 3, all_trans_softcounts_dims, NPY_DOUBLE, NULL, &r_trans_softcounts, 0, NPY_ARRAY_CARRAY, NULL);
    boost::python::handle<> all_trans_softcounts_handle( all_trans_softcounts_pyObj );
    boost::python::numeric::array all_trans_softcounts_arr( all_trans_softcounts_handle );
    result["all_trans_softcounts"] = all_trans_softcounts_arr;

    npy_intp all_emission_softcounts_dims[3] = {num_subparts,2,2}; //TODO: just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_emission_softcounts_pyObj = PyArray_New(&PyArray_Type, 3, all_emission_softcounts_dims, NPY_DOUBLE, NULL, &r_emission_softcounts, 0, NPY_ARRAY_CARRAY, NULL);
    boost::python::handle<> all_emission_softcounts_handle( all_emission_softcounts_pyObj );
    boost::python::numeric::array all_emission_softcounts_arr( all_emission_softcounts_handle );
    result["all_emission_softcounts"] = all_emission_softcounts_arr;

    npy_intp all_initial_softcounts_dims[2] = {2,1}; //TODO: just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_initial_softcounts_pyObj = PyArray_New(&PyArray_Type, 2, all_initial_softcounts_dims, NPY_DOUBLE, NULL, &r_init_softcounts, 0, NPY_ARRAY_CARRAY, NULL);
    boost::python::handle<> all_initial_softcounts_handle( all_initial_softcounts_pyObj );
    boost::python::numeric::array all_initial_softcounts_arr( all_initial_softcounts_handle );
    result["all_initial_softcounts"] = all_initial_softcounts_arr;

    npy_intp alpha_out_dims[2] = {2,bigT}; //TODO: just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * alpha_out_pyObj = PyArray_New(&PyArray_Type, 2, alpha_out_dims, NPY_DOUBLE, NULL, &r_alpha_out, 0, NPY_ARRAY_CARRAY, NULL);
    boost::python::handle<> alpha_out_handle( alpha_out_pyObj );
    boost::python::numeric::array alpha_out_arr( alpha_out_handle );
    result["alpha"] = alpha_out_arr;

    return(result);
}

BOOST_PYTHON_MODULE(E_step){
    //import_array();
    init_numpy();
    numeric::array::set_module_and_type("numpy", "ndarray");
    //to_python_converter<double, double_to_python_float>();
    enable_numpy_scalar_converter<boost::int8_t, NPY_INT8>();
    enable_numpy_scalar_converter<boost::int16_t, NPY_INT16>();
    enable_numpy_scalar_converter<boost::int32_t, NPY_INT32>();
    enable_numpy_scalar_converter<boost::int64_t, NPY_INT64>();

    def("run", run);

}


/*

Copyright (c) 2005-2017, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

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

*/
#ifndef LINEARPARABOLICPDESYSTEMWITHCOUPLEDODESYSTEMSOLVER_HPP_
#define LINEARPARABOLICPDESYSTEMWITHCOUPLEDODESYSTEMSOLVER_HPP_

#include "AbstractAssemblerSolverHybrid.hpp"
#include "AbstractDynamicLinearPdeSolver.hpp"
#include "AbstractLinearParabolicPdeSystemForCoupledOdeSystem.hpp"
#include "TetrahedralMesh.hpp"
#include "BoundaryConditionsContainer.hpp"
#include "AbstractOdeSystemForCoupledPdeSystem.hpp"
#include "CvodeAdaptor.hpp"
#include "BackwardEulerIvpOdeSolver.hpp"
#include "Warnings.hpp"
#include "VtkMeshWriter.hpp"

#include <boost/shared_ptr.hpp>

/**
 * A class for solving systems of parabolic PDEs and ODEs, which may be coupled
 * via their source terms:
 *
 * d/dt (u_i) = div (D(x) grad (u_i)) + f_i (x, u_1, ..., u_p, v_1, ..., v_q),  i=1,...,p,
 * d/dt (v_j) = g_j(x, u_1, ..., u_p, v_1, ..., v_q),  j=1,...,q.
 *
 * The solver class is templated over spatial dimension and PDE problem dimension (p).
 */
template<unsigned ELEMENT_DIM, unsigned SPACE_DIM=ELEMENT_DIM, unsigned PROBLEM_DIM=1>
class LinearParabolicPdeSystemWithCoupledOdeSystemSolver
    : public AbstractAssemblerSolverHybrid<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, NORMAL>,
      public AbstractDynamicLinearPdeSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>
{
private:

    /** Pointer to the mesh. */
    AbstractTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* mpMesh;

    /** The PDE system to be solved. */
    AbstractLinearParabolicPdeSystemForCoupledOdeSystem<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* mpPdeSystem;

    /** Vector of pointers to ODE systems, defined at nodes. */
    std::vector<AbstractOdeSystemForCoupledPdeSystem*> mOdeSystemsAtNodes;

    /** The values of the ODE system state variables, interpolated at a quadrature point. */
    std::vector<double> mInterpolatedOdeStateVariables;

    /** The ODE solver. */
    boost::shared_ptr<AbstractIvpOdeSolver> mpOdeSolver;

    /**
     * A sampling timestep for writing results to file. Set to
     * PdeSimulationTime::GetPdeTimeStep() in the constructor;
     * may be overwritten using the SetSamplingTimeStep() method.
     */
    double mSamplingTimeStep;

    /** Whether ODE systems are present (if not, then the system comprises coupled PDEs only). */
    bool mOdeSystemsPresent;

    /** Meta results file for VTK. */
    out_stream mpVtkMetaFile;

    /**
     * Whether the output directory should be cleared before solve or not. False by default.
     * Can be changed when setting the output directory
     */
    bool mClearOutputDirectory;

    /**
     * Write the current results to mpVtkMetaFile.
     */
    void WriteVtkResultsToFile();

    /**
     * @return the term to be added to the element stiffness matrix.
     *
     * @param rPhi The basis functions, rPhi(i) = phi_i, i=1..numBases
     * @param rGradPhi Basis gradients, rGradPhi(i,j) = d(phi_j)/d(X_i)
     * @param rX The point in space
     * @param rU The unknown as a vector, u(i) = u_i
     * @param rGradU The gradient of the unknown as a matrix, rGradU(i,j) = d(u_i)/d(X_j)
     * @param pElement Pointer to the element
     */
    c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> ComputeMatrixTerm(
        c_vector<double, ELEMENT_DIM+1>& rPhi,
        c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
        ChastePoint<SPACE_DIM>& rX,
        c_vector<double,PROBLEM_DIM>& rU,
        c_matrix<double, PROBLEM_DIM, SPACE_DIM>& rGradU,
        Element<ELEMENT_DIM, SPACE_DIM>* pElement);

    /**
     * @return the term to be added to the element stiffness vector.
     *
     * @param rPhi The basis functions, rPhi(i) = phi_i, i=1..numBases
     * @param rGradPhi Basis gradients, rGradPhi(i,j) = d(phi_j)/d(X_i)
     * @param rX The point in space
     * @param rU The unknown as a vector, u(i) = u_i
     * @param rGradU The gradient of the unknown as a matrix, rGradU(i,j) = d(u_i)/d(X_j)
     * @param pElement Pointer to the element
     */
    c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> ComputeVectorTerm(
        c_vector<double, ELEMENT_DIM+1>& rPhi,
        c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
        ChastePoint<SPACE_DIM>& rX,
        c_vector<double,PROBLEM_DIM>& rU,
        c_matrix<double,PROBLEM_DIM,SPACE_DIM>& rGradU,
        Element<ELEMENT_DIM, SPACE_DIM>* pElement);

    /**
     * Reset the member variable mInterpolatedOdeStateVariables.
     */
    void ResetInterpolatedQuantities();

    /**
     * Update the member variable mInterpolatedOdeStateVariables by computing the
     * interpolated value of each ODE state variable at each Gauss point.
     *
     * @param phiI
     * @param pNode pointer to a Node
     */
    void IncrementInterpolatedQuantities(double phiI, const Node<SPACE_DIM>* pNode);

    /**
     * Initialise method: sets up the linear system (using the mesh to
     * determine the number of unknowns per row to preallocate) if it is not
     * already set up. Can use an initial solution as PETSc template,
     * or base it on the mesh size.
     *
     * @param initialSolution Initial solution (defaults to NULL) for PETSc to use as a template.
     */
    void InitialiseForSolve(Vec initialSolution=NULL);

    /**
     * Completely set up the linear system that has to be solved each timestep.
     *
     * @param currentSolution The current solution which can be used in setting up
     *  the linear system if needed (NULL if there isn't a current solution)
     * @param computeMatrix Whether to compute the LHS matrix of the linear system
     *   (mainly for dynamic solves).
     */
    void SetupLinearSystem(Vec currentSolution, bool computeMatrix);

public:

    /**
     * Constructor.
     *
     * @param pMesh pointer to the mesh
     * @param pPdeSystem pointer to the PDE system
     * @param pBoundaryConditions pointer to the boundary conditions.
     * @param odeSystemsAtNodes optional vector of pointers to ODE systems, defined at nodes
     * @param pOdeSolver optional pointer to an ODE solver (defaults to NULL)
     */
    LinearParabolicPdeSystemWithCoupledOdeSystemSolver(TetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
                                                       AbstractLinearParabolicPdeSystemForCoupledOdeSystem<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pPdeSystem,
                                                       BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pBoundaryConditions,
                                                       std::vector<AbstractOdeSystemForCoupledPdeSystem*> odeSystemsAtNodes=std::vector<AbstractOdeSystemForCoupledPdeSystem*>(),
                                                       boost::shared_ptr<AbstractIvpOdeSolver> pOdeSolver=boost::shared_ptr<AbstractIvpOdeSolver>());

    /**
     * Destructor.
     * If an ODE system is present, the pointers to the ODE system objects are deleted here.
     */
    ~LinearParabolicPdeSystemWithCoupledOdeSystemSolver();

    /**
     * Overridden PrepareForSetupLinearSystem() method.
     * Pass the current solution to the PDE system to the ODE system and solve it over the next timestep.
     *
     * @param currentPdeSolution the solution to the PDE system at the current time
     */
    void PrepareForSetupLinearSystem(Vec currentPdeSolution);

    /**
     * Set mOutputDirectory.
     *
     * @param outputDirectory the output directory to use
     * @param clearDirectory whether to clear outputDirectory or not. Note that the actual clearing happens when you call SolveAndWriteResultsToFile().
     *                       False by default.
     */
    void SetOutputDirectory(std::string outputDirectory, bool clearDirectory=false);

    /**
     * Set mSamplingTimeStep.
     *
     * @param samplingTimeStep the sampling timestep to use
     */
    void SetSamplingTimeStep(double samplingTimeStep);

    /**
     * Solve the coupled PDE/ODE system over the pre-specified time interval,
     * and record results using mSamplingTimeStep.
     */
    void SolveAndWriteResultsToFile();

    /**
     * Write the solution to VTK. Called by SolveAndWriteResultsToFile().
     *
     * @param solution the solution of the coupled PDE/ODE system
     * @param numTimeStepsElapsed the number of timesteps that have elapsed
     */
    void WriteVtkResultsToFile(Vec solution, unsigned numTimeStepsElapsed);

    /**
     * Get a pointer to the ODE system defined at a given node.
     *
     * @param index the global index of a node in the mpMesh
     * @return mOdeSystemsAtNodes[index]
     */
    AbstractOdeSystemForCoupledPdeSystem* GetOdeSystemAtNode(unsigned index);
};

///////////////////////////////////////////////////////////////////////////////////
// Implementation
///////////////////////////////////////////////////////////////////////////////////

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeMatrixTerm(
    c_vector<double, ELEMENT_DIM+1>& rPhi,
    c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
    ChastePoint<SPACE_DIM>& rX,
    c_vector<double,PROBLEM_DIM>& rU,
    c_matrix<double, PROBLEM_DIM, SPACE_DIM>& rGradU,
    Element<ELEMENT_DIM, SPACE_DIM>* pElement)
{
    double timestep_inverse = PdeSimulationTime::GetPdeTimeStepInverse();
    c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> matrix_term = zero_matrix<double>(PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1));

    // Loop over PDEs and populate matrix_term
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        double this_dudt_coefficient = mpPdeSystem->ComputeDuDtCoefficientFunction(rX, pde_index);
        c_matrix<double, SPACE_DIM, SPACE_DIM> this_pde_diffusion_term = mpPdeSystem->ComputeDiffusionTerm(rX, pde_index, pElement);
        c_matrix<double, 1*(ELEMENT_DIM+1), 1*(ELEMENT_DIM+1)> this_stiffness_matrix =
            prod(trans(rGradPhi), c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>(prod(this_pde_diffusion_term, rGradPhi)) )
                + timestep_inverse * this_dudt_coefficient * outer_prod(rPhi, rPhi);

        for (unsigned i=0; i<ELEMENT_DIM+1; i++)
        {
            for (unsigned j=0; j<ELEMENT_DIM+1; j++)
            {
                matrix_term(i*PROBLEM_DIM + pde_index, j*PROBLEM_DIM + pde_index) = this_stiffness_matrix(i,j);
            }
        }
    }
    return matrix_term;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeVectorTerm(
    c_vector<double, ELEMENT_DIM+1>& rPhi,
    c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
    ChastePoint<SPACE_DIM>& rX,
    c_vector<double,PROBLEM_DIM>& rU,
    c_matrix<double,PROBLEM_DIM,SPACE_DIM>& rGradU,
    Element<ELEMENT_DIM, SPACE_DIM>* pElement)
{
    double timestep_inverse = PdeSimulationTime::GetPdeTimeStepInverse();
    c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> vector_term;
    vector_term = zero_vector<double>(PROBLEM_DIM*(ELEMENT_DIM+1));

    // Loop over PDEs and populate vector_term
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        double this_dudt_coefficient = mpPdeSystem->ComputeDuDtCoefficientFunction(rX, pde_index);
        double this_source_term = mpPdeSystem->ComputeSourceTerm(rX, rU, mInterpolatedOdeStateVariables, pde_index);
        c_vector<double, ELEMENT_DIM+1> this_vector_term;
        this_vector_term = (this_source_term + timestep_inverse*this_dudt_coefficient*rU(pde_index))* rPhi;

        for (unsigned i=0; i<ELEMENT_DIM+1; i++)
        {
            vector_term(i*PROBLEM_DIM + pde_index) = this_vector_term(i);
        }
    }

    return vector_term;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ResetInterpolatedQuantities()
{
    mInterpolatedOdeStateVariables.clear();

    if (mOdeSystemsPresent)
    {
        unsigned num_state_variables = mOdeSystemsAtNodes[0]->GetNumberOfStateVariables();
        mInterpolatedOdeStateVariables.resize(num_state_variables, 0.0);
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::IncrementInterpolatedQuantities(double phiI, const Node<SPACE_DIM>* pNode)
{
    if (mOdeSystemsPresent)
    {
        unsigned num_state_variables = mOdeSystemsAtNodes[0]->GetNumberOfStateVariables();

        for (unsigned i=0; i<num_state_variables; i++)
        {
            mInterpolatedOdeStateVariables[i] += phiI * mOdeSystemsAtNodes[pNode->GetIndex()]->rGetStateVariables()[i];
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::InitialiseForSolve(Vec initialSolution)
{
    if (this->mpLinearSystem == NULL)
    {
        unsigned preallocation = mpMesh->CalculateMaximumContainingElementsPerProcess() + ELEMENT_DIM;
        if (ELEMENT_DIM > 1)
        {
            // Highest connectivity is closed
            preallocation--;
        }
        preallocation *= PROBLEM_DIM;

        /*
         * Use the current solution (ie the initial solution) as the
         * template in the alternative constructor of LinearSystem.
         * This is to avoid problems with VecScatter.
         */
        this->mpLinearSystem = new LinearSystem(initialSolution, preallocation);
    }

    assert(this->mpLinearSystem);
    this->mpLinearSystem->SetMatrixIsSymmetric(true);
    this->mpLinearSystem->SetKspType("cg");
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetupLinearSystem(Vec currentSolution, bool computeMatrix)
{
    this->SetupGivenLinearSystem(currentSolution, computeMatrix, this->mpLinearSystem);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::LinearParabolicPdeSystemWithCoupledOdeSystemSolver(
        TetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
        AbstractLinearParabolicPdeSystemForCoupledOdeSystem<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pPdeSystem,
        BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pBoundaryConditions,
        std::vector<AbstractOdeSystemForCoupledPdeSystem*> odeSystemsAtNodes,
        boost::shared_ptr<AbstractIvpOdeSolver> pOdeSolver)
    : AbstractAssemblerSolverHybrid<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, NORMAL>(pMesh, pBoundaryConditions),
      AbstractDynamicLinearPdeSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>(pMesh),
      mpMesh(pMesh),
      mpPdeSystem(pPdeSystem),
      mOdeSystemsAtNodes(odeSystemsAtNodes),
      mpOdeSolver(pOdeSolver),
      mSamplingTimeStep(DOUBLE_UNSET),
      mOdeSystemsPresent(false),
      mClearOutputDirectory(false)
{
    this->mpBoundaryConditions = pBoundaryConditions;

    /*
     * If any ODE systems are passed in to the constructor, then we aren't just
     * solving a coupled PDE system, in which case the number of ODE system objects
     * must match the number of nodes in the finite element mesh.
     */
    if (!mOdeSystemsAtNodes.empty())
    {
        mOdeSystemsPresent = true;
        assert(mOdeSystemsAtNodes.size() == mpMesh->GetNumNodes());

        /*
         * In this case, if an ODE solver is not explicitly passed into the
         * constructor, then we create a default solver.
         */
        if (!mpOdeSolver)
        {
#ifdef CHASTE_CVODE
            mpOdeSolver.reset(new CvodeAdaptor);
#else
            mpOdeSolver.reset(new BackwardEulerIvpOdeSolver(mOdeSystemsAtNodes[0]->GetNumberOfStateVariables()));
#endif //CHASTE_CVODE
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::~LinearParabolicPdeSystemWithCoupledOdeSystemSolver()
{
    if (mOdeSystemsPresent)
    {
        for (unsigned i=0; i<mOdeSystemsAtNodes.size(); i++)
        {
            delete mOdeSystemsAtNodes[i];
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::PrepareForSetupLinearSystem(Vec currentPdeSolution)
{
    if (mOdeSystemsPresent)
    {
        double time = PdeSimulationTime::GetTime();
        double next_time = PdeSimulationTime::GetNextTime();
        double dt = PdeSimulationTime::GetPdeTimeStep();

        ReplicatableVector soln_repl(currentPdeSolution);
        std::vector<double> current_soln_this_node(PROBLEM_DIM);

        // Loop over nodes
        for (unsigned node_index=0; node_index<mpMesh->GetNumNodes(); node_index++)
        {
            // Store the current solution to the PDE system at this node
            for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
            {
                double current_soln_this_pde_this_node = soln_repl[PROBLEM_DIM*node_index + pde_index];
                current_soln_this_node[pde_index] = current_soln_this_pde_this_node;
            }

            // Pass it into the ODE system at this node
            mOdeSystemsAtNodes[node_index]->SetPdeSolution(current_soln_this_node);

            // Solve ODE system at this node
            mpOdeSolver->SolveAndUpdateStateVariable(mOdeSystemsAtNodes[node_index], time, next_time, dt);
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetOutputDirectory(std::string outputDirectory, bool clearDirectory)
{
    mClearOutputDirectory = clearDirectory;
    this->mOutputDirectory = outputDirectory;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetSamplingTimeStep(double samplingTimeStep)
{
    assert(samplingTimeStep >= this->mIdealTimeStep);
    mSamplingTimeStep = samplingTimeStep;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SolveAndWriteResultsToFile()
{
    // A number of methods must have been called prior to this method
    if (this->mOutputDirectory == "")
    {
        EXCEPTION("SetOutputDirectory() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (this->mTimesSet == false)
    {
        EXCEPTION("SetTimes() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (this->mIdealTimeStep <= 0.0)
    {
        EXCEPTION("SetTimeStep() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (mSamplingTimeStep == DOUBLE_UNSET)
    {
        EXCEPTION("SetSamplingTimeStep() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (!this->mInitialCondition)
    {
        EXCEPTION("SetInitialCondition() must be called prior to SolveAndWriteResultsToFile()");
    }

#ifdef CHASTE_VTK
    // Create a .pvd output file
    OutputFileHandler output_file_handler(this->mOutputDirectory, mClearOutputDirectory);
    mpVtkMetaFile = output_file_handler.OpenOutputFile("results.pvd");
    *mpVtkMetaFile << "<?xml version=\"1.0\"?>\n";
    *mpVtkMetaFile << "<VTKFile type=\"Collection\" version=\"0.1\" byte_order=\"LittleEndian\" compressor=\"vtkZLibDataCompressor\">\n";
    *mpVtkMetaFile << "    <Collection>\n";

    // Write initial condition to VTK
    Vec initial_condition = this->mInitialCondition;
    WriteVtkResultsToFile(initial_condition, 0);

    // The helper class TimeStepper deals with issues such as small final timesteps so we don't have to
    TimeStepper stepper(this->mTstart, this->mTend, mSamplingTimeStep);

    // Main time loop
    while (!stepper.IsTimeAtEnd())
    {
        // Reset start and end times
        this->SetTimes(stepper.GetTime(), stepper.GetNextTime());

        // Solve the system up to the new end time
        Vec soln = this->Solve();

        // Reset the initial condition for the next timestep
        if (this->mInitialCondition != initial_condition)
        {
            PetscTools::Destroy(this->mInitialCondition);
        }
        this->mInitialCondition = soln;

        // Move forward in time
        stepper.AdvanceOneTimeStep();

        // Write solution to VTK
        WriteVtkResultsToFile(soln, stepper.GetTotalTimeStepsTaken());
    }

    // Restore saved initial condition to avoid user confusion!
    if (this->mInitialCondition != initial_condition)
    {
        PetscTools::Destroy(this->mInitialCondition);
    }
    this->mInitialCondition = initial_condition;

    // Close .pvd output file
    *mpVtkMetaFile << "    </Collection>\n";
    *mpVtkMetaFile << "</VTKFile>\n";
    mpVtkMetaFile->close();
#else //CHASTE_VTK
// LCOV_EXCL_START // We only test this in weekly builds
    WARNING("VTK is not installed and is required for this functionality");
// LCOV_EXCL_STOP
#endif //CHASTE_VTK
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::WriteVtkResultsToFile(Vec solution, unsigned numTimeStepsElapsed)
{
#ifdef CHASTE_VTK

    // Create a new VTK file for this time step
    std::stringstream time;
    time << numTimeStepsElapsed;
    VtkMeshWriter<ELEMENT_DIM, SPACE_DIM> mesh_writer(this->mOutputDirectory, "results_"+time.str(), false);

    /*
     * We first loop over PDEs. For each PDE we store the solution
     * at each node in a vector, then pass this vector to the mesh
     * writer.
     */
    ReplicatableVector solution_repl(solution);
    unsigned num_nodes = mpMesh->GetNumNodes();
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        // Store the solution of this PDE at each node
        std::vector<double> pde_index_data;
        pde_index_data.resize(num_nodes, 0.0);
        for (unsigned node_index=0; node_index<num_nodes; node_index++)
        {
            pde_index_data[node_index] = solution_repl[PROBLEM_DIM*node_index + pde_index];
        }

        // Add this data to the mesh writer
        std::stringstream data_name;
        data_name << "PDE variable " << pde_index;
        mesh_writer.AddPointData(data_name.str(), pde_index_data);
    }

    if (mOdeSystemsPresent)
    {
        /*
         * We cannot loop over ODEs like PDEs, since the solutions are not
         * stored in one place. Therefore we build up a large 'vector of
         * vectors', then pass each component of this vector to the mesh
         * writer.
         */
        std::vector<std::vector<double> > ode_data;
        unsigned num_odes = mOdeSystemsAtNodes[0]->rGetStateVariables().size();
        ode_data.resize(num_odes);
        for (unsigned ode_index=0; ode_index<num_odes; ode_index++)
        {
            ode_data[ode_index].resize(num_nodes, 0.0);
        }

        for (unsigned node_index=0; node_index<num_nodes; node_index++)
        {
            std::vector<double> all_odes_this_node = mOdeSystemsAtNodes[node_index]->rGetStateVariables();
            for (unsigned i=0; i<num_odes; i++)
            {
                ode_data[i][node_index] = all_odes_this_node[i];
            }
        }

        for (unsigned ode_index=0; ode_index<num_odes; ode_index++)
        {
            std::vector<double> ode_index_data = ode_data[ode_index];

            // Add this data to the mesh writer
            std::stringstream data_name;
            data_name << "ODE variable " << ode_index;
            mesh_writer.AddPointData(data_name.str(), ode_index_data);
        }
    }

    mesh_writer.WriteFilesUsingMesh(*mpMesh);
    *mpVtkMetaFile << "        <DataSet timestep=\"";
    *mpVtkMetaFile << numTimeStepsElapsed;
    *mpVtkMetaFile << "\" group=\"\" part=\"0\" file=\"results_";
    *mpVtkMetaFile << numTimeStepsElapsed;
    *mpVtkMetaFile << ".vtu\"/>\n";
#endif // CHASTE_VTK
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
AbstractOdeSystemForCoupledPdeSystem* LinearParabolicPdeSystemWithCoupledOdeSystemSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::GetOdeSystemAtNode(unsigned index)
{
    return mOdeSystemsAtNodes[index];
}

#endif /*LINEARPARABOLICPDESYSTEMWITHCOUPLEDODESYSTEMSOLVER_HPP_*/


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <iostream>
#include <string>
#include <complex>
#include <vector>
#include <limits>
#include <time.h>
#include <unistd.h>
#include <proj_api.h>
#include <sys/stat.h>
#include <sys/types.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;
#include <boost/filesystem/operations.hpp>
namespace fs = boost::filesystem;

#include <vw/Core.h>
#include <vw/Image.h>
#include <vw/FileIO.h>
#include <vw/Cartography.h>
#include <vw/Math.h>
using namespace vw;
using namespace vw::cartography;

#include <vw/Photometry/Reconstruct.h>
#include <vw/Photometry/Misc.h>
#include <vw/Photometry/Shape.h>
using namespace vw::photometry;

//upsamples a geo referenced tiff image by four- used in sfs

void upsample_image(std::string output_file, std::string input_file, int upsampleFactor) {
  GeoReference geo;
  read_georeference(geo, input_file);
  DiskImageView<PixelGray<float> >   image(input_file);

  int cols = (image.cols())*upsampleFactor, rows = (image.rows())*upsampleFactor;
  ImageView<PixelGray<float> >  tm_image(cols, rows);

  ImageViewRef<PixelGray<float> >   interp = interpolate(edge_extend(image.impl(),
                                                                     ConstantEdgeExtension()),
                                                         BilinearInterpolation());

  int x, y;

  for (x=0; x<cols; ++x){
    for (y=0; y<rows; ++y){
      //if ( is_valid(image(2*x,2*y)) || is_valid(image(2*x+1,2*y)) || is_valid(image(2*x,2*y+1)) || is_valid(image(2*x+1,2*y+1)) ) {
      //if (is_valid(image(2*x,2*y)) && is_valid(image(2*x+1,2*y)) && is_valid(image(2*x,2*y+1)) && is_valid(image(2*x+1,2*y+1)) ) {
      float xx = x/upsampleFactor;
      float yy = y/upsampleFactor;

      if ( interp(x, y) != -10000 ){
           tm_image(x,y) = interp(x, y);
      }
      else{
           tm_image(x,y) = -10000;
      }
    }
  }

  Matrix<double> H = geo.transform();
  H(0,0) /= upsampleFactor;
  H(1,1) /= upsampleFactor;
  geo.set_transform(H);

  write_georeferenced_image(output_file, tm_image, geo, TerminalProgressCallback("photometry","Processing:"));
}

//subsamples a geo referenced tiff image by two
void subsample_image(std::string output_file, std::string input_file) {
  GeoReference geo;
  read_georeference(geo, input_file);
  DiskImageView<PixelMask<PixelGray<uint8> > >  image(input_file);
  int cols = (image.cols()+1)/2, rows = (image.rows()+1)/2;
  ImageView<PixelMask<PixelGray<uint8> > > tm_image(cols, rows);

  ImageViewRef<PixelMask<PixelGray<uint8> > >  interp = interpolate(edge_extend(image.impl(),
                                                                                ConstantEdgeExtension()),
                                                                    BilinearInterpolation());

  int x, y;

  for (x=0; x<cols; ++x){
    for (y=0; y<rows; ++y){
      //if ( is_valid(image(2*x,2*y)) || is_valid(image(2*x+1,2*y)) || is_valid(image(2*x,2*y+1)) || is_valid(image(2*x+1,2*y+1)) ) {
      //if (is_valid(image(2*x,2*y)) && is_valid(image(2*x+1,2*y)) && is_valid(image(2*x,2*y+1)) && is_valid(image(2*x+1,2*y+1)) ) {
      if ( is_valid(interp(2*x+0.5, 2*y+0.5)) ){
        tm_image(x,y) = interp(2*x+0.5, 2*y+0.5);
      }
      else{
        tm_image(x,y).invalidate();
      }
    }
  }

  Matrix<double> H = geo.transform();
  H(0,0) *= 2;
  H(1,1) *= 2;
  geo.set_transform(H);

  write_georeferenced_image(output_file, tm_image, geo, TerminalProgressCallback("photometry","Processing:"));
}

// Given two images and two georeferences, this function picks a set
// of matching pixel samples between the two images.  It rejects
// pixels that are not valid, and it should probably also reject
// pixels that are near saturation (though it does not yet!).
template <class ViewT>
std::vector<Vector4> sample_images(ImageViewBase<ViewT> const& image1,
                                   ImageViewBase<ViewT> const& image2,
                                   GeoReference const& geo1,
                                   GeoReference const& geo2,
                                   int num_samples,
                                   std::string const& DEM_file,
                                   std::vector<Vector3> *normalArray,
                                   std::vector<Vector3> *xyzArray ) {
  int sample = 0;
  int numtries = 0;
  std::vector<Vector4> result;

  // Random numbers
  srandom((unsigned int) clock());

  ImageViewRef<typename ViewT::pixel_type> interp_image1 = interpolate(edge_extend(image1.impl(),
                                                                       ConstantEdgeExtension()),
                                                                       BilinearInterpolation());
  ImageViewRef<typename ViewT::pixel_type> interp_image2 = interpolate(edge_extend(image2.impl(),
                                                                       ConstantEdgeExtension()),
                                                                       BilinearInterpolation());

  // This block of code samples the images comprehensively, adding a
  // sample pair for every valid pixel in interp_image1.
  // for (unsigned j=0; j < interp_image1.rows(); ++j) {
  //   for (unsigned i=0; i < interp_image1.cols(); ++i) {
  //     Vector2 sample_pix1(i,j);
  //     Vector2 sample_pix2 = geo2.lonlat_to_pixel(geo1.pixel_to_lonlat(sample_pix1));

  //     // Check to see whether these pixels are valid
  //     typename ViewT::pixel_type pix1 = interp_image1(sample_pix1[0], sample_pix1[1]);
  //     typename ViewT::pixel_type pix2 = interp_image2(sample_pix2[0], sample_pix2[1]);
  //     if ( is_valid(pix1) && is_valid(pix2) &&
  //          pix1[0] > 10 && pix1[0] < 245 &&
  //          pix2[0] > 10 && pix2[0] < 245 ) {
  //       result.push_back(Vector2(pix1[0],pix2[0]));
  //       ++sample;
  //       //        std::cout << result[result.size()-1][0] << " " << result[result.size()-1][1] << "\n";
  //     }
  //     ++numtries;
  //   }
  // }

  //added by Ara to support DEMs - START
  DiskImageView<PixelGray<float> >  dem_image(DEM_file);
  GeoReference GR;
  read_georeference(GR, DEM_file);
  //added by Ara to support DEMs - END

  // This block of code samples the images randomly, gathering up to
  // num_samples samples from the images.
  while (sample < num_samples && numtries < num_samples*10) {

    Vector2 sample_pix1(float(random())/RAND_MAX * image1.impl().cols(),
                        float(random())/RAND_MAX * image1.impl().rows());
    Vector2 sample_pix2 = geo2.lonlat_to_pixel(geo1.pixel_to_lonlat(sample_pix1));

    Vector2 sample_pix_dem = GR.lonlat_to_pixel(geo1.pixel_to_lonlat(sample_pix1));

    Vector2 lonlat = geo1.pixel_to_lonlat(sample_pix1);

    // Check to see whether these pixels are valid
    typename ViewT::pixel_type pix1 = interp_image1(sample_pix1[0], sample_pix1[1]);
    typename ViewT::pixel_type pix2 = interp_image2(sample_pix2[0], sample_pix2[1]);
    if ( is_valid(pix1) && is_valid(pix2) ) {

       //result.push_back(Vector4(pix1[0],pix2[0],lonlat[0],lonlat[1]));

       int x = (int)sample_pix_dem[0];
       int y = (int)sample_pix_dem[1];

       if (x < 0){
           x = 0;
       }
       if (x > dem_image.cols()-1){
           x = dem_image.cols()-1;
       }
       if (y < 0){
           y = 0;
       }
       if (y > dem_image.rows()-1){
           y = dem_image.rows()-1;
       }

       Vector3 longlat3(lonlat(0),lonlat(1),(dem_image)(x, y));
       Vector3 xyz = geo1.datum().geodetic_to_cartesian(longlat3);

       Vector2 sample_pix_dem_left;
       sample_pix_dem_left(0) = x-1;
       if (sample_pix_dem_left(0) < 0){
          sample_pix_dem_left(0) = 0;
          //break;
       }
       sample_pix_dem_left(1) = y;
       lonlat = GR.pixel_to_lonlat(sample_pix_dem_left);

       Vector3 longlat3_left(lonlat(0),lonlat(1),(dem_image)(sample_pix_dem_left(0), sample_pix_dem_left(1)));
       Vector3 xyz_left = geo1.datum().geodetic_to_cartesian(longlat3_left);

       Vector2 sample_pix_dem_top;
       sample_pix_dem_top(0) = x;
       sample_pix_dem_top(1) = y-1;
       if (sample_pix_dem_top(1) < 0){
         sample_pix_dem_top(1) = 0;
         //break;
       }

       lonlat = GR.pixel_to_lonlat(sample_pix_dem_top);
       Vector3 longlat3_top(lonlat(0),lonlat(1),(dem_image)(sample_pix_dem_top(0), sample_pix_dem_top(1)));
       Vector3 xyz_top = geo1.datum().geodetic_to_cartesian(longlat3_top);

       Vector3 normal = computeNormalFrom3DPoints(xyz, xyz_left, xyz_top);

       //printf("normal:%f %f %f\n", normal(0), normal(1), normal(2));
       //printf("%f %f %f\n", loc_longlat3(0), loc_longlat3(1), loc_longlat3(2));

       result.push_back(Vector4(pix1[0],pix2[0],lonlat[0],lonlat[1]));

       normalArray->push_back(normal);
       xyzArray->push_back(xyz);

       ++sample;
       //      std::cout << result[result.size()-1][0] << " " << result[result.size()-1][1] << "\n";
    }
    ++numtries;
  }
  return result;
}
/*
/// Erases a file suffix if one exists and returns the base string
static std::string prefix_from_filename(std::string const& filename) {
        std::string result = filename;
        int index = result.rfind(".");
        if (index != -1)
                result.erase(index, result.size());
        return result;
}

/// Erases a file suffix if one exists and returns the base string less3 characters
static std::string prefix_less3_from_filename(std::string const& filename) {
  std::string result = filename;
  int index = result.rfind(".");
  if (index != -1)
    result.erase(index-3, result.size()+3);
  return result;
}

/// Erases a file suffix if one exists and returns the base string less3 characters
static std::string sufix_from_filename(std::string const& filename) {
  std::string result = filename;
  int index = result.rfind("/");
  if (index != -1)
    result.erase(0, index);
  return result;
}
*/

//reads the tiff DEM into a 3D coordinate
//pos is a Vector2 of pixel coordinates, GR is georeference
template <class ViewT>
Vector3 pixel_to_cart (Vector2 pos, ImageViewBase<ViewT> const& img,  GeoReference GR) {
    Vector2 loc_longlat2=GR.point_to_lonlat(GR.pixel_to_point(pos));
    Vector3 loc_longlat3(loc_longlat2(0),loc_longlat2(1),img((int)pos[0],(int)pos[1]));
    Vector3 loc_cartesian=GR.datum().geodetic_to_cartesian(loc_longlat3);
    return loc_cartesian;
}

// Create the output, index, and radiance file names

std::vector<std::string> parse_command_arguments(int argc, char *argv[] ) {
        int num_matches;
        std::vector<std::string> input_files;

        po::options_description general_options("Options");
        general_options.add_options()
        ("help", "Display this help message")
        ("num-matches,m", po::value<int>(&num_matches)->default_value(1000), "Number of points to match for linear regression.");

        po::options_description hidden_options("");
        hidden_options.add_options()
        ("input-files", po::value<std::vector<std::string> >(&input_files));

        po::options_description options("Allowed Options");
        options.add(general_options).add(hidden_options);

        po::positional_options_description p;
        p.add("input-files", -1);

        po::variables_map vm;
        po::store( po::command_line_parser( argc, argv ).options(options).positional(p).run(), vm );
        po::notify( vm );

        std::ostringstream usage;
        usage << "Description: tonematches several images" << std::endl << std::endl;
        usage << "Usage: histeq [options] <filename1> <filename2> ..." << std::endl << std::endl;
        usage << general_options << std::endl;

        if( vm.count("help") ) {
                std::cerr << usage.str() << std::endl;
                exit(1);
        }

        if( vm.count("input-files")<1 ) {
                std::cerr << "Error: Must specify at least one input file!" << std::endl << std::endl;
                std::cerr << usage.str();
                exit(1);
        }

        return input_files;
}