#include <Rcpp.h>
#include <boost/multi_array.hpp>

using namespace Rcpp;

// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::depends(BH)]]

//'Estimate item parameter for binary{0,1} response data.
//'
//'1PL,2PL,3PL,Bayes1PL,Bayes2PL and multigroup estimation is avairable now. U must install C++ compiler(Rtools for windows or Xcode for Mac)in your PC or Mac.
//'@param x an item response data which class is data.frame object.
//'@param model0 Character.U can select which one, "1PL","2PL","3PL".
//'@param N the number of nodes in integration.
//'@param bg0 the number of base grade.
//'@param eMLL a convergence criteria(CC) for marginal log likelihood.
//'@param eEM a CC in EM cycle.
//'@param eM a CC in M step.
//'@param emu a CC for population distribution mean.
//'@param esd a CC for population distribution standard deviation.
//'@param fc0 a column of first item response.
//'@param ng the number of groups
//'@param gc0 a column of group. the element must be integer and the minimum number must be 1.
//'@param D a scaling constant.
//'@param fix Don't use. If 1, fix population distribution mean and sigma each EM cycle.
//'@param print How much information you want to display? from 1 to 3. The larger, more information is displayed.
//'@param ic initial value for guessing parameter.
//'@param max maximum value of theta in integration.
//'@param min minimum value of theta in integration.
//'@param mu hyperparameter for theta dist.
//'@param sigma same as above.
//'@param Bayes If 1, marginal Bayesian estimation runs.
//'@param method Optimising method in M step. "Fisher_Scoring" or "Newton_Raphson" can be selected.
//'@param mu_a hyperparameter of log normal dist for slope parameter.
//'@param sigma_a same as above.
//'@param mu_b hyperparameter of normal dist for location parameter.
//'@param sigma_b same as above.
//'@param mu_c hyperparameter for lower asymptote parameter.
//'@param w_c weight of a Beta dist.
//'@param min_a minimum value of slope parameter.
//'@param maxabs_b maximum absolute value of location parameter.
//'@param maxiter_em maximum iteration time for EM cycle.
//'@param maxiter_j maximum iteration time for Newton Raphton in M step.
//'@param maxskip_j Dont use.
//'@param rm_list a vector of item U want to remove for estimation. NOT list.
//'@param thdist Which distribution do you want `normal` or `empirical` for E step.
//'@param estdist If 1, calculate esimated population distribution via EM argorithm.
//'@examples
//'res <- estip(x=sim_data_1, fc=2)
//'# check the parameters
//'res$para
//'res$SE
//'@export
// [[Rcpp::export]]

List estip (DataFrame x,
            CharacterVector model0 = CharacterVector::create("2PL") ,
            const int N = 31,
            const int bg0 = 1,
            int fc0 = 2,
            int ng = 1,
            int gc0 = 2,
            const double eMLL = 1e-6,
            const double eEM = 1e-4,
            const double eM = 1e-3,
            const double emu = 1e-3,
            const double esd = 1e-3,
            const double D = 1.702,
            const double ic = 1/5,
            const double max = 6.0,
            const double min = -6.0,
            const double mu = 0,
            const double sigma = 1,
            const int Bayes = 0,
            const String method = "Fisher_Scoring",
            const double mu_a = 0,
            const double sigma_a = 1,
            const double mu_b = 0,
            const double sigma_b = 2,
            const double mu_c = 1/5,
            const double w_c = 5,
            const int fix = 1,
            const int print = 0,
            const double min_a = 0.1,
            const double maxabs_b = 20,
            const int maxiter_em = 200,
            const int maxiter_j = 20,
            const int maxskip_j = 0,
            CharacterVector rm_list = CharacterVector::create("NONE"),
            const String thdist = "normal",
            const int estdist = 1
){
  // argument check
  for(int j=0; j<model0.length(); j++){
    Rprintf("Checking the model string vector %i !\r", j+1);
    if(model0[j] != "1PL" && model0[j] != "2PL" && model0[j] != "3PL") stop("Errpr! option string of 'model' is incorrect. You should select '1~3PL'.");
  }
  Rprintf("\n");
  if(thdist != "normal" && thdist != "empirical") stop("Errpr! option string of 'thdist' is incorrect. You should select 'normal' or 'empirical'.");

  struct LocalFunc{ // R function define

    static NumericVector colSums_cpp(NumericMatrix mat){ // R::colSums
      Function f("colSums");
      return f(mat, _["na.rm"] = true);
    }

    static NumericVector rowSums_cpp(NumericMatrix mat){ // R::rowSums
      Function f("rowSums");
      return f(mat, _["na.rm"] = true);
    }

    static NumericVector cor_cpp(NumericVector vec, NumericMatrix mat){ //R::cor
      Function f("cor");
      return f(vec, _["y"] = mat, _["use"] = "pairwise.complete.obs");
    }

    static NumericVector colMeans_cpp(NumericMatrix mat){ // R::colMeans
      Function f("colMeans");
      return f(mat, _["na.rm"] = true);
    }
  };

  const int lc = x.length();// item n
  const int fc = fc0 -1;// Rcpp上では0から要素数がカウントされるので，数値を調整する。

  // 入力データの確認
  const int nj = lc - fc;
  const int ni = x.nrows(); // subject n
  const int gc = gc0 -1; // the column of group n
  const int bg = bg0 -1; // base grade

  IntegerVector group (ni,1);
  if(ng != 1) group = x[gc];
  group = group -1; // 集団変数を，C++の要素数に適合するように，-1する。

  NumericMatrix xall (ni, nj);
  CharacterVector Item_temp = x.names();
  CharacterVector Item (nj);
  for (int j=0; j<nj; j++){ // もとのデータフレームから項目反応パタンだけを抜き出し，行列として保存
    int jj = j + fc;
    NumericVector kk = x[jj];
    Item[j] = Item_temp[jj];
    for(int i=0; i<ni; i++){
      double k = kk[i];
      xall(i,j) = k;
    }
  }

  // 削除した項目の列番号（データフレーム）を取得
  IntegerVector rm_id(1,9999);
  int rm_n = 0;

  if(rm_list[0] != "NONE"){
    rm_n = rm_list.length();  // 削除項目
    Rcout<<"You select Item; "<<rm_list<<" as not use.\n";
    rm_id = IntegerVector(rm_n); // result
    for(int l=0; l<rm_n; l++){
      String r = rm_list[l];
      rm_id[l] = x.findName(r)-(fc0-1);
    }
  } else {

  }

  // 欠測値がある箇所を0，そうではない箇所を1とした行列を作成
  NumericMatrix resp (ni,nj);
  for(int i=0; i<ni; i++){
    for(int j=0; j<nj; j++){
      if(NumericVector::is_na(xall(i,j))) resp(i,j) = 0; // NAならば0を
      else resp(i,j) = 1; // NA以外（0か1）ならば1を
    }
  }

  NumericMatrix ind (ng,nj); // 当該集団が，項目に回答したか否か
  for(int g=0; g<ng; g++){
    // 集団が受検した項目を確認
    for(int i=0; i<ni; i++){
      if(group[i] != g) continue;
      for(int j=0; j<nj; j++){
        int q = ind(g,j);
        ind(g,j)= q + resp(i,j);
      }
    }

    for( int j=0; j<nj; j++){
      int q = ind(g,j);
      if(q != 0) ind(g,j) = 1; // 誰か一人でも回答していたら1，集団内が全員無回答なら0
    }
  }

  Rcout << "The number of subject is " << ni << ".\nThe number of item is " << nj-rm_n <<".\nThe number of remove item is "<<rm_n<<".\n";

  // initial values
  CharacterVector model (nj, model0[0]);
  const int nmodel = model0.length();
  if(nmodel == nj){
    Rprintf("You selected model variable yourself.\n");
    for(int j=0; j<nj; j++){
      model[j] = model0[j];
    }
  }
  IntegerVector cat (nj);
  NumericMatrix t0 (nj, 3); // 初期値代入用の行列
  NumericMatrix t0m (nj,3);
  NumericMatrix initial (nj,3);
  //NumericMatrix t1m (nj,cat);

  NumericVector rs = LocalFunc::rowSums_cpp(xall);
  NumericVector r  = LocalFunc::cor_cpp(rs, xall);
  NumericVector p  = LocalFunc::colMeans_cpp(xall);

  double a0, b0, c0;
  for(int j=0; j<nj; j++){

    if(model[j] == "1PL") cat[j] = 1;
    if(model[j] == "2PL") cat[j] = 2;
    if(model[j] == "3PL") cat[j] = 3;

    a0 = D*r[j]/sqrt(1-r[j]*r[j]); // P.BIS
    b0 = -log(p[j]/(1.0-p[j]));
    c0 = ic;

    if(a0 < 0.1) a0 = 0.3; // 初期値が小さすぎる場合に，大きい値に補正する。

    // a parameter
    if(model[j] == "2PL" || model[j] == "3PL"){
      t0(j,0) =  a0;
      t0m(j,0) = a0;
      initial(j,0) = a0;
    } else {
      t0(j,0) = 1.702/D;
      t0m(j,0) = 1.702/D;
      initial(j,0) = 1.702/D;
    }
    // b parameter
    t0(j,1) =  b0;
    t0m(j,1) = b0;
    initial(j,1) = b0;

    if(model[j] == "3PL"){
      // c parameter
      t0(j,2) =  c0;
      t0m(j,2) = c0;
      initial(j,2) = c0;
    }

  }

  double alpha_c = w_c*mu_c + 1; // 識別力の事前分布計算に用いる定数
  double beta_c = w_c*(1-mu_c) + 1;
  // default alpha=2, beta=5

  // 削除項目はすべて計算をスキップするので
  if(rm_list[0] != "NONE"){
    for(int l : rm_id){
      for(int g=0; g<ng; g++){
        ind(g,l) = 0;
      }
      for(int i=0; i<ni; i++){
        resp(i,l) = 0;
      }
      for(int p=0; p<3; p++){
        t0(l,p) = 0;
        t0m(l,p) = 0;
        initial(l,p) = 0;
      }
    }
  }

  // prior distribution
  double seq = (max - min)/(N-1); // 分点の間隔。これは全母集団で固定。
  NumericVector Xm (N);
  Xm[0] = min;
  for(int m=1; m<N; m++){
    double pre = Xm[m-1];
    Xm[m] = pre + seq;
  }

  // 母集団分布の初期値を決定。
  NumericMatrix Wm(N,ng); // 母集団ごとに事前分布の重みを計算（初期値なので，どの集団もおなじ
  NumericMatrix Um (N,ng); // 推定母集団分布計算用
  NumericVector mean (ng);
  NumericVector sd (ng);
  NumericMatrix dist (N,ng+1);
  dist(_,0) = Xm;
  mean = mean + mu;
  sd = sd + sigma;

  // 結果代入用の行列，ベクトルを準備　＝　こうしないと，最終的にスコープできない。
  boost::multi_array <double, 3> Gim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Lim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Nm (boost::extents[ng][nj][N]);
  boost::multi_array <double, 3> rjm (boost::extents[ng][nj][N]);
  LogicalVector conv (sum(cat)); // 収束判定代入用ベクトル。
  // 周辺対数尤度代入用ベクトル
  NumericVector MLL(1);
  double diff = 0;
  NumericMatrix SE (nj,3); // 推定の標準誤差代入用
  NumericMatrix skip_para (nj,3); // 推定に失敗した項目番号の代入用

  /////////////////////////
  // EM step starts here //
  /////////////////////////
  Rprintf("Start calculating estimated item parameters.\n");
  //convergence criteria
  int conv1 = 0; // for EM step

  //n of iteraiton
  int count1 = 0;
  int count2 = 0;

  while(conv1 != 1){ // EM step

    ///////////////////////
    //for debug.///////////
    //break; //////////////
    ///////////////////////

    // check cancel command Ctrl + c
    Rcpp::checkUserInterrupt();

    count1 = count1 + 1; // count up

    checkUserInterrupt(); // If user input Ctrl + c, stop EM cycle.

    if(print>=1)Rcout <<count1 << " times E-step calculation. \r";

    // 事前分布の重みを，母集団ごとに計算（前回のMステップの計算結果を考慮する。
    if(thdist == "normal" || (thdist == "empirical" && count1 == 1)){
      for(int g=0; g<ng; g++){
        double dn = 0;
        NumericVector dnn (N);
        for(int m=0; m<N; m++){
          dnn[m] = R::dnorm(Xm[m], mean[g], sd[g], false);
          dn = dn + dnn[m];
        }
        for(int m=0; m<N; m++){
          double wn = dnn[m] / dn;
          Wm(m,g) = wn;
        }
        dist(_,g+1) = Wm(_,g);
      }
    } else if(thdist == "empirical" && count1 != 1){
      for(int g=0; g<ng; g++){
        for(int m=0; m<N; m++){
          Wm(m,g) = Um(m,g);
        }
        dist(_,g+1) = Wm(_,g);
      }
    }

    //Rcout<<"likelihood of all subject in each nodes calculation.\n";
    // 受検者iの項目反応パタンに基づく尤度を，求積点ごとに求める。

    double a,b,c,t,tt,x,u;
    int g,i,m,j;

    for(g=0; g<ng; g++){
      for(i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        for(m=0; m<N; m++){
          t = 1;
          x = Xm[m];
          tt = 0;
          for(j=0; j<nj; j++){
            //if(resp(i,j)==0) continue; // NAの反応パタンのところは計算ループから外れる。
            a = t0(j,0);
            if(a == 0) continue;
            b = t0(j,1);
            c = t0(j,2);
            u = xall(i,j);
            if(u == 1){ // 正答した場合の尤度
              tt = c+(1.0-c)/(1.0+exp(-D*a*(x-b)));
            } else if (u == 0) { // 誤答した場合の尤度
              tt = 1.0-(c+(1.0-c)/(1.0+exp(-D*a*(x-b))));
            } else { // 欠測値の場合は尤度を計算しないので，1
              tt = 1;
            }
            // std::powやlog，expの関数を持ってくると急激に遅くなる。それよりかif文の方がいくらか高速。
            //tt =
            //  std::pow(c+(1.0-c)/(1.0+exp(-D*a*(x-b))),u)* // log of correct probabbility
            //  std::pow(1.0-(c+(1.0-c)/(1.0+exp(-D*a*(x-b)))),(1.0-u)); // log of incorrect probability
            t = t * tt; // sum
          }
          //boost::multi_array<double, 3>::index idx = {{g,i,m}};
          Lim[g][i][m] = t;
        }
      }
    }

    //Rcout<<"marginal log likelihood calculation.\n";
    // Calculation marginal log likelihood

    double f = 0; // 周辺対数尤度代入用
    double ff,fff,l,w;

    for(int g=0; g<ng; g++){
      for(int i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        ff = 0; // 受検者iの周辺対数尤度代入用
        for(int m=0; m<N; m++){
          l = Lim[g][i][m];
          w = Wm(m,g);
          fff =  l*w ;
          ff += fff;
        }
        f += log(ff);
        for(int m=0; m<N; m++){
          l = Lim[g][i][m];
          w = Wm(m,g);
          Gim[g][i][m] =  l * w / ff;
        }
      }
    }

    MLL.push_back(f);
    diff = fabs(MLL[count1-1] - MLL[count1]); // 周辺対数尤度の差

    //Rcout << "-2 Marginal Log Likelihood is " << -2 * f <<"\r";
    Rprintf("%d times -2 Marginal Log Likelihood is %f \r",count1,-2*f); // 小数点形式の出力に対応

    if(traits::is_nan<REALSXP>(f)){
      // 対数尤度の計算に失敗したら，計算を中止する。
      stop("Can't calculate marginal log likelihood.");
    }

    //Rcout<<"weight of a posterir distribution calculation.\n";
    // 各受検者のthetaごとに事後分布の重みを計算する。
    // double uu;
    // for(int g=0; g<ng; g++){
    //   for(int i=0; i<ni; i++){
    //     if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
    //     u = 0;
    //     for(int m=0; m<N; m++){ // 総和を1にするための分母の計算 // sum
    //       l = Lim[g][i][m];
    //       w = Wm(m,g);
    //       uu = l*w;
    //       u += uu;
    //     }
    //     for(int m=0; m<N; m++){
    //       l = Lim[g][i][m];
    //       w = Wm(m,g);
    //       Gim[g][i][m] =  l * w / u;
    //     }
    //   }
    // }

    //Rcout<<"expected frequency of subjects in each nodes calculation.\n";
    double k,kk;
    for(int g=0; g<ng; g++){
      for(int j=0; j<nj; j++){ // 各分点の期待度数
        if(ind(g,j) == 0) continue;
        for(int m=0; m<N; m++){
          k = 0;
          for(int i=0; i<ni; i++){ // 欠測値がある場合，項目ごとに受検者数が異なる。
            if(resp(i,j)==0) continue;
            //double d = resp(i,j);
            kk = Gim[g][i][m];
            k += kk; //* d;
          }
          Nm[g][j][m]= k;
        }
      }
    }

    //Rcout<<"expected freqency of correct response number in each nodes calculation.\n";

    double h,gg,hh;
    for(int g=0; g<ng; g++){
      for(int j=0; j<nj; j++){ // 各分点の正答受検者の期待度数
        if(ind(g,j) == 0) continue;
        for(int m=0; m<N; m++){
          h = 0;
          for(int i=0; i<ni; i++){ // sum
            //double d = resp(i,j);
            if(resp(i,j) == 0) continue;
            u = xall(i,j); // そもそもその項目に回答していない場合は，度数に数え上げない
            gg = Gim[g][i][m];
            hh = u*gg;
            h += hh;
          }
          rjm[g][j][m] = h;
        }
      }
    }

    //Rcout<<rjm(10,20)<<"\n";

    //Rcout<<"expected log complete likelihood calculation.\n";

    //const int e_ell = 0,//'@param e_ell If 1 use a CC of expected log likelihood.

    //double ell=0;
    //double rr,nm,elll;
    //int ell_check = 0;
    //if(e_ell==1){

    //  if(Bayes == 0){
    //    // 期待対数完全データ尤度関数
    //    for(int g=0; g<ng; g++){
    //      for(int j=0; j<nj; j++){
    //        if(ind(g,j) == 0) continue;
    //        a = t0(j,0);
    //        b = t0(j,1);
    //        c = t0(j,2);
    //        for(int m=0; m<N; m++){
    //          rr = rjm[g][j][m];
    //          nm = Nm[g][j][m];
    //          x = Xm[m];
    //          elll = rr*log(c+(1.0-c)/(1.0+exp(-D*a*(x-b)))) +
    //            (nm - rr) * log(1-(c+(1.0-c)/(1.0+exp(-D*a*(x-b)))));
    //          ell += + elll;
    //          if(traits::is_nan<REALSXP>(ell)) ell_check = j+1;
    //        }
    //      }
    //    }
    //  } else {    // Bayes parameter estimation
    //    double da,db,dc;
    //    for(int g=0; g<ng; g++){
    //      for(int j=0; j<nj; j++){
    //        if(ind(g,j) == 0) continue;
    //        a = t0(j,0);
    //        b = t0(j,1);
    //        c = t0(j,2);
    //        for(int m=0; m<N; m++){
    //          rr = rjm[g][j][m];
    //          nm = Nm[g][j][m];
    //          x = Xm[m];
    //          elll = rr*log(c+(1.0-c)/(1.0+exp(-D*a*(x-b)))) +
    //            (nm - rr) * log(1-(c+(1.0-c)/(1.0+exp(-D*a*(x-b)))));
    //          ell += elll;
    //        }
    //        da = -1/a - (log(a)-mu_a)/a*sigma_a*sigma_a;
    //        db = -(b-mu_b)/sigma_b*sigma_b;
    //        dc = (alpha_c-2)/c - (beta_c-2)/(1-c); // たぶんここのパラメタの計算が非数になる原因だと思われる。
    //        ell += (da + db + dc);
    //        if(traits::is_nan<REALSXP>(ell)) ell_check = j+1;
    //      }
    //    }
    //  }

    //  if(print >= 1) Rcout << "\n expected log complete data likelihood is " <<ell;

    //  if(traits::is_nan<REALSXP>(ell) && Bayes != 1){
    //    // 対数尤度の計算に失敗したら，計算を中止する。
    //    stop("Can't calculate expected log complete data likelihood.\nItem %d is not converged. Check P.BIS!", ell_check);
    //  }
    //}
    ////////////////////////
    // M step starts here //
    ////////////////////////
    if(print>=1) Rcout << count1 << " times M-step calculation.\r";

    // 項目ひとつごとに最大化
    for(int j=0; j<nj; j++){
      //Rcout << j << " item calculation.\r";
      LogicalVector rm_j = j==rm_id;
      if(is_true(any(rm_j))){
        //Rcout<<"CHECK!!!"<< rm_j <<"\n";
        continue;
      }

      if(skip_para(j,0) > maxskip_j || skip_para(j,1) > maxskip_j ||  skip_para(j,2) > maxskip_j){
        //Rcout<<"skip item "<<j+1<<"\r";
        continue;
      }

      // convergence criteria
      int conv2 = 0; // for M step

      NumericVector indj = ind(_,j); // item index

      while(conv2 != 1){ // Fisher scoring starts in item j

        count2 = count2 + 1;
        if(count2 >= maxiter_j)conv2 = 1;

        double a = t0m(j,0);
        double b = t0m(j,1);
        double c = t0m(j,2);
        // each element of Fisher scoring matrix and gradient vector

        double daa=0;
        double dbb=0;
        double dcc=0;
        double dab=0;
        double dac=0;
        double dbc=0;
        double da=0;
        double db=0;
        double dc=0;
        double d=0;
        double rr=0;
        double rrr=0;
        double nm=0;
        double nmm=0;
        double P=0;
        double Q=0;

        if(method == "Fisher_Scoring"){

          for(int m=0; m<N; m++){ // 行列計算
            nm = 0;
            for(int g=0; g<ng; g++){
              nmm = Nm[g][j][m];
              d = indj[g]; // 当該集団が受検していれば1，そうでなければ0
              nm = nm + nmm * d;
            }
            rr = 0;
            for(int g=0; g<ng; g++){
              rrr = rjm[g][j][m];
              d = indj[g]; // 当該集団が受検していれば1，そうでなければ0
              rr = rr + rrr * d;
            }
            double xm = Xm[m];
            P = c + (1-c)/(1+exp(-D*a*(xm-b)));
            Q = 1-P;
            // the element of Fisher Information Matrix
            // matrix[0,0]
            double daam = nm * (P-c) * (P-c) * Q * (xm - b) * (xm - b) * D * D
              / ((1-c) * (1-c) * P);
            daa = daa + daam;
            // matrix[1,1]
            double dbbm = nm * (P-c) * (P-c) * Q * a * a * D * D
              / ((1-c) * (1-c) * P);
            dbb = dbb + dbbm;
            // matrix[2,2]
            double dccm = nm * Q
              / (P * (1-c) * (1-c));
            dcc = dcc + dccm;
            //matrix[1,0], [0,1]
            double dabm = -nm * (P-c) * (P-c) * Q * a * (xm - b) * D * D
              / ((1-c) * (1-c) * P);
            dab = dab + dabm;
            // matrix[2,0],[0,2]
            double dacm = nm * (P-c) * Q * (xm - b) * D
              / ((1-c) * (1-c) * P);
            dac = dac + dacm;
            // matrix[2,1],[1,2]
            double dbcm = -nm * (P-c) * Q * a * D
              / ((1-c) * (1-c) * P);
            dbc = dbc + dbcm;

            // the elements of gradient vector
            double dad = D*(xm - b) * (rr - nm * P) * (P-c)
              / ((1-c) * P);
            da = da + dad;
            double dbd = -D*a*(rr - nm * P) * (P-c)
              / ((1-c) * P);
            db = db + dbd;
            double dcd = (rr - nm * P)
              /( (1-c) * P);
            dc = dc + dcd;
          } // 行列計算の下準備終了 // end of m

          if(Bayes==1){
            // the elements of gradient vector
            da += -1/a - (log(a)-mu_a)/(a*sigma_a*sigma_a);
            db += -(b-mu_b)/(sigma_b*sigma_b);
            dc += (alpha_c-2)/c - (beta_c-2)/(1-c);
          }
        }else if(method == "Newton_Raphson"){
          for(int m=0; m<N; m++){ // 行列計算
            nm = 0;
            for(int g=0; g<ng; g++){
              nmm = Nm[g][j][m];
              d = indj[g]; // 当該集団が受検していれば1，そうでなければ0
              nm = nm + nmm * d;
            }
            rr = 0;
            for(int g=0; g<ng; g++){
              rrr = rjm[g][j][m];
              d = indj[g]; // 当該集団が受検していれば1，そうでなければ0
              rr = rr + rrr * d;
            }
            double xm = Xm[m];
            P = c + (1-c)/(1+exp(-D*a*(xm-b)));
            Q = 1-P;
            // the element of Hessian Matrix
            // matrix[0,0]
            double daam = (P-c)*Q*(xm - b)*(xm - b)*D*D
            / ((1-c)*(1-c)*P*P);
            daam *= -nm * P*P + rr*c;
            daa += daam;
            // matrix[1,1]
            double dbbm = (P-c)*Q*a*a*D*D
              / ((1-c)*(1-c)*P*P);
            dbbm *= -nm*P*P + rr*c;
            dbb += dbbm;
            // matrix[2,2]
            double dccm = (rr/(nm*P) -1) - rr*Q/(P*P);
            dccm /= (1-c)*(1-c);
            dcc = dcc + dccm;
            //matrix[1,0], [0,1]
            double dabm = 1;
            dabm -= a*D*(xm-b)*Q/(1-c)*(rr/(nm*P)-1);
            dabm += D*a/(1-c)*(xm-b)*Q/P*(rr*c/P-rr);
            dabm *= -D /(1-c)*(P-c);
            dab += dabm;
            // matrix[2,0],[0,2]
            double dacm = -D/((1-c)*(1-c))*(xm-b)*(P-c)*Q/(P*P)*rr;
            dac += dacm;
            // matrix[2,1],[1,2]
            double dbcm = D/((1-c)*(1-c))*a*(P-c)*Q/(P*P)*rr;
            dbc += dbcm;

            // the elements of gradient vector
            double dad = D*(xm - b) * (rr - nm * P) * (P-c)
              / ((1-c) * P);
            da = da + dad;
            double dbd = -D*a*(rr - nm * P) * (P-c)
              / ((1-c) * P);
            db = db + dbd;
            double dcd = (rr - nm * P)
              /( (1-c) * P);
            dc = dc + dcd;
          } // 行列計算の下準備終了 // end of m

          if(Bayes==1){
            // the elements of gradient vector
            da += -1/a - (log(a)-mu_a)/(a*sigma_a*sigma_a);
            db += -(b-mu_b)/(sigma_b*sigma_b);
            dc += (alpha_c-2)/c - (beta_c-2)/(1-c);
            // Information matrix
            daa += 1/(a*a) - (1-log(a)+mu_a)/(a*a*sigma_a*sigma_a);
            dbb += -1/(sigma_b*sigma_b);
            dcc += (alpha_c-2)/(c*c) - (beta_c-2)/(1-c)*(1-c);
            //非対角要素は通常の二階偏微分と同じ。
          }
        }

        double a1 = 1.702/D;
        double b1 = 0;
        double c1 = 0;

        // 行列，ベクトルに代入
        if(model[j] == "3PL"){
          // determinant of Fisher's information matrix
          double det = 1/(daa*dbb*dcc + dab*dbc*dac + dac*dab*dbc - dac*dbb*dac - dab*dab*dcc - daa*dbc*dbc);

          // each element of inverse of Fisher's information matrix
          double inv_daa = (dbb*dcc-dbc*dbc) * det;
          double inv_dbb = (daa*dcc-dac*dac) * det;
          double inv_dcc = (daa*dbb-dab*dab) * det;
          double inv_dab = (dbc*dac-dab*dcc) * det;
          double inv_dac = (dab*dbc-dbb*dac) * det;
          double inv_dbc = (dab*dac-daa*dbc) * det;

          if(method == "Newton_Raphson"){
            a1 = a - inv_daa*da - inv_dab*db - inv_dac*dc;
            b1 = b - inv_dab*da - inv_dbb*db - inv_dbc*dc;
            c1 = c - inv_dac*da - inv_dbc*db - inv_dcc*dc;
          }
          if(method == "Fisher_Scoring") {
            a1 = a + inv_daa*da + inv_dab*db + inv_dac*dc;
            b1 = b + inv_dab*da + inv_dbb*db + inv_dbc*dc;
            c1 = c + inv_dac*da + inv_dbc*db + inv_dcc*dc;
          }

        } else if (model[j] == "2PL"){
          // determinant of Fisher's information matrix
          double det_Itj = 1/(daa * dbb - dab * dab);

          // each element of inverse of Fisher's information matrix
          double inv_daa = dbb * det_Itj;
          double inv_dbb = daa * det_Itj;
          double inv_dab = -dab * det_Itj;

          if(method == "Newton_Raphson"){
            a1 = a - inv_daa * da - inv_dab * db;
            b1 = b - inv_dab * da - inv_dbb * db;
          }
          if(method == "Fisher_Scoring") {
            a1 = a + inv_daa * da + inv_dab * db;
            b1 = b + inv_dab * da + inv_dbb * db;
          }

        } else {
          if(method == "Newton_Raphson"){
            b1 = b - db/dbb;
          }
          if(method == "Fisher_Scoring") {
            b1 = b + db/dbb;
          }

        }

        if(print >= 3) Rprintf("item %d -- abs a is %.7f, abs b is %.7f, abs c is %.7f in count %d\r", j+1, fabs(a-a1), fabs(b-b1), fabs(c-c1), count2);

        if( fabs(a1 - a) < eM && fabs(b1 - b) < eM && fabs(c1 - c) < eM ) conv2 = 1;

        // 次回の更新用に代入

        // discriminating parameter
        // aパラメタが0以下だったら，計算を中止
        if(a1 <= min_a || a1 > 10 || traits::is_nan<REALSXP>(fabs(a1 - a))){
          skip_para(j,0) += 1; // count up skip vector
          //t0m(j,0) = min_a;
          warning("'a' must be positive value. Error of item %i in %i time iteration.", j+1, count1);
          conv2 = 1;; // 次の項目パラメタの更新へスキップ
        } else {
          t0m(j,0) = a1;
        }

        // difficulty parameter

        if(fabs(b1) >= maxabs_b || traits::is_nan<REALSXP>(fabs(b1 - b))){
          skip_para(j,1) += 1; // count up skip vector
          if(b1 < 0){
            warning("'b' must be larger than %d. Error of item %i in %i time iteration.", maxabs_b, j+1, count1);
            //t0m(j,1) = -maxabs_b;
          } else if(b1 >= 0){
            warning("'b' must be smaller than %d. Error of item %i in %i time iteration.", maxabs_b, j+1, count1);
            //t0m(j,1) = maxabs_b;
          }
          conv2 = 1;; // 次の項目パラメタの更新へスキップ
        } else {
          t0m(j,1) = b1;
        }

        // guessing parameter
        if(c1 < 0 || c1 > 1 || traits::is_nan<REALSXP>(fabs(c1 - c))){
          //skip_para(j,2) += 1; // count up skip vector
          warning("'c' must be real value between 0 to 1. Error of item %i in %i time iteration.", j+1, count1);
          Rprintf("\nThe model of item %i is changed from '3PL' to '2PL'." , j+1);
          t0m(j,2) = 0;
          model[j] = "2PL"; // 2PLに変更する。
          conv2 = 1; // 次の項目パラメタの更新へスキップ
        } else {
          t0m(j,2) = c1;
        }

      }// end of Fisher scoring for item j

    }// end of Fisher scoring in a M step.

    // EMステップの収束判定

    int vv = 0;
    for(int j=0; j<nj; j++){
      if(model[j]=="1PL"){
        for(int p=1; p<2; p++){
          double p1 = t0m(j,p);
          double p2 = t0(j,p);
          double p3 = skip_para(j,p);
          if(fabs(p1 - p2) < eEM || p3>maxskip_j){// 項目パラメタの差の絶対値，と吹っ飛んだパラメタの抑制
            conv[vv] = 1;
          } else {
            conv[vv] = 0;
          }
          vv = vv + 1;
        }
      }else if(model[j]=="2PL"){
        for(int p=0; p<2; p++){
          double p1 = t0m(j,p);
          double p2 = t0(j,p);
          double p3 = skip_para(j,p);
          if(fabs(p1 - p2) < eEM || p3>maxskip_j){// 項目パラメタの差の絶対値，と吹っ飛んだパラメタの抑制
            conv[vv] = 1;
          } else {
            conv[vv] = 0;
          }
          vv = vv + 1;
        }
      }else if(model[j]=="3PL"){
        for(int p=0; p<3; p++){
          double p1 = t0m(j,p);
          double p2 = t0(j,p);
          double p3 = skip_para(j,p);
          if(fabs(p1 - p2) < eEM || p3>maxskip_j){// 項目パラメタの差の絶対値，と吹っ飛んだパラメタの抑制
            conv[vv] = 1;
          } else {
            conv[vv] = 0;
          }
          vv = vv + 1;
        }
      }
    }

    // update group distribution
    // 再計算

    // 母集団ごとの平均と標準偏差を計算
    LogicalVector conv4 (ng); // 平均と標準偏差の収束確認用

    if(thdist == "normal"){
      for(int g=0; g<ng; g++){
        // probability
        double mean_g = 0;
        double sd_g = 0;
        //int counti = 0; // 母集団の受検した項目数カウント用
        for(int j=0; j<nj; j++){
          // 集団内のだれも回答をしていなかったら，次の項目のループへスキップ。
          if(ind(g,j) == 0) continue;

          //counti = counti + 1;
          double mean_j = 0;
          double sNjm = 0; // 項目ごとの分点の受験者の期待度数の総和代入用
          for(int m=0; m<N; m++){  // sum
            double Njm = Nm[g][j][m];
            sNjm = sNjm + Njm;
          }

          for(int m=0; m<N; m++){ // sum
            double Njm = Nm[g][j][m];
            double xm = Xm[m];
            double tempM = xm * Njm/sNjm; // E(X) = \Sigma(x_i * prob_i)
            mean_j = mean_j + tempM; //
          } // end of m
          mean_g = mean_g + mean_j; // 項目ごとの母集団平均を足し上げたもの

          // sd

          double var_j = 0;
          double sd_j = 0;
          for(int m=0; m<N; m++){
            double Njm = Nm[g][j][m];
            double xm = Xm[m];
            double tempV = (xm -mean_j)*(xm -mean_j)*Njm/sNjm; // V(X) = Sigma(x_i-E(X))^2*prob_i
            var_j = var_j + tempV;
          }// end of m
          sd_j = std::sqrt(var_j); // 項目ごとに計算した標準偏差
          sd_g = sd_g + sd_j; // 項目ごとの母集団標準偏差を足し上げたもの

        } // end of j

        int counti = sum(ind(g,_));
        mean[g] = mean_g/counti; // 全項目における母集団平均の，平均
        sd[g] = sd_g/counti; // 全項目における母集団標準偏差の，平均

        //Rcout << "mean: "<< mean[g]<<", sd: "<<sd[g]<<"\n";
      }
    } else if(thdist == "empirical"){ // 受検者の分点ごとの期待度数を用いて，次回のEステップの事前分布を計算する。
      // 母集団ごとの平均と標準偏差を計算
      for(int g=0; g<ng; g++){
        // probability
        double mean_g = 0;
        double sd_g = 0;
        //int counti = 0; // 母集団の受検した項目数カウント用
        for(int j=0; j<nj; j++){
          // 集団内のだれも回答をしていなかったら，次の項目のループへスキップ。
          if(ind(g,j) == 0) continue;

          //counti = counti + 1;
          double mean_j = 0;
          double sNjm = 0; // 項目ごとの分点の受験者の期待度数の総和代入用
          for(int m=0; m<N; m++){  // sum
            double Njm = Nm[g][j][m];
            sNjm = sNjm + Njm;
          }

          for(int m=0; m<N; m++){ // sum
            double Njm = Nm[g][j][m];
            double xm = Xm[m];
            double tempM = xm * Njm/sNjm; // E(X) = \Sigma(x_i * prob_i)
            Um(m,g) += Njm/sNjm;
            mean_j += tempM; //
          } // end of m
          mean_g = mean_g + mean_j; // 項目ごとの母集団平均を足し上げたもの

          // sd

          double var_j = 0;
          double sd_j = 0;
          for(int m=0; m<N; m++){
            double Njm = Nm[g][j][m];
            double xm = Xm[m];
            double tempV = (xm -mean_j)*(xm -mean_j)*Njm/sNjm; // V(X) = Sigma(x_i-E(X))^2*prob_i
            var_j = var_j + tempV;
          }// end of m
          sd_j = std::sqrt(var_j); // 項目ごとに計算した標準偏差
          sd_g = sd_g + sd_j; // 項目ごとの母集団標準偏差を足し上げたもの

        } // end of j

        int counti = sum(ind(g,_));
        // 収束判定用
        if(fabs(mean[g] - mean_g/counti)<emu && fabs(sd[g] - sd_g/counti)<esd ) conv4[g] = 1;

        mean[g] = mean_g/counti; // 全項目における母集団平均の，平均
        sd[g] = sd_g/counti; // 全項目における母集団標準偏差の，平均
        if(print >= 1) Rcout << "mean: "<< mean[g]<<", sd: "<<sd[g]<<"\n";

        for(int m=0; m<N; m++){
          Um(m,g) /= counti;
        }

      } // end of g
    }

    double A = 1;
    double K = 0;

    if(fix == 1){// && thdist != "empirical"){
      A = sigma/sd[bg];
      K = mu - A * mean[bg];
    }

    // 平均0，標準偏差1になるように線形変換
    // 基準集団は１

    // 平均と標準偏差をキャリブレーション
    if(print == 1) Rprintf("\n----------------------------------");
    for(int g=0; g<ng; g++){
      mean[g] = A * mean[g] + K;
      sd[g] = A * sd[g];
      if (print >= 2) Rprintf("\n mean: %.5f, sd: %.5f ", mean[g],sd[g] );
      //Rcout <<"\n mean: "<< mean[g]<<", sd: "<<sd[g];
    }
    if(print == 1) Rprintf("\n----------------------------------");

    // 次回のEステップ用の項目パラメタもキャリブレーション

    for(int j=0; j<nj; j++){
      a = t0m(j,0); // a parameter
      b = t0m(j,1); // b parameter
      c=  t0m(j,2); // c parameter
      LogicalVector rm_j = 0;
      rm_j = j==rm_id;
      if(skip_para(j,0)>maxskip_j || skip_para(j,1)>maxskip_j || skip_para(j,2)>maxskip_j || is_true(any(rm_j))){
        // Mステップの更新をスキップした項目は，そのまま
        t0(j,0) = a;
        t0(j,1) = b;
        t0(j,2) = c;
        t0m(j,0) = a;
        t0m(j,1) = b;
        t0m(j,2) = c;
      } else {
        // きちんと更新をおこなった項目は，キャリブレーション
        if(model[j] != "1PL"){ // 1PLの場合は識別力の尺度調整を行わない
          t0(j,0) = a/A;
          t0m(j,0) = a/A;
        } else {
          t0(j,0) = a;
          t0m(j,0) = a;
        }
        t0(j,1) = A * b + K;
        t0(j,2) = c;
        t0m(j,1) = A * b + K;
        t0m(j,2) = c;
      }
    }

    if(print >= 2){
      Rprintf("\n Item Parameters in %d times iteration", count1);
      for(int j=0; j<nj; j++){
        //Rcout << "\n a: "<<t0m(j,0)<<", b: "<<t0m(j,1)<< ", c: "<<t0m(j,2);
        Rprintf("\n a: %.5f, b: %.5f, c: %.5f",t0m(j,0), t0m(j,1), t0m(j,2));
      }
      Rcout <<"\n MLL difference is "<<diff;
      Rprintf("\n------------------------------------------------------\n");

    }

    ////////////////////////////////////////////
    // 収束判定
    ////////////////////////////////////////////

    //  項目パラメタの差の絶対値，か周辺対数尤度の差の絶対値が一定数よりも下回ったら収束
    // もしくは最大繰り返し回数に達したら，終了。
    if((is_true(all(conv)))|| count1 == maxiter_em || diff < eMLL || is_true(all(conv4))){
      //Rcout << "eval EM conv\n";
      conv1 = 1; // 収束していたら繰り返しを打ち切り

      if(print>=1)Rcout <<"\nEstimating Standard Error";

      ///////////////////////
      // SE of estimation. //
      ///////////////////////

      for(int j=0; j<nj; j++){

        NumericVector indj = ind(_,j); // item index

        double a = t0m(j,0);
        double b = t0m(j,1);
        double c = t0m(j,2);
        // each element of Fisher's information matrix and gradient vector

        double daa,dbb,dcc,dab,dac,dbc;
        double da, db, dc;
        double d, rr, rrr, nm, nmm;
        double P,Q;
        // initialize
        daa=0;
        dbb=0;
        dcc=0;
        dab=0;
        dac=0;
        dbc=0;
        da=0;
        db=0;
        dc=0;
        P=0;
        Q=0;

        for(int m=0; m<N; m++){ // 行列計算

          nm = 0;
          for(int g=0; g<ng; g++){
            nmm = Nm[g][j][m];
            d = indj[g]; // 当該集団が受検していれば1，そうでなければ0
            nm = nm + nmm * d;
          }

          rr = 0;
          for(int g=0; g<ng; g++){
            rrr = rjm[g][j][m];
            d = indj[g]; // 当該集団が受検していれば1，そうでなければ0
            rr = rr + rrr * d;
          }

          double xm = Xm[m];

          P = c + (1-c)/(1+exp(-D*a*(xm-b)));
          Q = 1-P;

          // matrix[0,0]
          double daam = nm * (P-c) * (P-c) * Q * (xm - b) * (xm - b) * D * D
            / ((1-c) * (1-c) * P);
          daa = daa + daam;

          // matrix[1,1]
          double dbbm = nm * (P-c) * (P-c) * Q * a * a * D * D
            / ((1-c) * (1-c) * P);
          dbb = dbb + dbbm;

          // matrix[2,2]
          double dccm = nm * Q
            / (P * (1-c) * (1-c));
          dcc = dcc + dccm;

          //matrix[1,0], [0,1]
          double dabm = -nm * (P-c) * (P-c) * Q * a * (xm - b) * D * D
            / ((1-c) * (1-c) * P);
          dab = dab + dabm;

          // matrix[2,0],[0,2]
          double dacm = nm * (P-c) * Q * (xm - b) * D
            / ((1-c) * (1-c) * P);
          dac = dac + dacm;

          // matrix[2,1],[1,2]
          double dbcm = -nm * (P-c) * Q * a * D
            / ((1-c) * (1-c) * P);
          dbc = dbc + dbcm;

          // the elements of gradient vector
          double dad = D*(xm - b) * (rr - nm * P) * (P-c) // cが消える
            / ((1-c) * P);
          da = da + dad;

          double dbd = -D*a*(rr - nm * P) * (P-c)
            / ((1-c) * P);
          db = db + dbd;

          double dcd = (rr - nm * P)
            / ((1-c) * P);
          dc = dc + dcd;

        } // 行列計算の下準備終了 // end of m

        // 行列，ベクトルに代入
        if(model[j] == "3PL"){
          // determinant of Fisher's information matrix
          double det = 1/(daa*dbb*dcc+dab*dbc*dac+dac*dab*dbc-dac*dbb*dac-dab*dab*dcc-daa*dbc*dbc);
          // each element of inverse of Fisher's information matrix

          double inv_daa = (dbb*dcc-dbc*dbc) * det;
          double inv_dbb = (daa*dcc-dac*dac) * det;
          double inv_dcc = (daa*dbb-dab*dab) * det;

          SE(j,0) = std::sqrt(inv_daa); //
          SE(j,1) = std::sqrt(inv_dbb);
          SE(j,2) = std::sqrt(inv_dcc);

        } else if (model[j] == "2PL"){
          // determinant of Fisher's information matrix
          double det_Itj = 1/(daa * dbb - dab * dab);

          // each element of inverse of Fisher's information matrix
          double inv_daa = dbb * det_Itj;
          double inv_dbb = daa * det_Itj;

          SE(j,0) = std::sqrt(inv_daa); //
          SE(j,1) = std::sqrt(inv_dbb);

        } else {
          SE(j,1) = std::sqrt(1/dbb);
        }

      }// end of estimate SE for item j

      Rcout << "\nEM algorithm has been converged.\nTotal iteration time is " << count1 << "\n";

    }

  } // end of EM step

  // 推定母集団分布の計算
  // 初期値に一様分布をもちいた場合，Easy Estimationの計算結果とおおむね一致するはず。
  if(estdist == 1) Rprintf("Start calculating estimated population distribution.\n");

  // result
  NumericVector mean_pop(ng);
  NumericVector sd_pop(ng);
  double diff2 = 0;
  NumericVector MLL2 (0);

  // 一様分布を初期値に設定する。
  NumericMatrix unif_dist (0);
  double NN = Um.nrow();
  for(int i=0; i<N; i++){
    for(int j=0; j<ng; j++){
      Um(i,j) = 1/NN;
    }
  }

  // start loop
  int count3 =0;
  int conv3 = 0;
  LogicalVector conv0(ng);

  if(estdist == 0) conv3 = 1;
  while(conv3 == 0){
    count3 += 1;

    // calculate Lim
    double a,b,c,t,xx,tt,u;
    int g,i,m,j;

    for(g=0; g<ng; g++){
      for(i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        for(m=0; m<N; m++){
          t = 1;
          xx = Xm[m];
          tt = 0;
          for(j=0; j<nj; j++){
            //if(resp(i,j)==0) continue; // NAの反応パタンのところは計算ループから外れる。
            a = t0(j,0);
            b = t0(j,1);
            c = t0(j,2);
            u = xall(i,j);
            if(u == 1){ // 正答した場合の尤度
              tt = c+(1.0-c)/(1.0+exp(-D*a*(xx-b)));
            } else if (u == 0) { // 誤答した場合の尤度
              tt = 1.0-(c+(1.0-c)/(1.0+exp(-D*a*(xx-b))));
            } else { // 欠測値の場合は尤度を計算しないので，1
              tt = 1;
            }
            t = t * tt; // sum
          }
          //boost::multi_array<double, 3>::index idx = {{g,i,m}};
          Lim[g][i][m] = t;
        }
      }
    }

    //Rcout<<"marginal log likelihood calculation.\n";
    // Calculation marginal log likelihood

    double f = 0; // 周辺対数尤度代入用
    double ff,fff,l,w;

    for(int g=0; g<ng; g++){
      for(int i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        ff = 0; // 受検者iの周辺対数尤度代入用
        for(int m=0; m<N; m++){
          l = Lim[g][i][m];
          w = Um(m,g);
          fff =  l*w ;
          ff = ff + fff;
        }
        f = f + log(ff);
      }
    }

    MLL2.push_back(f);
    diff2 = fabs(MLL2[count3-1] - MLL2[count3]); // 周辺対数尤度の差

    //Rcout << "-2 Marginal Log Likelihood is " << -2 * f <<"\r";
    Rprintf("%d times -2 Marginal Log Likelihood is %f \r",count3,-2*f); // 小数点形式の出力に対応

    // calculate Gim
    double uu;
    for(int g=0; g<ng; g++){
      for(int i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        u = 0;
        for(int m=0; m<N; m++){ // 総和を1にするための分母の計算 // sum
          l = Lim[g][i][m];
          w = Um(m,g);
          uu = l*w;
          u = u + uu;
        }
        for(int m=0; m<N; m++){
          l = Lim[g][i][m];
          w = Um(m,g);
          Gim[g][i][m] =  l * w / u;
        }
      }
    }

    // calculate Nm

    //Rcout<<"expected frequency of subjects in each nodes calculation.\n";
    double k,kk;
    for(int g=0; g<ng; g++){
      for(int j=0; j<nj; j++){ // 各分点の期待度数
        if(ind(g,j) == 0) continue;
        for(int m=0; m<N; m++){
          k = 0;
          for(int i=0; i<ni; i++){ // 欠測値がある場合，項目ごとに受検者数が異なる。
            if(resp(i,j)==0) continue;
            //double d = resp(i,j);
            kk = Gim[g][i][m];
            k = k + kk; //* d;
          }
          Nm[g][j][m] = k;
        }
      }
    }

    // 一様分布の確率密度をリセット
    for(int m=0; m<N; m++){
      for(int g=0; g<ng; g++){
        Um(m,g) = 0;
      }
    }

    // 母集団ごとの平均と標準偏差を計算
    for(int g=0; g<ng; g++){
      // probability
      double mean_g = 0;
      double sd_g = 0;
      //int counti = 0; // 母集団の受検した項目数カウント用
      for(int j=0; j<nj; j++){
        // 集団内のだれも回答をしていなかったら，次の項目のループへスキップ。
        if(ind(g,j) == 0) continue;

        //counti = counti + 1;
        double mean_j = 0;
        double sNjm = 0; // 項目ごとの分点の受験者の期待度数の総和代入用
        for(int m=0; m<N; m++){  // sum
          double Njm = Nm[g][j][m];
          sNjm = sNjm + Njm;
        }

        for(int m=0; m<N; m++){ // sum
          double Njm = Nm[g][j][m];
          double xm = Xm[m];
          double tempM = xm * Njm/sNjm; // E(X) = \Sigma(x_i * prob_i)
          Um(m,g) += Njm/sNjm;
          mean_j += tempM; //
        } // end of m
        mean_g = mean_g + mean_j; // 項目ごとの母集団平均を足し上げたもの

        // sd

        double var_j = 0;
        double sd_j = 0;
        for(int m=0; m<N; m++){
          double Njm = Nm[g][j][m];
          double xm = Xm[m];
          double tempV = (xm -mean_j)*(xm -mean_j)*Njm/sNjm; // V(X) = Sigma(x_i-E(X))^2*prob_i
          var_j = var_j + tempV;
        }// end of m
        sd_j = std::sqrt(var_j); // 項目ごとに計算した標準偏差
        sd_g = sd_g + sd_j; // 項目ごとの母集団標準偏差を足し上げたもの

      } // end of j

      int counti = sum(ind(g,_));
      // 収束判定用
      if(fabs(mean_pop[g] - mean_g/counti)<emu && fabs(sd_pop[g] - sd_g/counti)<esd ) conv0[g] = 1;

      mean_pop[g] = mean_g/counti; // 全項目における母集団平均の，平均
      sd_pop[g] = sd_g/counti; // 全項目における母集団標準偏差の，平均
      if(print >= 1) Rcout << "mean: "<< mean_pop[g]<<", sd: "<<sd_pop[g]<<"\n";

      for(int m=0; m<N; m++){
        Um(m,g) /= counti;
      }

    } // end of g

    if(diff2 < eMLL || is_true(all(conv0)) ) conv3 = 1;
    //break;
  }

  unif_dist = cbind(Xm,Um);

  // Item Fit index
  // reference: Mayekawa, S. (1991) Estimation of Item parameter.
  // Xm; node
  //
  boost::multi_array <double, 3> EMfit (boost::extents[ng][nj][N]);

  if( print > 0) Rcout<<"expected frequency of subjects in each nodes calculation.\n";
  double k,kk;
  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の期待度数
      if(ind(g,j) == 0) continue;
      for(int m=0; m<N; m++){
        k = 0;
        for(int i=0; i<ni; i++){ // 欠測値がある場合，項目ごとに受検者数が異なる。
          if(resp(i,j)==0) continue;
          //double d = resp(i,j);
          kk = Gim[g][i][m];
          k += kk; //* d;
        }
        Nm[g][j][m]= k;
      }
    }
  }

  if( print > 0) Rcout<<"expected freqency of correct response number in each nodes calculation.\n";

  double h,gg,hh,u;
  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の正答受検者の期待度数
      if(ind(g,j) == 0) continue;
      for(int m=0; m<N; m++){
        h = 0;
        for(int i=0; i<ni; i++){ // sum
          //double d = resp(i,j);
          if(resp(i,j) == 0) continue;
          u = xall(i,j); // そもそもその項目に回答していない場合は，度数に数え上げない
          gg = Gim[g][i][m];
          hh = u*gg;
          h += hh;
        }
        rjm[g][j][m] = h;
      }
    }
  }

  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){
      double a = t0(j,0);
      double b = t0(j,1);
      double c = t0(j,2);
      //
      for(int m=0; m<N; m++){
        double tpjm = c+(1.0-c)/(1.0+exp(-D*a*(Xm[m]-b))); // theoritical item correct response rate
        double rpjm = rjm[g][j][m]/Nm[g][j][m]; // real item passing rate
        double ejm = rpjm - tpjm; // x-t
        EMfit[g][j][m] = ejm;
      }// end of m
    }// end of j(item)
  }// end of g

  // アウトプットファイルの調整

  NumericMatrix ms = cbind(mean,sd);
  DataFrame Para = DataFrame::create(Named("Item")=Item, Named("a") = t0(_,0), Named("b") = t0(_,1), Named("c") = t0(_,2), Named("model") = model);
  DataFrame SE_d = DataFrame::create(Named("Item")=Item, Named("a") = SE(_,0), Named("b") = SE(_,1), Named("c") = SE(_,2), Named("model") = model);

  List res = List::create(_["para"] = Para, _["SE"] = SE_d, _["initial"] = initial,
                          _["th_dist1"] = dist, _["ms"] = ms,_["mean"] = mean, _["sd"] = sd,
                          _["th_dist2"] = unif_dist, _["population_mean"] = mean_pop, _["population_sd"] = sd_pop,
                          _["MLL"] = MLL, _["conv"] = conv, _["count1"] = count1, _["count2"] = count2,
                          _["skip_para"] = skip_para, _["rm_n"] = rm_n, _["rm_id"] = rm_id ,
                          _["p_bis"] = r, _["passing_rate"] = p, _["itemfit_EM"] = EMfit
  );

  return res;

}


#include <bits/stdc++.h>
#include <boost/tokenizer.hpp>

std::vector<uint64_t> numbers;

uint64_t iterate(uint64_t max_ite) {
  std::map<uint64_t, std::pair<uint64_t, uint64_t>> memory;
  std::map<uint64_t, uint64_t> count;

  uint64_t ite = 0;
  uint64_t N   = numbers.size();
  uint64_t cur_spoken  = 0;
  uint64_t last_spoken = 0;
  bool dbg = true;

  auto memorize = [&] (const uint64_t n, const uint64_t ite) {
    memory[n].second = memory[n].first;
    memory[n].first  = ite;
  };

  // Counting the numbers for the first time
  for (auto n: numbers) {
    if (count.count(n) == 0)
      count[n] = 1;
    else
      count[n]++;
  }

  while (ite < max_ite) {
    if (ite < N)
      cur_spoken = numbers[ite];
    else {
      if (count[cur_spoken] > 1)
        cur_spoken = memory[cur_spoken].first - memory[cur_spoken].second;
      else
        cur_spoken = 0;

      if (count.count(cur_spoken) == 0)
        count[cur_spoken] = 1;
      else
        count[cur_spoken]++;
    }

    memorize(cur_spoken, ite);

    std::cout << cur_spoken << std::endl;
    if (count[cur_spoken] == 3 && dbg) {
      dbg = false;
      std::cerr << "First at 3 is " << cur_spoken << std::endl;
    }

    ite++;
  }

  return cur_spoken;
}

void part1() {
  std::cout << "Part 1 : " << iterate(2020) << std::endl;
}

void part2() {
  std::cout << "Part 2 : " << iterate(30000000) << std::endl;
}

int main(int argc, char **argv) {

  std::ifstream f_in;
  f_in.open("15.in");
  std::string line;
  std::getline(f_in, line);
  f_in.close();

  boost::char_separator<char> sep{",", ""};
  boost::tokenizer tokenizer{line, sep};
  for (const auto &t: tokenizer)
    numbers.push_back(std::stoi(t));

  part1();
  part2();

  return 0;
}


/* boost random/lagged_fibonacci.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: lagged_fibonacci.hpp 72951 2011-07-07 04:57:37Z steven_watanabe $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_LAGGED_FIBONACCI_HPP
#define BOOST_RANDOM_LAGGED_FIBONACCI_HPP

#include <istream>
#include <iosfwd>
#include <algorithm>     // std::max
#include <iterator>
#include <boost/config/no_tr1/cmath.hpp>         // std::pow
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/cstdint.hpp>
#include <boost/integer/integer_mask.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/generator_seed_seq.hpp>

namespace boost {
namespace random {

/**
 * Instantiations of class template \lagged_fibonacci_engine model a
 * \pseudo_random_number_generator. It uses a lagged Fibonacci
 * algorithm with two lags @c p and @c q:
 * x(i) = x(i-p) + x(i-q) (mod 2w) with p > q.
 */
template<class UIntType, int w, unsigned int p, unsigned int q>
class lagged_fibonacci_engine
{
public:
    typedef UIntType result_type;
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    BOOST_STATIC_CONSTANT(int, word_size = w);
    BOOST_STATIC_CONSTANT(unsigned int, long_lag = p);
    BOOST_STATIC_CONSTANT(unsigned int, short_lag = q);

    BOOST_STATIC_CONSTANT(UIntType, default_seed = 331u);

    /** Returns the smallest value that the generator can produce. */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () { return 0; }
    /** Returns the largest value that the generator can produce. */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return low_bits_mask_t<w>::sig_bits; }

    /** Creates a new @c lagged_fibonacci_engine and calls @c seed(). */
    lagged_fibonacci_engine() { seed(); }

    /** Creates a new @c lagged_fibonacci_engine and calls @c seed(value). */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(lagged_fibonacci_engine,
        UIntType, value)
    { seed(value); }

    /** Creates a new @c lagged_fibonacci_engine and calls @c seed(seq). */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(lagged_fibonacci_engine,
        SeedSeq, seq)
    { seed(seq); }

    /**
     * Creates a new @c lagged_fibonacci_engine and calls @c seed(first, last).
     */
    template<class It> lagged_fibonacci_engine(It& first, It last)
    { seed(first, last); }

    // compiler-generated copy ctor and assignment operator are fine

    /** Calls @c seed(default_seed). */
    void seed() { seed(default_seed); }

    /**
     * Sets the state of the generator to values produced by
     * a \minstd_rand0 generator.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(lagged_fibonacci_engine,
        UIntType, value)
    {
        minstd_rand0 intgen(static_cast<boost::uint32_t>(value));
        detail::generator_seed_seq<minstd_rand0> gen(intgen);
        seed(gen);
    }

    /**
     * Sets the state of the generator using values produced by seq.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(lagged_fibonacci_engine, SeedSeq, seq)
    {
        detail::seed_array_int<w>(seq, x);
        i = long_lag;
    }

    /**
     * Sets the state of the generator to values from the iterator
     * range [first, last).  If there are not enough elements in the
     * range [first, last) throws @c std::invalid_argument.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_int<w>(first, last, x);
        i = long_lag;
    }

    /** Returns the next value of the generator. */
    result_type operator()()
    {
        if(i >= long_lag)
            fill();
        return x[i++];
    }

    /** Fills a range with random values */
    template<class Iter>
    void generate(Iter first, Iter last)
    { detail::generate_from_int(*this, first, last); }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        for(boost::uintmax_t j = 0; j < z; ++j) {
            (*this)();
        }
    }

    /**
     * Writes the textual representation of the generator to a @c std::ostream.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, lagged_fibonacci_engine, f)
    {
        os << f.i;
        for(unsigned int i = 0; i < f.long_lag; ++i)
            os << ' ' << f.x[i];
        return os;
    }

    /**
     * Reads the textual representation of the generator from a @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, lagged_fibonacci_engine, f)
    {
        is >> f.i >> std::ws;
        for(unsigned int i = 0; i < f.long_lag; ++i)
            is >> f.x[i] >> std::ws;
        return is;
    }

    /**
     * Returns true if the two generators will produce identical
     * sequences of outputs.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(lagged_fibonacci_engine, x, y)
    { return x.i == y.i && std::equal(x.x, x.x+long_lag, y.x); }

    /**
     * Returns true if the two generators will produce different
     * sequences of outputs.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(lagged_fibonacci_engine)

private:
    /// \cond show_private
    void fill();
    /// \endcond

    unsigned int i;
    UIntType x[long_lag];
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UIntType, int w, unsigned int p, unsigned int q>
const bool lagged_fibonacci_engine<UIntType, w, p, q>::has_fixed_range;
template<class UIntType, int w, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci_engine<UIntType, w, p, q>::long_lag;
template<class UIntType, int w, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci_engine<UIntType, w, p, q>::short_lag;
template<class UIntType, int w, unsigned int p, unsigned int q>
const UIntType lagged_fibonacci_engine<UIntType, w, p, q>::default_seed;
#endif

/// \cond show_private

template<class UIntType, int w, unsigned int p, unsigned int q>
void lagged_fibonacci_engine<UIntType, w, p, q>::fill()
{
    // two loops to avoid costly modulo operations
    {  // extra scope for MSVC brokenness w.r.t. for scope
    for(unsigned int j = 0; j < short_lag; ++j)
        x[j] = (x[j] + x[j+(long_lag-short_lag)]) & low_bits_mask_t<w>::sig_bits;
    }
    for(unsigned int j = short_lag; j < long_lag; ++j)
        x[j] = (x[j] + x[j-short_lag]) & low_bits_mask_t<w>::sig_bits;
    i = 0;
}

/// \endcond

/// \cond show_deprecated

// provided for backwards compatibility
template<class UIntType, int w, unsigned int p, unsigned int q, UIntType v = 0>
class lagged_fibonacci : public lagged_fibonacci_engine<UIntType, w, p, q>
{
    typedef lagged_fibonacci_engine<UIntType, w, p, q> base_type;
public:
    lagged_fibonacci() {}
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(lagged_fibonacci, UIntType, val)
    { this->seed(val); }
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(lagged_fibonacci, SeedSeq, seq)
    { this->seed(seq); }
    template<class It>
    lagged_fibonacci(It& first, It last) : base_type(first, last) {}
};

/// \endcond

// lagged Fibonacci generator for the range [0..1)
// contributed by Matthias Troyer
// for p=55, q=24 originally by G. J. Mitchell and D. P. Moore 1958

/**
 * Instantiations of class template @c lagged_fibonacci_01 model a
 * \pseudo_random_number_generator. It uses a lagged Fibonacci
 * algorithm with two lags @c p and @c q, evaluated in floating-point
 * arithmetic: x(i) = x(i-p) + x(i-q) (mod 1) with p > q. See
 *
 *  @blockquote
 *  "Uniform random number generators for supercomputers", Richard Brent,
 *  Proc. of Fifth Australian Supercomputer Conference, Melbourne,
 *  Dec. 1992, pp. 704-706.
 *  @endblockquote
 *
 * @xmlnote
 * The quality of the generator crucially depends on the choice
 * of the parameters. User code should employ one of the sensibly
 * parameterized generators such as \lagged_fibonacci607 instead.
 * @endxmlnote
 *
 * The generator requires considerable amounts of memory for the storage
 * of its state array. For example, \lagged_fibonacci607 requires about
 * 4856 bytes and \lagged_fibonacci44497 requires about 350 KBytes.
 */
template<class RealType, int w, unsigned int p, unsigned int q>
class lagged_fibonacci_01_engine
{
public:
    typedef RealType result_type;
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    BOOST_STATIC_CONSTANT(int, word_size = w);
    BOOST_STATIC_CONSTANT(unsigned int, long_lag = p);
    BOOST_STATIC_CONSTANT(unsigned int, short_lag = q);

    BOOST_STATIC_CONSTANT(boost::uint32_t, default_seed = 331u);

    /** Constructs a @c lagged_fibonacci_01 generator and calls @c seed(). */
    lagged_fibonacci_01_engine() { seed(); }
    /** Constructs a @c lagged_fibonacci_01 generator and calls @c seed(value). */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(lagged_fibonacci_01_engine, uint32_t, value)
    { seed(value); }
    /** Constructs a @c lagged_fibonacci_01 generator and calls @c seed(gen). */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(lagged_fibonacci_01_engine, SeedSeq, seq)
    { seed(seq); }
    template<class It> lagged_fibonacci_01_engine(It& first, It last)
    { seed(first, last); }

    // compiler-generated copy ctor and assignment operator are fine

    /** Calls seed(default_seed). */
    void seed() { seed(default_seed); }

    /**
     * Constructs a \minstd_rand0 generator with the constructor parameter
     * value and calls seed with it. Distinct seeds in the range
     * [1, 2147483647) will produce generators with different states. Other
     * seeds will be equivalent to some seed within this range. See
     * \linear_congruential_engine for details.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(lagged_fibonacci_01_engine, boost::uint32_t, value)
    {
        minstd_rand0 intgen(value);
        detail::generator_seed_seq<minstd_rand0> gen(intgen);
        seed(gen);
    }

    /**
     * Seeds this @c lagged_fibonacci_01_engine using values produced by
     * @c seq.generate.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(lagged_fibonacci_01_engine, SeedSeq, seq)
    {
        detail::seed_array_real<w>(seq, x);
        i = long_lag;
    }

    /**
     * Seeds this @c lagged_fibonacci_01_engine using values from the
     * iterator range [first, last).  If there are not enough elements
     * in the range, throws @c std::invalid_argument.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_real<w>(first, last, x);
        i = long_lag;
    }

    /** Returns the smallest value that the generator can produce. */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () { return result_type(0); }
    /** Returns the upper bound of the generators outputs. */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () { return result_type(1); }

    /** Returns the next value of the generator. */
    result_type operator()()
    {
        if(i >= long_lag)
            fill();
        return x[i++];
    }

    /** Fills a range with random values */
    template<class Iter>
    void generate(Iter first, Iter last)
    { return detail::generate_from_real(*this, first, last); }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        for(boost::uintmax_t j = 0; j < z; ++j) {
            (*this)();
        }
    }

    /**
     * Writes the textual representation of the generator to a @c std::ostream.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, lagged_fibonacci_01_engine, f)
    {
        // allow for Koenig lookup
        using std::pow;
        os << f.i;
        std::ios_base::fmtflags oldflags = os.flags(os.dec | os.fixed | os.left);
        for(unsigned int i = 0; i < f.long_lag; ++i)
            os << ' ' << f.x[i] * f.modulus();
        os.flags(oldflags);
        return os;
    }

    /**
     * Reads the textual representation of the generator from a @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, lagged_fibonacci_01_engine, f)
    {
        is >> f.i;
        for(unsigned int i = 0; i < f.long_lag; ++i) {
            typename lagged_fibonacci_01_engine::result_type value;
            is >> std::ws >> value;
            f.x[i] = value / f.modulus();
        }
        return is;
    }

    /**
     * Returns true if the two generators will produce identical
     * sequences of outputs.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(lagged_fibonacci_01_engine, x, y)
    { return x.i == y.i && std::equal(x.x, x.x+long_lag, y.x); }

    /**
     * Returns true if the two generators will produce different
     * sequences of outputs.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(lagged_fibonacci_01_engine)

private:
    /// \cond show_private
    void fill();
    static RealType modulus()
    {
        using std::pow;
        return pow(RealType(2), word_size);
    }
    /// \endcond
    unsigned int i;
    RealType x[long_lag];
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class RealType, int w, unsigned int p, unsigned int q>
const bool lagged_fibonacci_01_engine<RealType, w, p, q>::has_fixed_range;
template<class RealType, int w, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci_01_engine<RealType, w, p, q>::long_lag;
template<class RealType, int w, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci_01_engine<RealType, w, p, q>::short_lag;
template<class RealType, int w, unsigned int p, unsigned int q>
const int lagged_fibonacci_01_engine<RealType,w,p,q>::word_size;
template<class RealType, int w, unsigned int p, unsigned int q>
const boost::uint32_t lagged_fibonacci_01_engine<RealType,w,p,q>::default_seed;
#endif

/// \cond show_private
template<class RealType, int w, unsigned int p, unsigned int q>
void lagged_fibonacci_01_engine<RealType, w, p, q>::fill()
{
    // two loops to avoid costly modulo operations
    {  // extra scope for MSVC brokenness w.r.t. for scope
    for(unsigned int j = 0; j < short_lag; ++j) {
        RealType t = x[j] + x[j+(long_lag-short_lag)];
        if(t >= RealType(1))
            t -= RealType(1);
        x[j] = t;
    }
    }
    for(unsigned int j = short_lag; j < long_lag; ++j) {
        RealType t = x[j] + x[j-short_lag];
        if(t >= RealType(1))
            t -= RealType(1);
        x[j] = t;
    }
    i = 0;
}
/// \endcond

/// \cond show_deprecated

// provided for backwards compatibility
template<class RealType, int w, unsigned int p, unsigned int q>
class lagged_fibonacci_01 : public lagged_fibonacci_01_engine<RealType, w, p, q>
{
    typedef lagged_fibonacci_01_engine<RealType, w, p, q> base_type;
public:
    lagged_fibonacci_01() {}
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(lagged_fibonacci_01, boost::uint32_t, val)
    { this->seed(val); }
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(lagged_fibonacci_01, SeedSeq, seq)
    { this->seed(seq); }
    template<class It>
    lagged_fibonacci_01(It& first, It last) : base_type(first, last) {}
};

/// \endcond

namespace detail {

template<class Engine>
struct generator_bits;

template<class RealType, int w, unsigned int p, unsigned int q>
struct generator_bits<lagged_fibonacci_01_engine<RealType, w, p, q> >
{
    static std::size_t value() { return w; }
};

template<class RealType, int w, unsigned int p, unsigned int q>
struct generator_bits<lagged_fibonacci_01<RealType, w, p, q> >
{
    static std::size_t value() { return w; }
};

}

#ifdef BOOST_RANDOM_DOXYGEN
namespace detail {
/**
 * The specializations lagged_fibonacci607 ... lagged_fibonacci44497
 * use well tested lags.
 *
 * See
 *
 *  @blockquote
 *  "On the Periods of Generalized Fibonacci Recurrences", Richard P. Brent
 *  Computer Sciences Laboratory Australian National University, December 1992
 *  @endblockquote
 *
 * The lags used here can be found in
 *
 *  @blockquote
 *  "Uniform random number generators for supercomputers", Richard Brent,
 *  Proc. of Fifth Australian Supercomputer Conference, Melbourne,
 *  Dec. 1992, pp. 704-706.
 *  @endblockquote
 */
struct lagged_fibonacci_doc {};
}
#endif

/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 607, 273> lagged_fibonacci607;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 1279, 418> lagged_fibonacci1279;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 2281, 1252> lagged_fibonacci2281;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 3217, 576> lagged_fibonacci3217;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 4423, 2098> lagged_fibonacci4423;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 9689, 5502> lagged_fibonacci9689;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 19937, 9842> lagged_fibonacci19937;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 23209, 13470> lagged_fibonacci23209;
/** @copydoc boost::random::detail::lagged_fibonacci_doc */
typedef lagged_fibonacci_01_engine<double, 48, 44497, 21034> lagged_fibonacci44497;

} // namespace random

using random::lagged_fibonacci607;
using random::lagged_fibonacci1279;
using random::lagged_fibonacci2281;
using random::lagged_fibonacci3217;
using random::lagged_fibonacci4423;
using random::lagged_fibonacci9689;
using random::lagged_fibonacci19937;
using random::lagged_fibonacci23209;
using random::lagged_fibonacci44497;

} // namespace boost

#endif // BOOST_RANDOM_LAGGED_FIBONACCI_HPP


#include <SFML/Graphics.hpp>
#include <Eigen/Dense>
#include <SFML/Graphics/Sprite.hpp>
#include <SFML/System/Vector2.hpp>
#include "TileMap.hpp"
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

bool TileMap::load(const std::string& tileset, sf::Vector2u tileSize, sf::Vector2u displaySize, MatrixXf tiles)
/* This fonction load a tileset file wich is a contanetation of multiple sprite. It split all the sprite based on their sides and affect them a number. Then it check the matrix of number tiles and affect them the good vertexes. This function allow to display the game !*/

    {
      int width = tiles.cols();
      int height = tiles.rows();

      // on charge la texture du tileset
      if (!m_tileset.loadFromFile(tileset))
	return false;

        // on redimensionne le tableau de vertex pour qu'il puisse contenir tout le niveau
      m_vertices.setPrimitiveType(sf::Quads);
      m_vertices.resize(width * height * 4);

      // on remplit le tableau de vertex, avec un quad par tuile
      for (unsigned int i = 0; i < width; ++i)
	for (unsigned int j = 0; j < height; ++j)
	  {
	    // on récupère le numéro de tuile courant
	    int tileNumber = tiles(j, i);

	    // on en déduit sa position dans la texture du tileset
	    int tu = tileNumber % (m_tileset.getSize().x / tileSize.x);
	    int tv = tileNumber / (m_tileset.getSize().x / tileSize.x);

	    // on récupère un pointeur vers le quad à définir dans le tableau de vertex
	    sf::Vertex* quad = &m_vertices[(i + j * width) * 4];

	    // on définit ses quatre coins
	    quad[0].position = sf::Vector2f(i * displaySize.x, j * displaySize.y);
	    quad[1].position = sf::Vector2f((i + 1) * displaySize.x, j * displaySize.y);
	    quad[2].position = sf::Vector2f((i + 1) * displaySize.x, (j + 1) * displaySize.y);
	    quad[3].position = sf::Vector2f(i * displaySize.x, (j + 1) * displaySize.y);
	    // on définit ses quatre coordonnées de texture
	    quad[0].texCoords = sf::Vector2f(tu * tileSize.x, tv * tileSize.y);
	    quad[1].texCoords = sf::Vector2f((tu + 1) * tileSize.x, tv * tileSize.y);
	    quad[2].texCoords = sf::Vector2f((tu + 1) * tileSize.x, (tv + 1) * tileSize.y);
	    quad[3].texCoords = sf::Vector2f(tu * tileSize.x, (tv + 1) * tileSize.y);
	  }

      return true;
    };

void TileMap::draw(sf::RenderTarget& target, sf::RenderStates states) const
    {
        // on applique la transformation
        states.transform *= getTransform();

        // on applique la texture du tileset
        states.texture = &m_tileset;

        // et on dessine enfin le tableau de vertex
        target.draw(m_vertices, states);
    }


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/core/make.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/range.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::length(hana::make_range(hana::int_c<0>, hana::int_c<0>)),
        hana::size_c<0>
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::length(hana::make_range(hana::int_c<0>, hana::int_c<1>)),
        hana::size_c<1>
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::length(hana::make_range(hana::int_c<0>, hana::int_c<2>)),
        hana::size_c<2>
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::length(hana::make_range(hana::int_c<4>, hana::int_c<4>)),
        hana::size_c<0>
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::length(hana::make_range(hana::int_c<4>, hana::int_c<10>)),
        hana::size_c<6>
    ));
}


//Includes all the headers necessary to use the most common public pieces of the ROS system.
#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Int32.h>
#include <ros/console.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <math.h>
#include <dirent.h>
#include <iomanip>
#include <ctime>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <unistd.h>
#include "icarus_rover_rc/Mapping_Node.h"
#include "icarus_rover_rc/Definitions.h"
#include <boost/algorithm/string.hpp>
//#include "icarus_rover_rc/ICARUS_Diagnostic.h"
#include <unistd.h>
#include <linux/input.h>
#include <fcntl.h>
#include <assert.h>
#define PI 3.14159265359
using namespace std;
//Function Prototypes
void initialize_occupancygrid();
void reset_occupancygrid();
void read_SonarModel_LUT();
int path_search();
//Communication Variables

//Operation Variables
string Mode = "";
int Beams_Per_Sonar = 0;
double Sonar_Beamwidth = 0.0;
double Sonar_MaxDistance = 0.0;
struct grid_cell cell_origin;
int New_Pose = 0;
int New_Scan = 0;
int Grid_Finished = 1;
int Path_Finished = 0;

struct GridCell
{
	int Value;
	int ID;
	int Updated;
	double Probability;
	double Center_Northing_m;
	double Center_Easting_m;
	int Changed;

};

struct Node
{
	int Parent_ID;
	int ID;
	double x,y;
	int i,j;
	double f,g,h;
};

std::vector<Node> OPEN_LIST;
std::vector<Node> CLOSED_LIST;
std::vector<Node> PATH;
struct SonarModel_Point
{
	int index;
	double value;
};
sensor_msgs::LaserScan SonarScan;
geometry_msgs::Pose2D RoverPose;
geometry_msgs::Pose2D GoalPose;
//Program Variables
double dtime = 0.0;
double Pose_X;
double Pose_Y;
double Pose_Theta;
int Pose_Valid = 0;
bool map_initialized = false;
const int grid_width = 100; //Cell Count, should be odd
const int grid_height = grid_width; //Cell Count, should be odd
double bottom_left_X = -15.0;
double bottom_left_Y = bottom_left_X;
double top_right_X = -1.0*bottom_left_X;
double top_right_Y = top_right_X;
double grid_resolution = (top_right_Y - bottom_left_Y)/grid_width;
int SonarModel_AngleSize = 0;
int SonarModel_DistanceSize = 0;
std::vector<SonarModel_Point> SonarModel_Angle;
std::vector<SonarModel_Point> SonarModel_Distance;

GridCell OccupancyGrid[grid_width][grid_height];
void ICARUS_SimulationState_Callback(const std_msgs::Int32::ConstPtr& msg)
{
	printf("Mapping_Node ICARUS_SimulationState_Callback Started.\r\n");

	try
	{
		int state = msg->data;
		if(state == STATUS_GENELIST_READY)
		{
			//printf("Resetting Map.\r\n");
			reset_occupancygrid();
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	printf("Mapping_Node ICARUS_SimulationState_Callback Completed.\r\n");
}
void read_SonarModel_LUT()
{
	printf("Mapping_Node read_SonarModel_LUT Started.\r\n");
	try
	{
		ifstream angle_file("/home/linaro/catkin_ws/src/icarus_rover_rc/include/icarus_rover_rc/SonarModel_Angle.csv");
		string line;

		if(angle_file.is_open())
		{
			int i = 0;
			while(getline(angle_file,line))
			{

				std::vector<std::string> strs;
				boost::split(strs,line,boost::is_any_of(","));
				SonarModel_Point newPoint;
				newPoint.index = atoi(strs.at(0).c_str());
				newPoint.value = atof(strs.at(1).c_str());
				SonarModel_Angle.push_back(newPoint);
				i++;
			}
			SonarModel_AngleSize = i;
			angle_file.close();
			/*for(i = 0; i < SonarModel_AngleSize;i++)
			{
				printf("Ai:%d V:%f\r\n",SonarModel_Angle.at(i).index,SonarModel_Angle.at(i).value);
			}*/
		}
		ifstream distance_file("/home/linaro/catkin_ws/src/icarus_rover_rc/include/icarus_rover_rc/SonarModel_Distance.csv");
		if(distance_file.is_open())
		{
			int i = 0;
			while(getline(distance_file,line))
			{

				std::vector<std::string> strs;
				boost::split(strs,line,boost::is_any_of(","));
				SonarModel_Point newPoint;
				newPoint.index = atoi(strs.at(0).c_str());
				newPoint.value = atof(strs.at(1).c_str());
				SonarModel_Distance.push_back(newPoint);
				i++;
			}
			SonarModel_DistanceSize = i;
			distance_file.close();
			/*for(i = 0; i < SonarModel_DistanceSize;i++)
			{
				printf("Di:%d V:%f\r\n",SonarModel_Distance.at(i).index,SonarModel_Distance.at(i).value);
			}*/
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	printf("Mapping_Node read_SonarModel_LUT Completed.\r\n");

}
void initialize_occupancygrid()
{
	printf("Mapping_Node initialize_occupancygrid Started.\r\n");
	try
	{
		double start_x = bottom_left_X;
		double start_y = top_right_Y;
		double curx = start_x + grid_resolution/2.0;
		double cury = start_y - grid_resolution/2.0;
		for(int i = 0; i < grid_width;i++)
		{
			for(int j = 0; j < grid_height;j++)
			{
				OccupancyGrid[i][j].Updated = 0;
				OccupancyGrid[i][j].Probability = 0.5;
				OccupancyGrid[i][j].Value = 50;
				OccupancyGrid[i][j].ID = i*grid_width + j;
				OccupancyGrid[i][j].Center_Northing_m = curx;
				OccupancyGrid[i][j].Center_Easting_m = cury;
				cury = cury - grid_resolution;
				//Debugging
				//if(OccupancyGrid[i][j].ID % 2 == 0) { OccupancyGrid[i][j].Value = 255; }
				//else { OccupancyGrid[i][j].Value = 0; }
			}
			curx = curx + grid_resolution;
			cury = start_y - grid_resolution/2.0;
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	printf("Mapping_Node initialize_occupancygrid Completed.\r\n");
}
void reset_occupancygrid()
{
	printf("Mapping_Node reset_occupancygrid Started.\r\n");
	try
	{
		for(int i = 0; i < grid_width;i++)
		{
			for(int j = 0; j < grid_height;j++)
			{
				OccupancyGrid[i][j].Updated = 0;
				OccupancyGrid[i][j].Probability = 0.5;
				OccupancyGrid[i][j].Value = 50;
			}
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	//printf("Mapping_Node reset_occupancygrid Completed.\r\n");

}
void update_occupancygrid()
{
	//printf("Mapping_Node update_occupancygrid Started.\r\n");
	try
	{
		if(Pose_Valid == 1)
		{
			Grid_Finished = 0;
			for(int i = 0; i < grid_width;i++)
			{
				for(int j = 0; j < grid_height;j++)
				{
					OccupancyGrid[i][j].Updated = 0;
					//OccupancyGrid[i][j].Value = 0;
				}
			}
			int beam_count = SonarScan.ranges.size();
			double angle = RoverPose.theta + SonarScan.angle_min;
			//printf("START\r\n");
			for(int s = 0; s < beam_count;s++)
			{
				double distance = SonarScan.ranges[s];
				int beam_intercepted = 0;
				if(SonarScan.ranges[s] < 0.0)
				{
					beam_intercepted = 0;
					distance = Sonar_MaxDistance;
				}
				else if(SonarScan.ranges[s] > Sonar_MaxDistance)
				{
					beam_intercepted = 0;
				}
				else
				{
					beam_intercepted = 1;
				}
				while(distance > 0.0)
				{
				double p_x = distance*sin(angle) + RoverPose.x;
				double p_y = distance*cos(angle) + RoverPose.y;
				int j = round((p_x/grid_resolution) + grid_width/2.0)-1;
				int i = round((p_y/grid_resolution) + grid_height/2.0)-1;
				if((RoverPose.x > bottom_left_X)  and (RoverPose.x < top_right_X) and (RoverPose.y > bottom_left_Y) and (RoverPose.y < top_right_Y))
				{
					if((i >= 0) and (i < grid_width) and (j >= 0) and (j < grid_height))
					{
						if(OccupancyGrid[i][j].Updated == 0)
						{
							double temp;
							int distance_index;
							int angle_index;
							int beam_index = s % Beams_Per_Sonar;
							double angle = (beam_index-(Beams_Per_Sonar/2.0)) * ((Sonar_Beamwidth/2.0)/(Beams_Per_Sonar/2.0));

							if(beam_intercepted == 0)
							{
								distance_index = 0;
								angle_index = 1000;
							}
							else
							{
								 distance_index = (int)1000.0*(distance/SonarScan.ranges[s]);
								 angle_index = (int)1000.0*(fabs(angle)/(Sonar_Beamwidth/2.0));
							}
							if(distance_index < 0) { distance_index = 0;}
							if(distance_index > (SonarModel_DistanceSize-1)) {distance_index = SonarModel_DistanceSize-1;}
							if(angle_index < 0) { angle_index = 0;}
							if(angle_index > (SonarModel_AngleSize-1)) {angle_index = SonarModel_AngleSize-1;}
							double P_distance = SonarModel_Distance.at(distance_index).value;
							double P_angle = SonarModel_Angle.at(angle_index).value;

							double P = P_distance * P_angle;
							OccupancyGrid[i][j].Probability = (P*OccupancyGrid[i][j].Probability)/((P*OccupancyGrid[i][j].Probability)+((1.0-P)*OccupancyGrid[i][j].Probability));
							if(OccupancyGrid[i][j].Probability < 0.0) { OccupancyGrid[i][j].Probability = 0.0;}
							if(OccupancyGrid[i][j].Probability > 1.0) { OccupancyGrid[i][j].Probability = 1.0;}
							OccupancyGrid[i][j].Value = (int)(100.0*OccupancyGrid[i][j].Probability);
							if(OccupancyGrid[i][j].Value < 0) { OccupancyGrid[i][j].Value = 0;}
							if(OccupancyGrid[i][j].Value > 100) {OccupancyGrid[i][j].Value = 100;}
							//if(s == 450)
							//{
								//printf("%d/%d t: %f i: %d D: %f d:%f P: %f P: %f V: %d\r\n",i,j,temp,distance_index,SonarScan.ranges[s],distance,P_distance,OccupancyGrid[i][j].Probability,OccupancyGrid[i][j].Value);
							//}
							//printf("i: %d j: %d x:%f y:%f theta:%f/%f  res: %f",i,j,p_x,p_y,angle,angle*180.0/PI,grid_resolution);
							//OccupancyGrid[i][j].Value = 255;

							/*if((distance >= .9*SonarScan.ranges[s]))
							{
								OccupancyGrid[i][j].Value = OccupancyGrid[i][j].Value + 1;
								if(OccupancyGrid[i][j].Value > 255) {OccupancyGrid[i][j].Value = 255; }
							}
							else
							{
								OccupancyGrid[i][j].Value = OccupancyGrid[i][j].Value - 1;
								if(OccupancyGrid[i][j].Value < 0) {OccupancyGrid[i][j].Value = 0;}
							}
							OccupancyGrid[i][j].Updated = 1;*/
						}
					}
				}

				distance = distance - grid_resolution;
			}

				angle = angle + SonarScan.angle_increment;
				//if(angle < SonarScan.angle_min) { break; }
			}
			//printf("FINISH\r\n");
			Grid_Finished = 1;
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	//printf("Mapping_Node update_occupancygrid Completed.\r\n");
}
void print_occupancygrid()
{
	try
	{
		printf("START\r\n");
		for(int i = 0; i < grid_width; i++)
		{
			for(int j = 0; j < grid_height; j++)
			{
				printf("i:%dj:%dN:%fE:%fv:%d   ",i,j,OccupancyGrid[i][j].Center_Northing_m,OccupancyGrid[i][j].Center_Easting_m,OccupancyGrid[i][j].Value);
			}
			printf("\r\n");
		}
		printf("FINISH\r\n");
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
}
void ICARUS_Rover_Pose_Callback(const geometry_msgs::Pose2D::ConstPtr& msg)
{
	//printf("Mapping_Node ICARUS_Rover_Pose_Callback Started.\r\n");
	//printf("Mapping_Node ICARUS_Rover_Pose_Callback Started.\r\n");
	try
	{
		if((abs(msg->x) < 15.0) and (abs(msg->y) < 15.0))
		{
			RoverPose.x = msg->x;
			RoverPose.y = msg->y;
			RoverPose.theta = msg->theta;
			New_Pose = 1;
			Pose_Valid = 1;
		}
		else
		{
			Pose_Valid = 0;
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	//printf("Mapping_Node ICARUS_Rover_Pose_Callback Completed.\r\n");
	//printf("Got a Pose x: %f y: %f theta: %f\r\n",msg->x,msg->y,msg->theta);
}
void ICARUS_Rover_Goal_Callback(const geometry_msgs::Pose2D::ConstPtr& msg)
{
	//printf("Mapping_Node ICARUS_Rover_Goal_Callback Started.\r\n");
	try
	{
		if((abs(msg->x) < 15.0) and (abs(msg->y) < 15.0) and (abs(msg->theta) < 10.0))
		{
			GoalPose.x = msg->x;
			GoalPose.y = msg->y;
			GoalPose.theta = msg->theta;
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	//printf("Mapping_Node ICARUS_Rover_Goal_Callback Completed.\r\n");
	//printf("Got a Goal x: %f y: %f theta: %f\r\n",msg->x,msg->y,msg->theta);
}
void ICARUS_Sonar_Scan_Callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
	//printf("Mapping_Node ICARUS_Sonar_Scan_Callback Started.\r\n");
	try
	{
		if(Pose_Valid == 1)
		{
			//cout << "Got a Scan" << endl;
			SonarScan.angle_min = msg->angle_min;
			SonarScan.angle_max = msg->angle_max;
			SonarScan.angle_increment = msg->angle_increment;
			SonarScan.range_min = msg->range_min;
			SonarScan.range_max = msg->range_max;
			SonarScan.ranges = msg->ranges;
			New_Scan = 1;
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	//printf("Mapping_Node ICARUS_Sonar_Scan_Callback Completed.\r\n");
}

int main(int argc, char **argv)
{
	try
	{
		ros::init(argc, argv, "Mapping_Node");
		ros::NodeHandle nh("~");

		nh.getParam("Operation_Mode",Mode);
		nh.getParam("Beams_Per_Sonar",Beams_Per_Sonar);
		nh.getParam("Sonar_Beamwidth",Sonar_Beamwidth);
		nh.getParam("Sonar_MaxDistance",Sonar_MaxDistance);
		//printf("Got Debug Mode: %d\r\n",0);
		//ros::Publisher Pub_ICARUS_Mapping_Diagnostic = nh.advertise<icarus_rover_rc::ICARUS_Diagnostic>("ICARUS_Mapping_Diagnostic",1000);
		ros::Rate loop_rate(3);
		std::clock_t    start;
		ros::Publisher Pub_ICARUS_OccupancyGrid;
		ros::Subscriber Sub_Rover_Pose;
		ros::Subscriber Sub_Sonar_Scan;
		ros::Subscriber Sub_Rover_Goal;
		ros::Subscriber Sub_Simulation_State;
		ros::Publisher Pub_Rover_GlobalPath;
		if(Mode.compare("SIM") == 0)
		{
			Pub_ICARUS_OccupancyGrid = nh.advertise<nav_msgs::OccupancyGrid>("ICARUS_SimOccupancyGrid",1000);
			Sub_Rover_Pose = nh.subscribe<geometry_msgs::Pose2D>("/Matlab_Node/ICARUS_SimRover_Pose",1000,ICARUS_Rover_Pose_Callback);
			Sub_Rover_Goal = nh.subscribe<geometry_msgs::Pose2D>("/Matlab_Node/ICARUS_SimRover_Goal",1000,ICARUS_Rover_Goal_Callback);
			Sub_Simulation_State = nh.subscribe<std_msgs::Int32>("/Matlab_Node/ICARUS_State",1000,ICARUS_SimulationState_Callback);
			//Sub_Sonar_Scan = nh.subscribe<sensor_msgs::LaserScan>("/Matlab_Node/ICARUS_SimSonar_Scan",1000,ICARUS_Sonar_Scan_Callback);
			cout << "Mapping Node: Sim Mode" << endl;
		}
		else if(Mode.compare("LIVE") == 0)
		{
			Pub_ICARUS_OccupancyGrid = nh.advertise<nav_msgs::OccupancyGrid>("ICARUS_OccupancyGrid",1000);
			Sub_Rover_Pose = nh.subscribe<geometry_msgs::Pose2D>("/Motion_Controller_Node/ICARUS_Rover_Pose",1000,ICARUS_Rover_Pose_Callback);
			Sub_Rover_Goal = nh.subscribe<geometry_msgs::Pose2D>("/Robot_Controller_Node/Rover_Goal",1000,ICARUS_Rover_Goal_Callback);
			//Sub_Sonar_Scan = nh.subscribe<sensor_msgs::LaserScan>("/Sonic_Controller_Node/ICARUS_Sonar_Scan",1000,ICARUS_Sonar_Scan_Callback);
			cout << "Mapping Node: Live Mode" << endl;
		}
		Sub_Sonar_Scan = nh.subscribe<sensor_msgs::LaserScan>("/Sonic_Controller_Node/ICARUS_Sonar_Scan",1000,ICARUS_Sonar_Scan_Callback);
		Pub_Rover_GlobalPath = nh.advertise<nav_msgs::Path>("ICARUS_Rover_GlobalPath",1000);
		initialize_occupancygrid();
		//::icarus_rover_rc::ICARUS_Diagnostic ICARUS_Diagnostic;
		nav_msgs::OccupancyGrid ros_OccupancyGrid;
		/*ICARUS_Diagnostic.header.frame_id = "ICARUS_Mapping_Diagnostic";
		ICARUS_Diagnostic.System = ROVER;
		ICARUS_Diagnostic.SubSystem = ROBOT_CONTROLLER;
		ICARUS_Diagnostic.Component = MAPPING_NODE;*/
		read_SonarModel_LUT();
		while(ros::ok())
		{
			//printf("Mapping_Node ROS Spin.\r\n");
			start = std::clock();
			ros::spinOnce();
			loop_rate.sleep();

			//printf("bx: %f by: %f tx: %f ty: %f\r\n",bottom_left_x,bottom_left_y,top_right_x,top_right_y);
			try
			{

				if((Grid_Finished == 1) and (New_Scan == 1))
				{
					//printf("Mapping_Node Grid Finished and New Scan.\r\n");
					Grid_Finished = 0;
					update_occupancygrid();
					ros_OccupancyGrid.header.stamp = ros::Time::now();
					ros_OccupancyGrid.info.resolution = grid_resolution;
					ros_OccupancyGrid.info.width = grid_width;
					ros_OccupancyGrid.info.height = grid_height;
					ros_OccupancyGrid.info.origin.position.x = bottom_left_X;
					ros_OccupancyGrid.info.origin.position.y = bottom_left_Y;
					ros_OccupancyGrid.data.resize(grid_width*grid_height);
					for(int i = 0; i < grid_width; i++)
					{
						for(int j = 0; j < grid_height;j++)
						{
							ros_OccupancyGrid.data[i*grid_width+j] = OccupancyGrid[i][j].Value;
						}
					}
					//ros_OccupancyGrid.origin.
					Pub_ICARUS_OccupancyGrid.publish(ros_OccupancyGrid);
					printf("Mapping_Node Published Grid\r\n");
					New_Scan = 0;
					Grid_Finished = 1;
					if(Pose_Valid == 1)
					{
						if(path_search() == 1)
						{
							//printf("Mapping_Node Path Search Complete\r\n");
							//("PATH FOUND!");
							std::reverse(PATH.begin(),PATH.end());
							//printf("Mapping_Node Reversed Path\r\n");
							nav_msgs::Path GlobalPath;
							GlobalPath.header.stamp = ros::Time::now();
							GlobalPath.header.frame_id = "map";
							GlobalPath.poses.resize(PATH.size());
							//os_OccupancyGrid.data.resize(grid_width*grid_height);
							for(int k = 0; k < PATH.size();k++)
							{
								//printf("%d(%d,%d,%f,%f,%f)->",PATH.at(k).ID,PATH.at(k).i,PATH.at(k).j,PATH.at(k).x,PATH.at(k).y,OccupancyGrid[PATH.at(k).i][PATH.at(k).j].Probability);
								GlobalPath.poses[k].header.stamp = ros::Time::now();
								GlobalPath.poses[k].pose.position.x = PATH.at(k).x;
								GlobalPath.poses[k].pose.position.y = PATH.at(k).y;

							}
							//printf("PATH COMPLETE.\r\n");
							Pub_Rover_GlobalPath.publish(GlobalPath);
							//printf("Mapping_Node Published Path\r\n");
						}
						else
						{
							//printf("Mapping_Node Path Search Failed\r\n");
						}
					}

				}
				dtime = (std::clock() - start) / (double)(CLOCKS_PER_SEC /1);

				//print_occupancygrid();
			}
			catch(exception& e)
			{
				cout << "RC Node: " << e.what() << endl;
				ROS_ERROR("%s",e.what());
			}
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
}
int path_search()
{
	//printf("Mapping_Node Path Search Started\r\n");
	try
	{
		OPEN_LIST.clear();
		CLOSED_LIST.clear();
		PATH.clear();
		Node NewNode;

		/*
		double p_x = distance*sin(angle) + RoverPose.x;
			double p_y = distance*cos(angle) + RoverPose.y;
			int j = round((p_x/grid_resolution) + grid_width/2.0)-1;
			int i = round((p_y/grid_resolution) + grid_height/2.0)-1;
		*/
		NewNode.x = RoverPose.x;
		NewNode.y = RoverPose.y;
		NewNode.i = round((NewNode.y/grid_resolution) + grid_height/2.0)-1;
		NewNode.j = round((NewNode.x/grid_resolution) + grid_width/2.0)-1;
		NewNode.ID = (NewNode.i*grid_width) + NewNode.j;
		NewNode.Parent_ID = -1; //No Parent here
		NewNode.f = 0.0;
		NewNode.g = 0.0;
		NewNode.h = 0.0;
		OPEN_LIST.push_back(NewNode);
		int Goal_i = round((GoalPose.y/grid_resolution) + grid_height/2.0) -1;
		int Goal_j = round((GoalPose.x/grid_resolution) + grid_width/2.0) -1;
		//int Goal_i = 80;
		//int Goal_j = 50;
		int Start_ID = NewNode.ID;
		//printf("Si: %d Sj: %d SID: %d Gi: %d Gj: %d\r\n",NewNode.i,NewNode.j,NewNode.ID,Goal_i,Goal_j);
		int iteration_counter = 0;
		int path_found = 0;
		while(OPEN_LIST.size() > 0)
		{
			//printf("\r\n\r\n\r\nI: %d OPEN:\r\n",iteration_counter);
			/*for(int l = 0; l < OPEN_LIST.size();l++)
			{
				printf(" ID: %d i: %d j: %d\r\n",OPEN_LIST.at(l).ID,OPEN_LIST.at(l).i,OPEN_LIST.at(l).j);
			}*/
			/*printf("I: %d CLOSED:\r\n",iteration_counter);
			for(int l = 0; l < CLOSED_LIST.size();l++)
			{
				printf(" ID: %d i: %d j: %d P: %d\r\n",CLOSED_LIST.at(l).ID,CLOSED_LIST.at(l).i,CLOSED_LIST.at(l).j,CLOSED_LIST.at(l).Parent_ID);
			}*/
			if(iteration_counter > (10*grid_width*grid_width))//I SEARCHED WAY TOO MANY CELLS
			{
				printf("NO PATH FOUND!\r\n");
				return 0;
			}
			//Assume the lowest f Node is the first one, and search the rest to see if there is anyone better.
			double min_f = OPEN_LIST.at(0).f;
			int index = 0;
			for(int i = 0; i < OPEN_LIST.size();i++)
			{
				if(OPEN_LIST.at(i).f < min_f)
				{
					min_f = OPEN_LIST.at(i).f;
					index = i;
				}
			}
			Node q = OPEN_LIST.at(index);
			OPEN_LIST.erase(OPEN_LIST.begin()+index);
			//printf("OPEN LIST SIZE: %d\r\n",OPEN_LIST.size());
			//printf("OPENING ID: %d P: %d\r\n",q.ID,q.Parent_ID);
			std::vector<Node> SUCCESSORS;
			//Add Successors
			if((q.i == 0) || (q.i == grid_width) || (q.j == 0) || (q.j == grid_width))
			{
				//printf("Border cell, don't check this.\r\n");
				break;
			} //Check if cell is on border.
			Node NewNode;
			//Add Successor 0

			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i+1;
			NewNode.j = q.j-1;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			//Add Successor 1
			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i+1;
			NewNode.j = q.j;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			//Add Successor 2
			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i+1;
			NewNode.j = q.j+1;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			//Add Successor 3
			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i;
			NewNode.j = q.j+1;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			//Add Successor 4
			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i-1;
			NewNode.j = q.j+1;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			//Add Successor 5
			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i-1;
			NewNode.j = q.j;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			//Add Successor 6
			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i-1;
			NewNode.j = q.j-1;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			//Add Successor 7
			NewNode.Parent_ID = q.ID;
			NewNode.i = q.i;
			NewNode.j = q.j-1;
			NewNode.ID = (NewNode.i * grid_width) + NewNode.j;
			SUCCESSORS.push_back(NewNode);
			/*for(int k = 0; k < SUCCESSORS.size();k++)
			{
				printf("S ID: %d (%d,%d),",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
			}
			printf("\r\n");*/
			for(int k = 0; k < SUCCESSORS.size();k++)
			{

				//printf("2S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
				SUCCESSORS.at(k).x = (double)((double)SUCCESSORS.at(k).j - (grid_width/2.0) + 1.0)*grid_resolution;
				SUCCESSORS.at(k).y = (double)((double)SUCCESSORS.at(k).i - (grid_height/2.0) + 1.0)*grid_resolution;
				double di;
				double dj;
				di = q.i - SUCCESSORS.at(k).i;
				dj = q.j - SUCCESSORS.at(k).j;
				double d_successor_to_q = pow((pow(di,2.0)+pow(dj,2.0)),0.5);
				//printf("3S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
				SUCCESSORS.at(k).g = q.g + d_successor_to_q;
				di = Goal_i - SUCCESSORS.at(k).i;
				dj = Goal_j - SUCCESSORS.at(k).j;
				double d_successor_to_goal = pow((pow(di,2.0)+pow(dj,2.0)),0.5);
				//printf("4S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);

				int add_successor = 1;
				if(( SUCCESSORS.at(k).i >= 0 ) and (SUCCESSORS.at(k).i < grid_width) and (SUCCESSORS.at(k).j >= 0) and (SUCCESSORS.at(k).j < grid_height))
				{
					SUCCESSORS.at(k).h = d_successor_to_goal;
					//printf("Here1\r\n");
					SUCCESSORS.at(k).f = SUCCESSORS.at(k).g + SUCCESSORS.at(k).h;
					if(OccupancyGrid[SUCCESSORS.at(k).i][SUCCESSORS.at(k).j].Probability > 0.5){ add_successor = 0; }

				}
				else
				{
					add_successor = 0;
				}

				/*
					int Value;
	int ID;
	int Updated;
	double Probability;
	double Center_Northing_m;
	double Center_Easting_m;
	int Changed;
				*/

				//printf("5S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
				//printf("OPEN LIST SIZE: %d\r\n",OPEN_LIST.size());
				if(OPEN_LIST.size() > 0)
				{
					for(int l = 0; l < OPEN_LIST.size();l++)
					{
						//printf("S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
						//printf("Checking: OPEN ID: %d w/ SUCCESSOR ID: %d\r\n",OPEN_LIST.at(l).ID,SUCCESSORS.at(k).ID);
						if(OPEN_LIST.at(l).ID == SUCCESSORS.at(k).ID)
						{
							//if(OPEN_LIST.at(l).f < SUCCESSORS.at(k).f) { add_successor = 0; }
							add_successor = 0;
						}
					}
				}
				//printf("S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
				for(int l = 0; l < CLOSED_LIST.size();l++)
				{
					//printf("S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
					if(CLOSED_LIST.at(l).ID == SUCCESSORS.at(k).ID)
					{
						//if(CLOSED_LIST.at(l).f < SUCCESSORS.at(k).f) { add_successor = 0; }
						add_successor = 0;
						//break;
					}
				}
				if(add_successor == 1)
				{
					//printf("OPEN_LIST PUSH S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
					OPEN_LIST.push_back(SUCCESSORS.at(k));
				}
				else
				{
					//printf("NOT PUSHING TO OPEN LIST: %d(%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
				}
			}
			CLOSED_LIST.push_back(q);
			for(int k = 0; k < OPEN_LIST.size();k++)
			{

			//printf("1S ID:%d (%d,%d)\r\n",SUCCESSORS.at(k).ID,SUCCESSORS.at(k).i,SUCCESSORS.at(k).j);
				if ((OPEN_LIST.at(k).i == Goal_i) and (OPEN_LIST.at(k).j == Goal_j))
				{
					//printf("PATH FOUND!\r\n");

					path_found =1;
					int path_complete = 0;
					int id = OPEN_LIST.at(k).ID;

					//printf("PATH ID's: ");
					//printf("%d(%d)->",id,OPEN_LIST.at(k).Parent_ID);
					id = OPEN_LIST.at(k).Parent_ID;
					while(path_complete == 0)
					{
						for(int l = 0; l < CLOSED_LIST.size();l++)
						{
							//printf("id: %d checking: %d\r\n",id,CLOSED_LIST.at(l).ID);
							if(id < 0)
							{
								path_complete = 1;
								//printf("PATH COMPLETE\r\n");
								//printf("Mapping_Node Path Search Returning\r\n");
								return 1;
							}
							else if(id == CLOSED_LIST.at(l).ID)
							{
								//printf("%d(%d)->",id,CLOSED_LIST.at(l).Parent_ID);
								id = CLOSED_LIST.at(l).Parent_ID;
								PATH.push_back(CLOSED_LIST.at(l));
							}
						}
					}

				}
			}
			//printf("Searching...\r\n");
			iteration_counter++;

		}
		if(path_found == 0)
		{
			//printf("PATH NOT FOUND!\r\n");
			return 0;
		}
	}
	catch(exception& e)
	{
		cout << "RC Node: " << e.what() << endl;
		ROS_ERROR("%s",e.what());
	}
	printf("Mapping_Node Path Search Finished\r\n");
}
  /*ICARUS_Diagnostic.header.stamp = ros::Time::now();
  ICARUS_Diagnostic.Diagnostic_Type = GENERAL_ERROR;
  ICARUS_Diagnostic.Level = SEVERE;
  ICARUS_Diagnostic.Diagnostic_Message = DEVICE_NOT_AVAILABLE;
  ICARUS_Diagnostic.Description = "Node Closed.";
  Pub_ICARUS_Mapping_Diagnostic.publish(ICARUS_Diagnostic);*/


//////////////////////////////////////////////////////////////////////////////////
// survival::src::main.cpp                                                      //
//																				//
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                   //
//  Software License, Version 1.0. (See accompanying file                       //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <boost/statistics/detail/math/constant/shortcut.hpp>
#include <libs/statistics/detail/distribution/survival/test/model/functions.cpp>
#include <libs/statistics/detail/distribution/survival/example/data/dataset.h>
#include <libs/statistics/detail/distribution/survival/example/data/random.h>
#include <libs/statistics/detail/distribution/survival/example/models/exponential.h>

int main()
{
    //test_model_functions<double>(std::cout);
    //example_data_dataset(std::cout);
    //example_data_random(std::cout);
    typedef boost::statistics::detail::math::constant::shortcut<double> const_;
    example_model_exponential(
        // TODO n_bath should be deduced from input (getline)
        5e4,            // runs,
        1e2,            // records
        1e3,            // n_mod
        1e4,            // proposals
        1e4,            // targets
        const_::inf,    // time of analysis
        "/Users/erwann/projets/2009/Xcode/distribution_survival/build/", // path
        std::cout
    );

    return 0;
}


/*
   Copyright (c) Marshall Clow 2011-2012.

   Distributed under the Boost Software License, Version 1.0. (See accompanying
   file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/

/// \file  partition_point.hpp
/// \brief Find the partition point in a sequence
/// \author Marshall Clow

#ifndef BOOST_ALGORITHM_PARTITION_POINT_HPP
#define BOOST_ALGORITHM_PARTITION_POINT_HPP

#include <iterator>    // for std::distance, advance

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

namespace boost { namespace algorithm {

/// \fn partition_point ( ForwardIterator first, ForwardIterator last, Predicate p )
/// \brief Given a partitioned range, returns the partition point, i.e, the first element
///     that does not satisfy p
///
/// \param first    The start of the input sequence
/// \param last     One past the end of the input sequence
/// \param p        The predicate to test the values with
/// \note           This function is part of the C++2011 standard library.
template <typename ForwardIterator, typename Predicate>
ForwardIterator partition_point ( ForwardIterator first, ForwardIterator last, Predicate p )
{
    std::size_t dist = std::distance ( first, last );
    while ( first != last ) {
        std::size_t d2 = dist / 2;
        ForwardIterator ret_val = first;
        std::advance (ret_val, d2);
        if (p (*ret_val)) {
            first = ++ret_val;
            dist -= d2 + 1;
            }
        else {
            last = ret_val;
            dist = d2;
            }
        }
    return first;
}

/// \fn partition_point ( Range &r, Predicate p )
/// \brief Given a partitioned range, returns the partition point
///
/// \param r        The input range
/// \param p        The predicate to test the values with
///
template <typename Range, typename Predicate>
typename boost::range_iterator<Range>::type partition_point ( Range &r, Predicate p )
{
    return boost::algorithm::partition_point (boost::begin(r), boost::end(r), p);
}

}}

#endif  // BOOST_ALGORITHM_PARTITION_POINT_HPP


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 6> test_int_1_6(const Eigen::Matrix<int, 1, 6> & M)
{
	return M;
}
void export_int_1_6()
{
	boost::python::def("test_int_1_6",test_int_1_6);
}