//
//  bpp.cpp
//  PhyloAcc
//
//  Created by hzr on 3/8/16.
//  Copyright © 2016 hzr. All rights reserved.
//

#include "bpp.hpp"
#include <armadillo>
#include <sys/types.h>
#include <dirent.h>
#include<queue>

#include <cmath>
#include <cassert>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cdf.h>

#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <ctype.h>

#include "newick.h"
#include "utils.h"
#include "bpp_c.hpp"

using namespace std;
using namespace arma;

// load the phylogenetic tree
void BPP::InitPhyloTree(PhyloTree & tree) //, double indel_pi), double indel, double indel2
{

//    cx_mat bvec;
//    cx_vec aval;
//
//    if(num_base <= 4)  // no indel
//    {
//
//    }else{
//        tree.subs_rate *= (1-indel);
//        mat B = ones<mat>(4,1) * indel;
//        mat C = ones<mat>(1,5) * indel2;
//        //tree.subs_rate.diag() -= indel;
//        tree.subs_rate.insert_cols(4, B);
//        tree.subs_rate.insert_rows(4, C);
//        //tree.subs_rate(4,4) = -0.04;
//        colvec c = sum(tree.subs_rate,1);
//        tree.subs_rate.diag() -=c;
//
////        tree.pi.insert_rows(4,1);
////        tree.pi.head(4) *= 1 - indel_pi;
////        tree.pi[4] = indel_pi;
//
//    }
//
//    eig_gen(aval, bvec, tree.subs_rate);
//    eigenval = conv_to<mat>::from(aval);
//    eigenvec = conv_to<mat>::from(bvec).t();
//    eigeninv = inv(eigenvec);
//    submat = tree.subs_rate;
//
//    //cout <<eigenval;
//    //cout <<"eigenvec: " << eigenvec;
//    //cout <<tree.subs_rate;
//
//    mat a = null(tree.subs_rate.t());
//
//
//    pi = a/accu(a); //tree.pi;
//    //cout <<"pi: " <<  pi.t();
//
//    log_pi = log(pi);

    submat = tree.subs_rate;
    children    = new int[N][2];
    parent      = new int[N];
    distances   = new double[N];

    for(int s=0; s<N; s++)
    {
        distances[s] = tree.distances[s];

    }

    for(int i=0; i<N; i++)
    {
        children[i][0] = -1;
        children[i][1] = -1;
        parent[i] = -1;
    }

    for(int i=0; i<N; i++)
    {
        int p = -1;
        for(int j=0; j<N; j++)
        {
            if (tree.dag[i][j])
            {
                p++;
                children[i][p] = j;
                parent[j] = i;
            }
        }
    }

//    distances[children[N-1][1]] += distances[children[N-1][0]];
//    distances[children[N-1][0]] = 0;
//    moveroot = children[N-1][0];

    //distances[83] += distances[42];  // modify distance for root!
    //distances[42] =0 ;

}

// try to match the phylogenetic profile and tree
void BPP::MatchProfAndTree(PhyloProf & _prof, PhyloTree & _tree)
{

    // try to match the species
    bool success_match = true;
    int S = _tree.S;
    int S2 = _prof.S;
    vector<int> reorder(S);  //each species in the tree where is in prof
    for(int s1=0; s1<S; s1++)
    {
        bool has_same_species = false;
        string sname1 = _tree.species_names[s1];
        for(int s2=0; s2<S2; s2++)
        {
            string sname2 = _prof.species_names[s2];
            //            cout << sname1 << " ? " << sname2 << endl;
            if (sname1 == sname2)
            {
                has_same_species = true;
                reorder[s1] = s2;
                break;
            }
        }
        if (!has_same_species)
        {
            cout << "No matrix species " << _prof.species_names[s1] << " found in tree." << endl;
            success_match = false;
            break;
        }
    }

    if (!success_match) // if cannot match literally
    {
        cout << endl << "The species in phylogenetic profile and tree cannot be matched literally:" << endl;
        cout << "The program will use the default mapping in data:" << endl;
        for(int s=0; s<S; s++)
            cout << "(" << _prof.species_names[s] << "\t=  " << _tree.species_names[s] << ")" << endl;
        cout << endl;
    }
    else                // if successully matched
    {
        cout << "The species in profile and tree match perfectly. Reorder the species in profile matrix by the tree." << endl << endl;
        vector<string> old_X = _prof.X;
        for(int s=0; s<S; s++)
        {
            int reorder_s = reorder[s];
            _prof.X[s] = old_X[reorder_s];
        }
        _prof.species_names = _tree.species_names;
    }

}

void BPP::InitMCMC(int _num_burn, int _num_mcmc, int _num_thin)
{
    // init parameters
    num_burn = _num_burn;

    num_mcmc = _num_mcmc;
    num_thin = _num_thin;

    last_time = time(NULL);

    // init MCMC sampling storage
    Max_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));
    Max_B = vector<vector < vector <bool > >> (3,vector < vector <bool >>  (C,vector <bool > (N,0)));
    cur_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));
    //cur_B = vector < vector <double > > (3, vector <double >  (C, 2.0));

    log_liks_null = vector <double>(C,0);
    log_liks_Z = vector<vector <double>>(3,vector <double>(C,0));
    log_liks_sgl = vector <double>(C,0);
    log_liks_resZ = vector <double>(C,0);

    log_liks_curZ = vector <double>(C,0);
    log_liks_propZ = vector <double>(C,0);
    MH_ratio_gain = vector <double>(C,0);
    MH_ratio_loss = vector <double>(C,0);

    cur_crate = vector <double>(C,ratio0);
    cur_nrate = vector <double>(C,ratio1);
    cur_nGB = vector <double>(C,3);
    //cur_cGB = vector <double>(C,0.01);

}

void BPP::sample_proposal(int iter, double & lrate_prop, double & grate_prop, ofstream & output)
{
    //indel_prop =gsl_ran_gamma(RNG,20 ,indel/20);
    //indel2_prop =gsl_ran_gamma(RNG,10 ,indel2/10);
    lrate_prop =gsl_ran_beta(RNG, ind_lrate * vlr, (1 - ind_lrate) *vlr);
    grate_prop =gsl_ran_beta(RNG, ind_grate * vgr, (1 - ind_grate) *vgr); //gsl_ran_gamma(RNG, vgr, ind_grate/vgr);

//    submat.submat(0,0,3,3) *= (1 - indel_prop)/(1 - indel);
//    submat.col(4) = ones<mat>(5,1) * indel_prop;
//    submat.row(4) = ones<mat>(1,5) * indel2_prop;
//    colvec c = sum(submat,1);
//    submat.diag() -=c;
//
//    cx_mat bvec;
//    cx_vec aval;
//    eig_gen(aval, bvec, submat);
//    eigenvalprop = conv_to<mat>::from(aval);
//    eigenvecprop = conv_to<mat>::from(bvec).t();
//    eigeninvprop = inv(eigenvecprop);
//
//    mat a = null(submat.t());
//    piprop = a/accu(a);
//    //log_piprop = log(a/accu(a));
//
//    //cout <<"proposed subs matrix: " << submat << endl;
//    //cout <<"piprop: " <<  piprop.t();

    output << iter << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< indel << "\t"<< indel2 << "\t"<<ind_grate<< "\t"<< ind_lrate <<endl;
}

double BPP::log_lik(vector< vector<vec> > & lambda, double _indel, double _indel2, int start1, int end1, vector<unsigned int> & v, double p)
{
    // compute loglik
    double result =0;
    mat x(2,2);

    int rr = *subtree.rbegin();
    // 1. sending the lambda msg from leaves bottom up through the network
    for(vector<int>::iterator it = subtree.begin(); it!=subtree.end(); it++) //int s=S; s<N; s++)
    {
        int s = *it;
        if(s<S) continue;
        int* p = children[s];
        for(int it = start1; it < end1; it++)  lambda[v[it]][s].fill(0);

        for(int cc=0;cc<2;cc++)
        {
            int chi = p[cc];
            assert(chi != -1);
            if(distances[chi]>0 )
            {
                double tt = (1 - exp(-(_indel + _indel2) * distances[chi]))/(_indel + _indel2);
                x.at(1,0) = _indel * tt;
                x.at(0,0) = 1 - x.at(1,0);

                x.at(0,1) = _indel2 * tt;
                x.at(1,1) = 1 - x.at(0,1);

                //cout << x;
                x = log(x);

            }
            else{
                x.fill(-INFINITY); //83, root
                x.diag().fill(0);
            }

            #pragma omp parallel for schedule (guided)
            for(int it = start1; it < end1; it++)  lambda[v[it]][s] +=  BPP::log_multi(x,lambda[v[it]][chi]);
        }

    }

    // 2. processing the distribution of root species

    for(int it = start1; it < end1; it++)
    {

//        if(lambda[v[it]][N-1][1] < -1e3)
//        {
//            cout <<v[it]<<": "<< lambda[v[it]][N-1].t();
//            cout <<children[N-1][0]<<": " << lambda[v[it]][children[N-1][0]].t();
//            cout <<children[N-1][1]<<": " << lambda[v[it]][children[N-1][1]].t();
//
//        }
        lambda[v[it]][rr][0] += log(1-p); //N-1
        lambda[v[it]][rr][1] += log(p) ;
        result += BPP::log_exp_sum(lambda[v[it]][rr]);
    }

    return(result);
}

void BPP::sample_hyperparam(double lrate_prop, double grate_prop) // recompute log_TM, double indel_prop, double indel2_prop,
{

    //indepent MH to sample hyperparam of rates
    double p=1,r = 1; //hyperparam for shape
    double q=0.1,s = 0.1; // hyperparam for scale

    double vna = 100, vnb = 100, vca = 100, vcb = 100;
    double nprior_a_prop =gsl_ran_gamma(RNG, vna, nprior_a/vna);
    double cprior_a_prop =gsl_ran_gamma(RNG, vca, cprior_a/vca);

    double nprior_b_prop =gsl_ran_gamma(RNG, vnb, nprior_b/vnb);
    double cprior_b_prop =gsl_ran_gamma(RNG, vcb, cprior_b/vcb);

    //MH proposal
    double sum_r = 0;
    double log_prod_r = 0;
    //double var_r = 0;
    for(vector<double>::iterator it = cur_nrate.begin(); it< cur_nrate.end(); it++)
    {
        sum_r += *it;
        //var_r += pow(*it, 2);
        log_prod_r += log(*it);
    }

    double M_ratio = (nprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b_prop - (C + r)*lgamma(nprior_a_prop) - log(nprior_b_prop) * nprior_a_prop * (s + C);
    M_ratio -= (nprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b - (C + r)*lgamma(nprior_a) - log(nprior_b) * nprior_a * (s + C);

    double H_ratio = log(gsl_ran_gamma_pdf(nprior_a,vna,nprior_a_prop/vna)) - log(gsl_ran_gamma_pdf(nprior_a_prop,vna,nprior_a/vna)) + log(gsl_ran_gamma_pdf(nprior_b,vnb,nprior_b_prop/vnb)) - log(gsl_ran_gamma_pdf(nprior_b_prop,vnb,nprior_b/vnb));

    cout << "nrate_MH_ratio: " << M_ratio <<", " << H_ratio << ", " << nprior_a << ", " << nprior_a_prop << ", " << nprior_b << ", " << nprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        nprior_a = nprior_a_prop;
        nprior_b = nprior_b_prop;
    }

    sum_r = 0;
    log_prod_r = 0;
    for(vector<double>::iterator it = cur_crate.begin(); it< cur_crate.end(); it++)
    {
        sum_r += *it;
        //var_r += pow(*it, 2);
        log_prod_r += log(*it);
    }

    M_ratio = (cprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b_prop - (C + r)*lgamma(cprior_a_prop) - log(cprior_b_prop) * cprior_a_prop * (s + C) - ((cprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b - (C + r)*lgamma(cprior_a) - log(cprior_b) * cprior_a * (s + C));

    H_ratio = log(gsl_ran_gamma_pdf(cprior_a,vca,cprior_a_prop/vca)) - log(gsl_ran_gamma_pdf(cprior_a_prop,vca,cprior_a/vca)) + log(gsl_ran_gamma_pdf(cprior_b,vcb,cprior_b_prop/vcb)) - log(gsl_ran_gamma_pdf(cprior_b_prop,vcb,cprior_b/vcb));

    cout << "crate_MH_ratio: " << M_ratio + H_ratio << ", " << cprior_a << ", " << cprior_a_prop <<", " << cprior_b << ", " << cprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        cprior_a = cprior_a_prop;
        cprior_b = cprior_b_prop;
    }

    //MH to sample lrate, grate
    //lrate
    double MH_ratio =0;
    for(int c =0 ; c<C; c++)
    {
        MH_ratio += MH_ratio_gain[c];
    }

    MH_ratio += log(gsl_ran_beta_pdf(ind_grate, grate_prop*vgr, (1-grate_prop)*vgr)) - log(gsl_ran_beta_pdf(grate_prop, ind_grate*vgr, (1-ind_grate)*vgr));

    cout << "grate: " << MH_ratio << ", " << ind_grate << ", " << grate_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < MH_ratio)
    {
        ind_grate = grate_prop;
        for(int s=0; s<N; s++)
        {

            //double x = exp(-ind_grate *distances[s]);
            double x = 1 - ind_grate;
            TM_Int[s](0,0) = x;
            TM_Int[s](1,0) = 1-x;

        }
    }

    MH_ratio = 0;
    for(int c =0 ; c<C; c++)
    {
        MH_ratio += MH_ratio_loss[c];
    }
    MH_ratio += log(gsl_ran_beta_pdf(ind_lrate, lrate_prop*vlr, (1-lrate_prop)*vlr)) - log(gsl_ran_beta_pdf(lrate_prop, ind_lrate*vlr, (1-ind_lrate)*vlr));

    cout << "lrate: " << MH_ratio << ", " << ind_lrate << ", " << lrate_prop << endl << endl;

    if(log(gsl_rng_uniform(RNG)) < MH_ratio)
    {
        ind_lrate = lrate_prop;
        for(vector<int>::iterator it = subtree.begin(); it<subtree.end(); it++)
        {
            int s = *it;
            //double y = exp(-ind_lrate *distances[s]);
            double y = 1 - ind_lrate;
            TM_Int[s](1,1) = y;
            TM_Int[s](2,1) = 1-y;

        }
    }

    for(int s=0; s<N; s++){
        log_TM_Int[s] = log(TM_Int[s]);
    }

}

void BPP::getUppertree(int root, vector<int>& child, set<int> & visited_init)  // include root!
{
    for(vector<int>::iterator it = child.begin(); it!=child.end(); it++)
    {
        int p = *it;
       while(p!=root)
       {
           visited_init.insert(p);
           p = parent[p];
       }

    }

    visited_init.insert(root);

}

void BPP::getSubtree(int root, vector<int> & visited_init)  // traverse from root to children, include root
{

    int j = root;

    //cout << nodes_names[j]<<"\t";

    if(children[j][0]!=-1)
    {

        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], visited_init);

        }

    }

    visited_init.push_back(j);

}

void BPP::getSubtree(int root, set<int>& child, vector<int> & visited_init)  // traverse from root, stop at children, 74 & 64; do include 1-S!
{

    int j = root;

    //cout << nodes_names[j]<<"\t";
    if(child.find(j) != child.end())
    {

        visited_init.push_back(j);

        return;
    }

    if(children[j][0]!=-1)
    {

        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], child, visited_init);

        }

      //  visited_init.push_back(j);

    }

     visited_init.push_back(j);

}

void BPP::Output_init(PhyloProf & prof, string output_path){

        string outpath_elem = output_path+ "_elem_lik.txt";
        ofstream out_lik(outpath_elem.c_str());
        out_lik.precision(8);
        out_lik << "No.\tID\tloglik_NUll\tloglik_RES\tloglik_all\tlog_ratio\tloglik_Max1\tloglik_Max2\tloglik_Max3"<<endl;
        for(int cc=0; cc<C;cc++)
        {
            out_lik <<cc << "\t" << prof.element_names[cc] << "\t" << log_liks_null[cc] <<"\t"  <<log_liks_resZ[cc] <<"\t"  <<log_liks_sgl[cc]<< "\t"<< log_liks_resZ[cc] -  log_liks_null[cc];
            for(int r=0;r<3;r++) out_lik <<"\t" <<log_liks_Z[r][cc];
            out_lik << endl;
        }

        out_lik.close();

    ofstream out_z;
    for(int r =1;r<2;r++)
    {
            outpath_elem = output_path+"_" +to_string(r) + "_elem_Z.txt";
            out_z.open(outpath_elem.c_str());

                for(int s =0 ;s<N;s++){  // header: species name
                    out_z<<nodes_names[s] << "_B\t";
                }
        for(int s =0 ;s<N;s++){  // header: species name
            out_z<<nodes_names[s] << "\t";
        }
                out_z <<endl;
            for(int c=0;c<C;c++)
            {
                    for(int s=0; s<N;s++)
                            out_z<<Max_Z[r][c][s]<<"\t";
                    out_z <<endl;
            }
        out_z.close();
    }

}

void BPP::Output_init0(PhyloProf & prof, ofstream& out_lik){

    for(int cc=0; cc<C;cc++)
    {
        out_lik <<cc << "\t" << prof.element_names[cc] << "\t"  <<log_liks_sgl[cc]<< "\t"<<log_liks_Z[1][cc];
        out_lik << endl;
    }

    //out_lik.close();

}


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
//
// Performs a unary operation or a cast on a tensor with any desired shape,
// mapped in any desired way among tiles.

#include <poplar/Engine.hpp>
#include <popops/Zero.hpp>

#include "poputil/VertexTemplates.hpp"

#include "../lib/popops/ExprOpUtil.hpp"
#include <boost/format.hpp>
#include <poplibs_test/Util.hpp>
#include <popnn/NonLinearity.hpp>
#include <popnn/codelets.hpp>
#include <popops/Cast.hpp>
#include <popops/codelets.hpp>

#include "codelets/UnaryCodeletsTest.hpp"
#include <algorithm>
#include <cfenv>
#include <cstdlib>
#include <exception>
#include <fstream>
#include <optional>
#include <type_traits>

using namespace poplar::program;
using namespace poputil;
using namespace poplibs_test::util;

// Some non linearities can be executed also via popnn calls as well, in
// addition to the popops 'map' call.
// This returns the equivalent popnn enum or nullopt if it doesn't exist.
std::optional<popnn::NonLinearityType> popnnNLType(UnaryOpType op) {
  switch (op) {
  case UnaryOpType::TANH:
    return popnn::NonLinearityType::TANH;
  case UnaryOpType::SIGMOID:
    return popnn::NonLinearityType::SIGMOID;
  case UnaryOpType::RELU:
    return popnn::NonLinearityType::RELU;
  default:
    return std::nullopt;
  }
}

//*************************************************************************
/// Verifies if the results of the operation performed on the device match
/// with the one the host
///
/// \param deviceType          The device used.
/// \param dataType            The data type used (float, half).
/// \param in1Host             Data buffer for the first operand.
/// \param shape1Ext           Shape for first operand, rank-extended.
/// \param in2Host, shape1Ext  Data and shape for second operand.
/// \param outHost, outShape   Data (and shape) for result, obtained from
///                            device and converted to host types.
/// \param operation           Operation performed on device.
template <typename HostDataType, typename HostOutType>
static bool
verifyResult(const DeviceType &deviceType, const Type &dataType,
             const Type &outType, const std::vector<HostDataType> &inHost,
             const std::vector<size_t> &shapeExt,
             const std::vector<HostOutType> &outHost, const Operation op) {
  bool isIpuModel_ = isIpuModel(deviceType);
  unsigned errCount = 0; // how many mismatched elements we find

  unsigned n = shapeExt.size(); // How many dimensions we have

  bool opIsCast = isCast(op);
  UnaryOpType unaryOp;
  std::string opStr;
  if (opIsCast) {
    opStr = "cast<" + outType.toString() + ">";
    if (isSimulator(deviceType) || isHw(deviceType)) {
      // Currently the IPU has only 1 rounding mode for floating point to int
      // conversions (f32toi32/f32toui32 instructions): Round-To-Nearest,
      // Ties-To-Even (see use of 'nearbyint()' in 'performCast'
      std::fesetround(FE_TONEAREST);
    } else {
      std::fesetround(FE_TOWARDZERO);
    }
  } else {
    unaryOp = std::get<UnaryOpType>(op);
    opStr = unaryOpToString.at(unaryOp);
  }

  // Perform the specified 'op'eration element-wise between in1 and in2 (on
  // the host) and compare with what is returned by the device.
  // We need to nest a variable number ('n') of loops to scan through all
  // 'n' dimensions. We use a recursive function the will recur once for each
  // nested loop ('n' times).
  // The vector 'i[]' contains the indices into 'in1', 'in2, 'out'
  std::vector<unsigned> i(n);
  // Cannot use 'auto' for the type, because it's a recursive function.
  std::function<void(unsigned)> loopOn = [&](unsigned k) {
    // Run the k-th nested loop
    for (i[k] = 0; i[k] < shapeExt[k]; i[k]++) {
      if (k == n - 1) {
        // This is the "innermost loop"; we need to compute:
        // expected[ i[0], i[1],... ] =
        //                in1[ i[0], i[1],... ] *OP*  in2[ i[0], i[1],... ]
        // and compare with the actual value from the device

        HostOutType actual = get(outHost.data(), shapeExt, i); // from device

        HostDataType val = get(inHost.data(), shapeExt, i);

        HostOutType expected = 0;

        if (opIsCast) {
          performCast(isIpuModel_, val, expected, dataType, outType);
        } else {
          performOp(unaryOp, val, expected);
        }
        bool equal = equalValues(isIpuModel_, op, dataType, expected, actual);

        if (!equal) {
          std::cerr << "out[" << i[0];
          for (unsigned j = 1; j < n; j++)
            std::cerr << "," << i[j];
          std::cerr << "] = " << opStr << "(" << convertToString(val)
                    << ") =>  expected:" << convertToString(expected)
                    << ";  actual:" << convertToString(actual) << "\n";
          errCount++;
        }
      } else {
        loopOn(k + 1); // recur to go down to next nested loop
      }
    }
  };
  loopOn(0);

  if (errCount > 0) {
    std::cerr << "Failed: mismatch on " << errCount << " value(s)\n";
  }
  return errCount == 0;
}

//*************************************************************************
/// Do a unary operation, where the operand is described by 'desc1'.
///
/// \param deviceType            The device used.
/// \param dataType              The data type used for the two operands.
/// \param outputType            The type for the result of the operation.
/// \param desc                  Description for first operand.
/// \param tiles                 How many tiles to allocate.
/// \param mapLinearly           If yes, we map linearly the two operands on
///                              all tiles.
/// \param operation             Operation performed on device.
/// \param inPlace               Is the operation to be done in place?
/// \param doReport              Print poplar report.
/// \param doPrintTensors        Print the tensor (for verification).
/// \param ignoreData            Do not verify results.
/// \param enableOptimisations   Enable broadcasted vector op optimisations.
template <typename HostDataType, typename HostOutType>
static bool doUnaryOpTest(const DeviceType &deviceType, const Type &dataType,
                          const Type &outputType, const OperandDescriptor &desc,
                          const unsigned tiles, const bool mapLinearly,
                          const Operation operation, const bool inPlace,
                          const bool doReport, const bool doPrintTensors,
                          const unsigned randomSeed, const bool ignoreData,
                          const bool enableOptimisations,
                          const bool popnnNonLinearity) {

  UnaryOpType unaryOp = UnaryOpType::NEGATE;
  std::string opStr;
  bool isCastOp = isCast(operation);
  if (!isCastOp) {
    unaryOp = std::get<UnaryOpType>(operation);
  }

  auto nElems = std::accumulate(desc.shape.begin(), desc.shape.end(),
                                std::size_t(1), std::multiplies<std::size_t>());

  if (!isCastOp) {
    if (inPlace && (outputType != dataType)) {
      throw std::runtime_error(
          "For in place operations, the data and output "
          "types must be the same (specified data type=" +
          dataType.toString() +
          ", specified output type=" + outputType.toString() + ")");
    }

    if (isIntOp(unaryOp) && (dataType == HALF || dataType == FLOAT)) {
      throw std::runtime_error(unaryOpToString.at(unaryOp) +
                               " requires data "
                               "of integer type (specified  data type=" +
                               dataType.toString() + ")");
    }
  }

  // Allocate and initialise host buffers with appropriate values.
  std::vector<HostDataType> inHost(nElems);
  fillHostBuffer(operation, dataType, randomSeed, inHost);

  // Create Graph object, target and device
  auto device = createTestDevice(deviceType, 1, tiles);
  Target target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  std::vector<poplar::Tensor> tensors;
  Tensor in = graph.addVariable(dataType, desc.shape, "in");
  mapTensor(graph, in, mapLinearly, desc.map);

  // Make a program sequence to run the operation
  Sequence prog;
  Tensor out;

  if (isCastOp) {
    out = cast(graph, in, outputType, prog);
  } else {
    std::optional<popnn::NonLinearityType> nlType = popnnNLType(unaryOp);
    if (popnnNonLinearity && nlType) {
      if (inPlace) {
        nonLinearityInPlace(graph, *nlType, in, prog);
        out = in;
      } else {
        out = nonLinearity(graph, *nlType, in, prog);
      }
    } else {
      OptionFlags opOpts{{"enableVectorBroadcastOptimisations",
                          (enableOptimisations ? "true" : "false")}};
      if (inPlace) {
        mapInPlace(graph, unaryOp, {in}, prog, "", opOpts);
        out = in;
      } else {
        out = map(graph, unaryOp, {in}, prog, "", opOpts);
      }
    }
  }

  // Create host 'transfer' buffers with the right size for the device type
  std::vector<std::pair<std::string, char *>> tmap;
  Sequence uploadProg, downloadProg;
  std::unique_ptr<char[]> inHostRaw;
  std::unique_ptr<char[]> outHostRaw;
  char *outHostRawPtr = nullptr;
  inHostRaw = allocateHostMemoryForTensor(in, "in", graph, uploadProg,
                                          downloadProg, tmap);
  if (!inPlace) {
    outHostRaw = allocateHostMemoryForTensor(out, "out", graph, uploadProg,
                                             downloadProg, tmap);
    outHostRawPtr = outHostRaw.get();
  } else {
    outHostRawPtr = inHostRaw.get();
  }

  // Copy and convert the data from the initialised buffers to the transfer
  // buffers (still on host)
  auto copyBuffer = [&](std::vector<HostDataType> &buf,
                        std::unique_ptr<char[]> &rawBuf) {
    copy(target, buf.data(), buf.size(), dataType, rawBuf.get());
    // For HALF, we copy and convert back into the (float) host buffers so that
    // the host buffers contain the exact HALF values (which are exactly
    // representable in float). This helps with the validation for the
    // comparison operators
    if (dataType == HALF)
      copy(target, dataType, rawBuf.get(), buf.data(), buf.size());
  };
  copyBuffer(inHost, inHostRaw);

  if (doPrintTensors) {
    prog.add(PrintTensor("in", in));
    if (!inPlace)
      prog.add(PrintTensor("out", out));
  }

  // Run sequences
  OptionFlags engOpts;
  if (doReport) {
    engOpts.set("debug.instrumentCompute", "true");
  }
  Engine engine(graph, Sequence{uploadProg, prog, downloadProg}, engOpts);
  attachStreams(engine, tmap);

  device.bind([&](const Device &d) {
    engine.load(d);
    engine.run();

    if (doReport) {
      OptionFlags opt;
      opt.set("showExecutionSteps", "true");
      engine.printProfileSummary(std::cout, opt);
    }
  });

  // Check the result
  if (ignoreData) {
    std::cout << "Result not checked for correctness\n";
  } else {
    // Get the result out of the device
    std::vector<HostOutType> outHost(nElems);
    copy(target, outputType, outHostRawPtr, outHost.data(), outHost.size());
    return verifyResult<HostDataType, HostOutType>(
        deviceType, dataType, outputType, inHost, desc.shapeExt, outHost,
        operation);
  }
  return true;
}

//*************************************************************************
int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType;
  Type dataType;

  std::string operationStr;
  bool doReport = false;
  bool doPrintTensors = false;
  bool inPlace = false;
  unsigned tiles = 0;
  bool ignoreData = false;
  unsigned randomSeed = 1; // we use '0' to mean 'not random'
  bool enableOptimisations = true;
  bool popnnNonLinearity = false;
  ShapeOption<size_t> shape;
  OperandDescriptor opDesc;

  // clang-format off
  const static std::string description =
  "Performs a unary operation between two tensors having any specified \n"
  "shape, each mapped in any desired way among tiles.\n"
  "\n"
  "Options are:";

  po::options_description poDesc(description);

  poDesc.add_options()
    ("help", "Print help")
    ("report",
     po::value<bool>(&doReport)->implicit_value(true),
     "Provide a poplar report")
    ("options-file",
     po::value<std::string>(),
     "A file containing options, with the same syntax as the command line; "
     "can be also specified with '@options_file_name'")
    ("print",
     po::value<bool>(&doPrintTensors)->implicit_value(true),
     "Print the tensors")
    ("random-seed",
     po::value<unsigned>(&randomSeed)->implicit_value(randomSeed),
     "Seed for random data. Value of 0 means 'no random data'")
    ("ignore-data",
     po::value<bool>(&ignoreData)->implicit_value(true),
     "Do not check correctness of result, useful for benchmarking without "
     "overhead of host-side computation")
    ("device-type",
     po::value<DeviceType>(&deviceType)->default_value(DeviceType::Sim2),
     "Device Type")
    ("data-type",
     po::value<Type>(&dataType)->default_value(HALF),
     "Data Type: half, float, int, unsigned, bool")
    ("in-place",
     po::value<bool>(&inPlace)->implicit_value(true),
     "Do the specified operation in place")
    ("tiles",
     po::value<unsigned>(&tiles)->default_value(tiles),
     "Number of tiles to use for linearly mapping the operands. If "
     "unspecified, or 0, do not map linearly the operands (use only the "
     "explicit mapping specified by --map)")
    ("shape",
     po::value<ShapeOption<size_t>>(&shape)->multitoken()->required(),
     "Shape for the operand, curly bracket delimited:  {d1,d2,...}.")
    ("map",
     po::value<std::vector<MappingDesc>>(&opDesc.map)->multitoken(),
     "Tile mapping for the operand; a sequence of one or more: "
     "T:{d1,d2,...} ... , where T is the tile number and {d1,d2,...} is the "
     "slice mapped on T. If not specified, the operand is mapped linearly on "
     "the allocated tiles.")
    ("operation",
     po::value<std::string>(&operationStr)->required(),
     ("Operation to perform, one of: " + allOpsStr() + ", or 'cast<OUT_TYPE>'")
     .c_str())
    ("enable-optimisations",
     po::value<bool>(&enableOptimisations)->default_value(enableOptimisations),
     "Enable broadcast operation optimisations")
    ("popnn-non-linearity",
     po::value<bool>(&popnnNonLinearity)->implicit_value(true),
     "Run the non linearity through popnn library calls, if available")
    ;
  // clang-format on
  parseOptions(argc, argv, poDesc);

  auto [operation, outputType] = stringToOperation(operationStr, dataType);

  // Find the shape of the output, applying the broadcasting rules
  // First, get the 'extended to the left' operand shapes; for instance, if
  // the two operands have shapes {9,8,7,6} and {7,1}, the second is
  // 'extended' to {1,1,7,1}
  opDesc.shape = shape.val;
  unsigned n = opDesc.shape.size();
  opDesc.shapeExt = extendShape(opDesc.shape, n);

  bool mapLinearly = tiles > 0;

  if (tiles == 0) {
    // Find the highest tile number in the tile mapping for the two operands
    for (auto m : opDesc.map) {
      tiles = std::max(tiles, m.tile);
    }
    tiles++;
  }

#define SELECT_ONE(DATA_TYPE, OUT_TYPE, HOST_DATA_TYPE, HOST_OUT_TYPE)         \
  if (dataType == DATA_TYPE && outputType == OUT_TYPE) {                       \
    return doUnaryOpTest<HOST_DATA_TYPE, HOST_OUT_TYPE>(                       \
               deviceType, dataType, outputType, opDesc, tiles, mapLinearly,   \
               operation, inPlace, doReport, doPrintTensors, randomSeed,       \
               ignoreData, enableOptimisations, popnnNonLinearity)             \
               ? 0                                                             \
               : 1;                                                            \
  } // nonzero value = error
  SELECT_BY_TYPES()
  throw invalid_types(dataType, outputType);
}


/*
 * Copyright (c) 2018 Tuukka Norri
 * This code is licensed under MIT license (see LICENSE for details).
 */

#define BOOST_TEST_MODULE "Unit tests"
// For some reason BOOST_TEST_DYN_LINK is required on macOS
// but cannot be applied on Linux.
#ifdef __APPLE__
#	define BOOST_TEST_DYN_LINK
#endif
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <libbio/int_vector.hh>
#include <text_align/alignment_graph_builder.hh>
#include <text_align/code_point_range.hh>
#include <text_align/smith_waterman/aligner.hh>
#include <text_align/smith_waterman/alignment_context.hh>

#include <tuple>
#include <type_traits>

namespace ta = text_align;

// For aligner tests.
typedef std::int32_t score_type;

template <typename t_block>
using alignment_context_type = text_align::smith_waterman::alignment_context <score_type, t_block, libbio::bit_vector>;

template <typename t_range>
std::size_t copy_distance(t_range range)
{
	return ranges::distance(range);
}

template <typename t_block>
void run_aligner(
	alignment_context_type <t_block> &ctx,
	std::string const &lhs,
	std::string const &rhs,
	typename alignment_context_type <t_block>::bit_vector_type const &expected_lhs,
	typename alignment_context_type <t_block>::bit_vector_type const &expected_rhs,
	score_type const expected_score,
	std::size_t const block_size,
	score_type const match_score,
	score_type const mismatch_penalty,
	score_type const gap_start_penalty,
	score_type const gap_penalty
)
{
	auto &aligner(ctx.get_aligner());

	aligner.set_segment_length(block_size);
	aligner.set_identity_score(match_score);
	aligner.set_mismatch_penalty(mismatch_penalty);
	aligner.set_gap_start_penalty(gap_start_penalty);
	aligner.set_gap_penalty(gap_penalty);
	aligner.set_reverses_texts(true);

	auto const lhsr(ta::make_reversed_code_point_range(ranges::view::reverse(lhs)));
	auto const rhsr(ta::make_reversed_code_point_range(ranges::view::reverse(rhs)));
	auto const lhs_len(copy_distance(lhsr));
	auto const rhs_len(copy_distance(rhsr));
	assert(lhsr.begin() != lhsr.end());
	assert(rhsr.begin() != rhsr.end());

	aligner.align(lhsr, rhsr, lhs_len, rhs_len);
	ctx.run();

	BOOST_TEST(aligner.alignment_score() == expected_score);
	BOOST_TEST(ctx.lhs_gaps() == expected_lhs);
	BOOST_TEST(ctx.rhs_gaps() == expected_rhs);
}

// Aligner tests
BOOST_AUTO_TEST_CASE(test_aligner_0)
{
	typedef alignment_context_type <std::uint16_t> alignment_context;
	typedef typename alignment_context::bit_vector_type bit_vector;

	bit_vector const lhs(4, 0x0);
	bit_vector rhs(4, 0x0);
	alignment_context ctx;
	run_aligner(ctx, "asdf", "asdf", lhs, rhs, 8, 8, 2, -2, -2, -1);
}

BOOST_AUTO_TEST_CASE(test_aligner_1)
{
	typedef alignment_context_type <std::uint16_t> alignment_context;
	typedef typename alignment_context::bit_vector_type bit_vector;

	bit_vector const lhs(5, 0x0);
	bit_vector rhs(5, 0x0);
	*rhs.word_begin() = 0x4;
	alignment_context ctx;
	run_aligner(ctx, "xaasd", "xasd", lhs, rhs, 5, 8, 2, -2, -2, -1);
}

BOOST_AUTO_TEST_CASE(test_aligner_2_32)
{
	typedef alignment_context_type <std::uint32_t> alignment_context;
	typedef typename alignment_context::bit_vector_type bit_vector;

	bit_vector const lhs(10, 0x0);
	bit_vector rhs(10, 0x0);
	*rhs.word_begin() = 0x84;
	alignment_context ctx;
	run_aligner(ctx, "xaasdxaasd", "xasdxasd", lhs, rhs, 10, 16, 2, -2, -2, -1);
}

BOOST_AUTO_TEST_CASE(test_aligner_2_16)
{
	typedef alignment_context_type <std::uint16_t> alignment_context;
	typedef typename alignment_context::bit_vector_type bit_vector;

	bit_vector const lhs(10, 0x0);
	bit_vector rhs(10, 0x0);
	*rhs.word_begin() = 0x84;
	alignment_context ctx;
	run_aligner(ctx, "xaasdxaasd", "xasdxasd", lhs, rhs, 10, 8, 2, -2, -2, -1);
}

BOOST_AUTO_TEST_CASE(test_aligner_2_8)
{
	typedef alignment_context_type <std::uint16_t> alignment_context;
	typedef typename alignment_context::bit_vector_type bit_vector;

	bit_vector const lhs(10, 0x0);
	bit_vector rhs(10, 0x0);
	*rhs.word_begin() = 0x84;
	alignment_context ctx;
	run_aligner(ctx, "xaasdxaasd", "xasdxasd", lhs, rhs, 10, 4, 2, -2, -2, -1);
}

BOOST_AUTO_TEST_CASE(test_aligner_2_8_graph)
{
	typedef alignment_context_type <std::uint16_t> alignment_context;
	typedef typename alignment_context::bit_vector_type bit_vector;

	bit_vector const lhs(10, 0x0);
	bit_vector rhs(10, 0x0);
	*rhs.word_begin() = 0x84;
	alignment_context ctx;

	auto const lhss("xaasdxaasd");
	auto const rhss("xasdxasd");
	run_aligner(ctx, lhss, rhss, lhs, rhs, 10, 4, 2, -2, -2, -1);

	ta::alignment_graph_builder <char32_t> builder;
	builder.build_graph(std::string(lhss), std::string(rhss), ctx.lhs_gaps(), ctx.rhs_gaps());
}


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( interleave_odd_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<A0>;
      return  bitwise_cast<A0>(f_t( _mm256_unpackhi_pd( bitwise_cast<f_t>(a0)
                                                  , bitwise_cast<f_t>(a1)
                                                  )
                                  )
                              );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( interleave_odd_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<A0>;
      auto b0 = bitwise_cast<f_t>(a0);
      auto b1 = bitwise_cast<f_t>(a1);
      return  bitwise_cast<A0>(f_t( _mm256_unpackhi_ps( _mm256_shuffle_ps(b0, b0, _MM_SHUFFLE(3,1,2,0))
                                                  , _mm256_shuffle_ps(b1, b1, _MM_SHUFFLE(3,1,2,0))
                                                  )
                                  )
                              );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( interleave_odd_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      auto s0 = slice(a0);
      auto s1 = slice(a1);
      return  combine(interleave_odd(s0[0],s1[0]), interleave_odd(s0[1],s1[1]));
    }
  };
} } }

#endif


/*
 * calibrationTable.hpp
 *
 *  Created on: May 14, 2012
 *      Author: wjiang2
 */

#ifndef CALIBRATIONTABLE_HPP_
#define CALIBRATIONTABLE_HPP_

#include <map>
#include <string>
#include <vector>
#include <stdexcept>
#include "spline.hpp"
using namespace std;
#include <boost/config.hpp>
#include <boost/graph/adj_list_serialize.hpp>
#include <boost/foreach.hpp>
#include "GatingSet.pb.h"

struct Spline_Coefs{
	map<string,vector<double> > coefs;
	int method;
	string type;//to be deprecated
};

class calibrationTable{
	friend std::ostream & operator<<(std::ostream &os, const calibrationTable &gh);
private:
	valarray<double> x,y,b,c,d;
	int spline_method;
	string caltype;//TODO:move this to transformation class
	bool flag;
public:
	calibrationTable();
	calibrationTable & operator=(const calibrationTable& source);
	calibrationTable(string _caltype,int _spline_method);
	void interpolate();
	void init(unsigned);
	valarray<double> transforming(valarray<double> &);
	Spline_Coefs getSplineCoefs();
	valarray<double> getX(){return x;};
	valarray<double> getY(){return y;};
	void setY(valarray<double> _y){
			y.resize(_y.size());
			y=_y;
			};
	void setX(valarray<double> _x){
				x.resize(_x.size());
				x=_x;
				};
	valarray<double> getB(){return b;};
	valarray<double> getC(){return c;};
	valarray<double> getD(){return d;};
	void setCaltype(string _caltype){caltype=_caltype;};
	string getCaltype(){return caltype;};
	void setMethod(int _spline_method){spline_method=_spline_method;};
	int getMethod(){return spline_method;};
	void setInterpolated(bool _flag){flag=_flag;};
	bool isInterpolated(){return flag;}
	void convertToPb(pb::calibrationTable & cal_pb);
	calibrationTable(const pb::calibrationTable & cal_pb);
};

#endif /* CALIBRATIONTABLE_HPP_ */


/*自分付近の最下点を算出
 *
 */
#include <ros/ros.h>
#include <string>
#include <vector>
#include <stdio.h>
#include <iostream>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>

#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

#include <local_tool/filters.hpp>

#define MIN(x,y) ((x) < (y) ? (x) : (y))
#define MAX(x,y) ((x) > (y) ? (x) : (y))

int grid_dim_ = 8;
double m_per_cell_ = 0.5;

using namespace std;

const float map_limit = 2.0;

float x_now = 0;
float y_now = 0;
float yaw_now = 0;

sensor_msgs::PointCloud2 transformed_pc;

pcl::PointCloud<pcl::PointXYZ>::Ptr use_map_cloud(new pcl::PointCloud<pcl::PointXYZ>);

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_IN(new pcl::PointCloud<pcl::PointXYZ>);

pcl::PointCloud<pcl::PointXYZ>::Ptr local_map_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr before_cloud (new pcl::PointCloud<pcl::PointXYZ>);

struct Data {
	float min;
	float max;
	int min_d;
	int max_d;
};

void lcl_Callback(const nav_msgs::OdometryConstPtr msg)//現在地把握
{
    x_now = msg->pose.pose.position.x;
    y_now = msg->pose.pose.position.y;
    yaw_now = msg->pose.pose.orientation.z;

}

void pcCallback(const sensor_msgs::PointCloud2ConstPtr msg)//現在地把握
{
	pcl::fromROSMsg(*msg,*before_cloud);
}

void local_map(pcl::PointCloud<pcl::PointXYZ>::Ptr input_point)
{
    //limit_laserを設定
	local_map_cloud->points.clear();

    size_t velodyne_size = input_point->points.size();
	for(size_t i = 0; i < velodyne_size; i++){
		pcl::PointXYZ temp_point;
		temp_point.x = input_point->points[i].x;
		temp_point.y = input_point->points[i].y;
		temp_point.z = input_point->points[i].z;

        //if(d <= map_limit && (-0.5 < temp_point.z && temp_point.z <5.0)){
		if((map_limit * (-1) + x_now <= temp_point.x && temp_point.x  <= map_limit + x_now) && (map_limit *(-1) + y_now <= temp_point.y && temp_point.y <= map_limit + y_now)) {//つくば

			local_map_cloud->points.push_back(temp_point);
		}
	}
}

void get_min(pcl::PointCloud<pcl::PointXYZ>::Ptr input_point,float& z_ans,float& pitch_ans,float& roll_ans){

    float min[grid_dim_][grid_dim_];
    bool init[grid_dim_][grid_dim_];

	fill(min[0],min[grid_dim_],100);
	fill(init[0],init[grid_dim_],0);

	float pitch[grid_dim_] = {};
	float roll[grid_dim_] = {};

	Data pitch_;
	Data roll_;

	int count;
	z_ans =0;
	pitch_ans =0;
	roll_ans =0;

    size_t velodyne_size = input_point->points.size();
    for (size_t i = 0; i < velodyne_size; ++i) {
        pcl::PointXYZ scan;

        scan.x = input_point->points[i].x - x_now;
        scan.y = input_point->points[i].y - y_now;
        scan.z = input_point->points[i].z;

        int x = ((grid_dim_/2)+scan.x /m_per_cell_);
        int y = ((grid_dim_/2)+scan.y /m_per_cell_);

        if (x >= 0 && x < grid_dim_  && y >= 0 && y < grid_dim_) {
			if (!init[x][y]) {
                min[x][y] = scan.z;
                // all_grid++;
                init[x][y] = true;
            }
			else min[x][y] = MIN(min[x][y], scan.z);
        }
    }

	count =0;
	// tkb用
	//minimumの近傍を平均する
	//roll
	for (int i=0;i<grid_dim_;i++){
		bool flag = false;
		roll_.max_d = 0;

		for (int j=0;j<grid_dim_;j++){
			if(init[i][j]){
				z_ans = (z_ans * count + min[i][j])/(count+1);
				count++;

				if(!flag){
					roll_.min = min[i][j];
					roll_.min_d = j;
					flag = true;
				}
				roll_.max = min[i][j];
				roll_.max_d = j;
			}
		}
		if(roll_.max_d) roll[i] = atan2((-1)*(roll_.max - roll_.min),(roll_.max_d-roll_.min_d+1)*m_per_cell_ );
	}

	//pitch
	for (int i=0;i<grid_dim_;i++){
		bool flag = false;
		pitch_.max_d = 0;

		for (int j=0;j<grid_dim_;j++){
			if(init[j][i]){
				if(!flag){
					pitch_.min = min[j][i];
					pitch_.min_d = j;
					flag = true;
				}
				pitch_.max = min[j][i];
				pitch_.max_d = j;
			}
		}
		if(pitch_.max_d) pitch[i] = atan2((-1)*(pitch_.max - pitch_.min),(pitch_.max_d-pitch_.min_d+1)*m_per_cell_);
	}

	count =0;

	for(int i=0;i<grid_dim_;i++){
		// cout<<pitch[i]<<endl;
		if(pitch[i]){
			pitch_ans = (pitch_ans * count + pitch[i])/(count+1);
			count++;
		}
	}

	count =0;

	for(int i=0;i<grid_dim_;i++){
		// cout<<roll[i]<<endl;
		if(roll[i]){
			roll_ans = (roll_ans * count + roll[i])/(count+1);
			count++;
		}
	}
	// cout<<"roll:"<<roll_ans<<"pitch:"<<pitch_ans<<endl;

}

void pc2transform(float pitch_theta,float roll_theta){

	// float yaw_theta = 0;
    //
	Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
	//
	// transform_1 (0,0) = cos (pitch_theta) * cos(yaw_theta);
	// transform_1 (0,1) = (-1) * sin(roll_theta) * (-1) * sin(pitch_theta) * cos(yaw_theta) - cos(roll_theta ) * sin(yaw_theta);
	// transform_1 (0,2) = cos(roll_theta) * (-1) * sin(pitch_theta) * cos(yaw_theta) + (-1) * sin(roll_theta ) * sin(yaw_theta);
	// transform_1 (1,0) = cos(pitch_theta) * sin(yaw_theta);
	// transform_1 (1,1) = (-1) * sin(roll_theta) * (-1) * sin(pitch_theta) * sin(yaw_theta) + cos(roll_theta ) * cos(yaw_theta);
	// transform_1 (1,2) = cos(roll_theta) * (-1) * sin(pitch_theta) * sin(yaw_theta) -(-1) * sin(roll_theta)  * cos(yaw_theta);
	// transform_1 (2,0) = (-1) * -sin (pitch_theta);
	// transform_1 (2,1) = (-1) * sin(roll_theta) * cos(pitch_theta);
	// transform_1 (2,2) = cos(roll_theta) * cos(pitch_theta);
	//
	// pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
	// pcl::transformPointCloud (*before_cloud, *transformed_cloud, transform_1);
	// pcl::toROSMsg(*transformed_cloud,transformed_pc);

	Eigen::Matrix3f rot;
	rot = Eigen::AngleAxisf(roll_theta*(-1), Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(pitch_theta*(-1), Eigen::Vector3f::UnitY());

	Eigen::Translation3f init_translation (0, 0, 0);

	Eigen::Matrix4f transform = (rot * init_translation).matrix ();

	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
	pcl::transformPointCloud (*before_cloud, *transformed_cloud, transform);
	pcl::toROSMsg(*transformed_cloud,transformed_pc);

}

// void tf_broad(double roll_now, double pitch_now, double yaw_now ,double x_now,double y_now,double z_now){
// 	tf::TransformBroadcaster br;
// 	tf::Transform transform; //位置と姿勢を持つ座標系を表すクラス
//
//
// 	transform.setOrigin( tf::Vector3(x_now, y_now, z_now) );
// 	tf::Quaternion q;
// 	q.setRPY(roll_now, pitch_now, yaw_now);
//
// 	transform.setRotation(q);
// 	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map" ,"sample_baselink"));
//
// 	cout<<"a"<<endl;
// }

int main (int argc, char** argv)
{
	ros::init(argc, argv, "pc2transform4map");
  	ros::NodeHandle n;
	ros::Rate roop(10);

	ros::Publisher pub_height  = n.advertise<std_msgs::Float64>("/height", 10);
	ros::Publisher trans_pub  = n.advertise<sensor_msgs::PointCloud2>("/velodyne_obstacles_after", 10);

    ros::Subscriber odom_sub = n.subscribe("/lcl_ekf", 1000, lcl_Callback);//lclから現在地求む
    ros::Subscriber pc_sub = n.subscribe("/velodyne_obstacles", 1000, pcCallback);

	string map_file;
	n.getParam("map_file_ground",map_file);
	map_reader(0.2,map_file,cloud_IN);

	use_map_cloud = cloud_IN;

	std_msgs::Float64 num;
	float z_ans;
	float pitch_ans;
	float roll_ans;

	while(ros::ok()){

		local_map(use_map_cloud);
        get_min(local_map_cloud,z_ans,pitch_ans,roll_ans);

		num.data = z_ans;
		pub_height.publish(num);

		pc2transform(pitch_ans,roll_ans);

		transformed_pc.header.stamp = ros::Time::now();
		transformed_pc.header.frame_id = "/velodyne";

		trans_pub.publish(transformed_pc);

		// tf_broad(pitch_ans,roll_ans,yaw_now,x_now,y_now,z_ans);

		roop.sleep();
		ros::spinOnce();
	}
	return (0);
}


#include <cstdlib>
#include <iostream>
#include <set>
#include <climits>
#include <algorithm>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/filtered_graph.hpp>

#include <vector>

//Adapted from http://stackoverflow.com/questions/14470566/how-to-traverse-graph-in-boost-use-bfs
typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS > Graph;
typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;

/**
Functor to filter vertices in a graph which are not in the given set
 */
class VertexSetFilter
{
  public:
    std::set<Vertex> S;

    VertexSetFilter(std::set<Vertex> inputSet)
    {
	S = inputSet;
    }

  bool operator()(const Vertex& v) const
  {
      return S.find(v) != S.end(); // keep all vertx_descriptors greater than 3
  }
};

template < typename TVertex, typename TGraph >
class MyVisitor : public boost::default_bfs_visitor
{
private:
    int numFound;

public:

    void discover_vertex(TVertex u, const TGraph & g) const
	{
	    std::cout << u << std::endl;
	}
    int getNumFound()
	{
	    return numFound;
	}
};

int restrictedBFS(std::set<Vertex> S, Vertex v, Graph G)
{
    VertexSetFilter myFilter(S);
    typedef boost::filtered_graph<Graph, boost::keep_all, VertexSetFilter > SubGraph;
    SubGraph filteredG(G, boost::keep_all(), myFilter);

    MyVisitor<Vertex, SubGraph> vis;
    //won't compile when this line is uncommented
    boost::breadth_first_search(filteredG, v, boost::visitor(vis));
    return -1;
}

int main()
{
  std::cout << "Hello";
}


#include <mlpack/core.hpp>
#include <NN_basis_function.hpp>
#include <armadillo>
#include <boost/program_options.hpp>
#include <iostream>
#include <robotics.hpp>
#include <HSMM.hpp>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
namespace po = boost::program_options;

field<mat> fromMatToField(const mat& obs) {
    field<mat> ret(obs.n_cols);
    for(int i = 0; i < obs.n_cols; i++)
        ret(i) = obs.col(i);
    return ret;
}

mat join_mats(vector<mat> v) {
    int total_cols = 0;
    int total_rows = v.at(0).n_rows;
    for(auto& m: v)
        total_cols += m.n_cols;
    mat ret(total_rows, total_cols);
    int idx = 0;
    for(auto& m: v) {
        int d = m.n_cols;
        ret.cols(idx, idx + d - 1) = m;
        idx += d;
    }
    assert(idx == total_cols);
    return ret;
}

mat reverse(const mat& m) {
    mat ret(size(m));
    for(int i = 0; i < m.n_cols; i++)
        ret.col(m.n_cols - 1 - i) = m.col(i);
    return ret;
}

pair<vec, mat> getMeanAndCovFromSamples(const vector<vec>& samples) {
    vec mean(size(samples.at(0)), fill::zeros);
    mat cov(mean.n_elem, mean.n_elem, fill::zeros);
    for(const auto& s: samples) {
        mean += s;
        cov += s * s.t();
    }
    mean = mean * (1.0/samples.size());
    cov = cov * (1.0/samples.size());
    cov = cov - mean * mean.t();
    return make_pair(mean, cov);
}

ivec get_residual_time_from_vit(const imat& vit) {
    vector<int> ret;
    ivec dur = vit.col(1);
    for(int i = 0; i < vit.n_rows; i++) {
        int d = dur(i);
        for(int j = d - 1; j >= 0; j--)
            ret.push_back(j);
    }
    assert (sum(dur) == ret.size());
    return conv_to<ivec>::from(ret);
}

int main(int argc, char *argv[]) {
    mlpack::Log::Info.ignoreInput = false;
    po::options_description desc("Outputs a fitted HSMM+ProMP model assuming"
            " all the hidden variables are known. Thus, no EM algorithm.");
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<string>(), "Path to the input obs")
        ("output,o", po::value<string>(), "Path to the json output params")
        ("viterbilabels,l", po::value<string>(), "Path to the input viterbi "
            "files (labels). Note that the format is viterbi (not labels)")
        ("nstates,s", po::value<int>(), "Number of hidden states")
        ("mindur", po::value<int>(), "Minimum duration of a segment")
        ("ndur", po::value<int>(), "Number of different durations supported")
        ("hiddenunits,u", po::value<int>()->default_value(10),
                "Number of hidden units per hidden layer")
        ("nlayers", po::value<int>()->default_value(1),
                "Number of hidden layers")
        ("nfiles,n", po::value<int>(), "Number of input files to process")
        ("leaveoneout", po::value<int>()->default_value(-1),
                "Leaves the training file associated to this number out")
        ("debug", "Flag for activating debug mode in HSMM")
        ("nodur", "Flag to deactivate the learning of durations")
        ("notrans", "Flag to deactivate the learning of transitions")
        ("nopi", "Flag to deactivate the learning of initial pmf")
        ("polybasisfun", po::value<int>()->default_value(0), "Order of the "
                "poly basis functions")
        ("noselftransitions", "Flag to deactive self transitions")
        ("delta", po::value<double>()->default_value(-1), "If this is given "
                "a value, the model switches to segment agnostic and the "
                "samples are generated according to the provided delta")
        ("ms", po::value<string>(), "File name where a "
                "matrix containing the state marginals will be stored.")
        ("mr", po::value<string>(), "File name where a "
                "matrix containing the run length marginals will be stored.")
        ("ml", po::value<string>(), "File name where a matrix"
                " containing the residual time marginals will be stored.")
        ("md", po::value<string>(), "File name where a "
                "matrix containing the duration marginals will be stored.")
        ("savebasisfunparams", po::value<string>(), "File where the NN weights"
                " will be saved after training")
        ("test,t", po::value<string>(), "Path to the test observation file")
        ("vittest", po::value<string>(), "Path to the test vit file (truth)")
        ("offsettest", po::value<int>()->default_value(0), "this many number "
                "of observations are skipped for the test llk computation.")
        ("nobstest", po::value<int>()->default_value(-1), "Number of obs to "
                "feed into the model from the test file. Defaults to all obs"
                "(i.e., -1)");
    vector<string> required_fields = {"input", "viterbilabels",
            "nfiles", "nstates", "mindur", "ndur"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    string input_filename = vm["input"].as<string>();
    string viterbi_filename = vm["viterbilabels"].as<string>();
    int nseq = vm["nfiles"].as<int>();
    int leaveoneout = vm["leaveoneout"].as<int>();
    int nstates = vm["nstates"].as<int>();
    int hidden_units = vm["hiddenunits"].as<int>();
    int nlayers = vm["nlayers"].as<int>();
    double delta = vm["delta"].as<double>();
    vector<mat> obs_for_each_state[nstates];
    vector<mat> times_for_each_state[nstates];
    field<field<mat>> seq_obs(nseq);
    field<Labels> seq_labels(nseq);
    vector<imat> vit_file_for_each_obs;
    for(int i = 0; i < nseq; i++) {
        if (i == leaveoneout)
            continue;
        string iname = input_filename;
        string vname = viterbi_filename;
        if (nseq != 1) {
            iname += string(".") + to_string(i);
            vname += string(".") + to_string(i);
        }
        mat obs;
        obs.load(iname, raw_ascii);
        seq_obs(i) = fromMatToField(obs);
        imat vit;
        vit.load(vname, raw_ascii);
        ivec hs = vit.col(0);
        ivec dur = vit.col(1);
        vit_file_for_each_obs.push_back(vit);
        int idx = 0;
        assert(sum(dur) == obs.n_cols);
        for(int j = 0; j < dur.n_rows; j++) {
            mat segment = obs.cols(idx, idx + dur(j) - 1);
            obs_for_each_state[hs(j)].push_back(segment);

            // NOTE: the training time steps are generated according to delta.
            rowvec times = linspace<rowvec>(0, 1.0, dur(j));
            if (delta > 0)
                times = linspace<rowvec>(0, (dur(j)-1)*delta, dur(j));
            times_for_each_state[hs(j)].push_back(times);
            idx += dur(j);
            seq_labels(i).setLabel(idx - 1, dur(j), hs(j));
        }
    }

    // There should be at least one segment for the hidden state 0.
    int njoints = obs_for_each_state[0].at(0).n_rows;
    vector<shared_ptr<ScalarCombBasis>> basis;
    vector<vec> means;
    vector<nlohmann::json> basis_fun_params;
    for(int i = 0; i < nstates; i++) {
        mat inputs = join_mats(times_for_each_state[i]);
        mat outputs = join_mats(obs_for_each_state[i]);
        assert (outputs.n_rows == njoints);
        assert(outputs.n_cols == inputs.n_cols);

        // Defining the architecture of the NN.
        ivec hidden_units_per_layer = ones<ivec>(nlayers) * hidden_units;
        auto nn = make_shared<ScalarNNBasis>(hidden_units_per_layer, njoints);

        // Training the NN.
        nn->getNeuralNet().Train(inputs, outputs);
        auto serialized = nn->to_stream();
        basis_fun_params.push_back(serialized);

        // Testing the NN building from serialized parameters.
        auto nn_copy = make_shared<ScalarNNBasis>(serialized);

        // Extracting the parameters from the output layer.
        pair<mat,vec> out_params = nn->getOutputLayerParams();
        mat joint_params = join_horiz(out_params.first, out_params.second);
        vec flattened_params = conv_to<vec>::from(vectorise(joint_params, 1));
        means.push_back(flattened_params);
        cout << "Weights " << endl << out_params.first << endl;
        cout << "Bias " << endl << out_params.second << endl;

        // Adding polynomial terms to the NN basis.
        auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
        auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis(
                    {nn, poly}));
        // Predicting.
        vec test_input = linspace<vec>(0,1,100);
        vector<mat> test_output;
        for(int j = 0; j < test_input.n_elem; j++) {
            vec output = nn->eval(test_input(j));
            vec output2 = nn_copy->eval(test_input(j));
            assert(approx_equal(output, output2, "reldiff", 1e-8));
            test_output.push_back(output);
        }
        mat mat_test_output;
        mat mat_test_input = linspace<rowvec>(0, 1, 100);
        nn->getNeuralNet().Predict(mat_test_input, mat_test_output);
        //mat_test_output.save("prediction.txt." + to_string(i) , raw_ascii);
        mat prueba = out_params.first * join_mats(test_output);
        for(int i = 0; i < prueba.n_cols; i++)
            prueba.col(i) += out_params.second;
        //prueba.save("fprediction.txt." + to_string(i) , raw_ascii);
        assert(approx_equal(mat_test_output, prueba, "reldiff", 1e-8));
        basis.push_back(comb);
    }
    int n_basis_functions = basis.at(0)->dim();

    if (vm.count("savebasisfunparams")) {
        nlohmann::json basisfunparams = basis_fun_params;
        std::ofstream basis_fun_file(vm["savebasisfunparams"].as<string>());
        basis_fun_file << std::setw(4) << basisfunparams << std::endl;
        basis_fun_file.close();
    }

    // Computing the the mean and covariance of Omega using least squares.
    // NOTE: this is only tested for one dimensional observations.
    vector<pair<vec, mat>> mle_gaussians;
    for(int i = 0; i < nstates; i++) {
        const auto& nn = basis.at(i);
        const vector<mat>& t_seq = times_for_each_state[i];
        const vector<mat>& obs_seq = obs_for_each_state[i];
        vector<vec> ls_omegas;
        for(int j = 0; j < t_seq.size(); j++) {
            vector<mat> Phi_vec, Y_vec;
            for(const auto& time_step: t_seq.at(j))
                Phi_vec.push_back(nn->eval(time_step));
            mat Phi = join_mats(Phi_vec).t();
            mat Y = vectorise(obs_seq.at(j));
            vec ls_omega = solve(Phi, Y);
            ls_omegas.push_back(ls_omega);
            assert(ls_omega.n_rows == n_basis_functions * njoints);
        }
        pair<vec, mat> curr = getMeanAndCovFromSamples(ls_omegas);
        mle_gaussians.push_back(curr);
        //cout << "Mean: " << curr.first << endl;
        //cout << "Covariance: " << curr.second << endl;

        vec test_input = linspace<vec>(0,1,100);
        mat mean(njoints, test_input.n_elem);
        mat std_dev(njoints, test_input.n_elem);
        for(int j = 0; j < test_input.n_elem; j++) {
            vec m = nn->eval(test_input(j));
            vec output_mean = m.t() * curr.first;
            mat output_cov = m.t() * curr.second * m;
            mean.col(j) = output_mean;
            std_dev.col(j) = sqrt(output_cov.diag());
        }

        //Debug
        //mean.save("mean_prediction.txt." + to_string(i), raw_ascii);
        //std_dev.save("stddev_prediction.txt." + to_string(i), raw_ascii);
    }

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w = mle_gaussians.at(i).first;
        assert(mu_w.n_elem == n_basis_functions * njoints);

        // TODO: the covariance is fixed for now due to overfitting.
        mat Sigma_w = 0.05*eye(mu_w.n_elem, mu_w.n_elem);
        mat Sigma_y = 0.1*eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP nn_promp(basis.at(i), promp, njoints);
        promps.push_back(nn_promp);
    }
    int min_duration = vm["mindur"].as<int>();
    int ndurations = vm["ndur"].as<int>();
    mat transition(nstates, nstates);
    transition.fill(1.0 / nstates );
    if (vm.count("noselftransitions")) {
        transition.fill(1.0 / (nstates - 1));
        transition.diag().zeros();
    }
    vec pi(nstates);
    pi.fill(1.0/nstates);
    mat durations(nstates, ndurations);
    durations.fill(1.0 / ndurations);

    // Computing the required statistics from the provided labels if required.
    // NOTE: pmfs with zero mass are allowed in the transtion and duration
    // matrices.
    if (!vm.count("nopi")) {
        pi.fill(0.0);
        for(const auto& vit: vit_file_for_each_obs)
            pi(vit(0, 0)) += 1.0;
        pi = pi * (1.0/accu(pi));
    }
    cout << "Initial pmf" << endl << pi << endl;
    if (!vm.count("notrans")) {
        transition.fill(0.0);
        for(const auto& vit: vit_file_for_each_obs)
            for(int i = 0; i < vit.col(0).n_elem - 1; i++)
                transition(vit(i, 0), vit(i + 1, 0)) += 1.0;
        for(int i = 0; i < nstates; i++)
            if (accu(transition.row(i)) > 1e-7)
                transition.row(i) *= (1.0/accu(transition.row(i)));
    }
    cout << "Transition matrix" << endl << transition << endl;
    if (!vm.count("nodur")) {
        durations.fill(0.0);
        for(const auto& vit: vit_file_for_each_obs)
            for(int i = 0; i < vit.n_rows; i++) {
                durations(vit(i,0), vit(i,1) - min_duration) += 1.0;
                //durations(1 - vit(i,0), vit(i,1) - min_duration) += 1.0;
            }
        for(int i = 0; i < nstates; i++)
            if (accu(durations.row(i)) > 1e-7)
                durations.row(i) *= (1.0/accu(durations.row(i)));
    }
    cout << "Duration matrix" << endl << durations << endl;

    // Creating the ProMP emission.
    shared_ptr<ProMPsEmission> ptr_emission(new ProMPsEmission(promps));

    if (delta > 0.0)
        ptr_emission->setDelta(delta);

    OnlineHSMM promp_hsmm(std::static_pointer_cast<
            AbstractEmissionOnlineSetting>(ptr_emission),
            transition, pi, durations, min_duration);

    if (!vm.count("test")) {
        cout << "No test file was provided" << endl;
        return 0;
    }
    mat obs_for_cond;
    obs_for_cond.load(vm["test"].as<string>(), raw_ascii);
    field<field<mat>> field_obs = {fromMatToField(obs_for_cond)};

    // Saving the model in a json file.
    if (vm.count("output")) {
        string output_filename = vm["output"].as<string>();
        std::ofstream initial_params(output_filename);
        nlohmann::json initial_model = promp_hsmm.to_stream();
        initial_params << std::setw(4) << initial_model << std::endl;
        initial_params.close();
    }

    // Number of test observations to feed in.
    int n_obs_test = vm["nobstest"].as<int>();
    n_obs_test = (n_obs_test < 0) ?  obs_for_cond.n_cols: n_obs_test;
    int offset_test = vm["offsettest"].as<int>();

    // Load ground truth values of residual time is available.
    double residual_times_ll = 0.0;
    int residual_times_ll_neval = 0;
    ivec ground_truth_residual_t;
    if (vm.count("vittest")) {
        imat test_vit;
        test_vit.load(vm["vittest"].as<string>(), raw_ascii);
        ground_truth_residual_t = get_residual_time_from_vit(test_vit);
    }

    // Testing the online inference algorithm.
    mat state_marginals_over_time(nstates, n_obs_test);
    mat runlength_marginals_over_time(min_duration + ndurations,
            n_obs_test);
    mat residualtime_marginals_over_time(min_duration + ndurations,
            n_obs_test);
    mat duration_marginals_over_time(ndurations, n_obs_test);
    vec onlineloglikelihoods(n_obs_test);
    for(int c = 0; c < n_obs_test; c++) {
        cout << "Processing obs idx: " << c << endl;
        promp_hsmm.addNewObservation(obs_for_cond.col(c));
        onlineloglikelihoods(c) = promp_hsmm.getLastOneStepAheadLoglikelihood();
        if (vm.count("ms")) {
            vec s_marginal = promp_hsmm.getStateMarginal();
            state_marginals_over_time.col(c) = s_marginal;
        }
        if (vm.count("mr")) {
            vec r_marginal = promp_hsmm.getRunlengthMarginal();
            runlength_marginals_over_time.col(c) = r_marginal;
        }
        if (vm.count("ml")) {
            vec l_marginal = promp_hsmm.getResidualTimeMarginal();
            residualtime_marginals_over_time.col(c) = l_marginal;
        }
        if (vm.count("md")) {
            vec d_marginal = promp_hsmm.getDurationMarginal();
            duration_marginals_over_time.col(c) = d_marginal;
        }

        // Computing the loglikelihoods of the residual times if needed.
        if (c >= offset_test && !ground_truth_residual_t.empty()) {
            vec r_marginal = promp_hsmm.getResidualTimeMarginal();
            double cll = r_marginal.at(ground_truth_residual_t.at(c));
            residual_times_ll += log(cll);
            residual_times_ll_neval++;
        }
    }

    // Saving the marginals if required.
    if (vm.count("ms"))
        state_marginals_over_time.save(vm["ms"].as<string>(), raw_ascii);
    if (vm.count("mr"))
        runlength_marginals_over_time.save(vm["mr"].as<string>(), raw_ascii);
    if (vm.count("ml"))
        residualtime_marginals_over_time.save(vm["ml"].as<string>(), raw_ascii);
    if (vm.count("md"))
        duration_marginals_over_time.save(vm["md"].as<string>(), raw_ascii);

    // Evaluation the likelihood of the test observation sequence.
    // The batch one uses the HSMM algos whereas the online takes into account
    // that the last segment might not be complete.
    // double batch_ll = promp_hsmm.loglikelihood(field_obs);
    // cout << "batch loglikelihood: " << batch_ll << endl;
    double online_ll = accu(onlineloglikelihoods);
    cout << "online loglikelihood: " << online_ll << endl;

    // Outputting the llk of the residual times if needed.
    if (!ground_truth_residual_t.empty()) {
        cout << "evaluated terms: " << residual_times_ll_neval << endl;
        cout << "residual times loglikelihood: " << residual_times_ll << endl;
    }
    return 0;
}


#ifndef __GLM_MODEL_H__
#define __GLM_MODEL_H__

#include <armadillo>

#include <glm/models/links/glm_link.hpp>

/**
 * General base class for GLM models that will be solved with
 * the Iteratively reweighted least squares algorithm.
 */
class glm_model
{
public:
    /**
     * Constructor.
     *
     * @param model_name Name of the model.
     * @param link_name Name of the link function.
     */
    glm_model(const std::string &model_name, const std::string &link_name)
        : m_model( model_name )
    {
        m_link = make_link( link_name );
    }

    /**
     * Constructor.
     *
     * @param model_name Name of the model.
     * @param link A link function, this class will be responsible for freeing it.
     */
    glm_model(const std::string &model_name, glm_link *link)
        : m_model( model_name )
    {
        m_link = link;
    }

    /**
     * Destructor.
     */
    virtual ~glm_model()
    {
        delete m_link;
    };

    /**
     * Returns the name of the model.
     *
     * @return the name of the model.
     */
    std::string get_name() const
    {
        return m_model;
    }

    /**
     * Returns the link function used in this model.
     *
     * @return the link function used in this model.
     */
    const glm_link &get_link() const
    {
        return *m_link;
    }

    /**
    * Checks that mu is in the allowed range. Should return false
    * if this is not the case, the irls algorithm will terminate
    * in this case.
    *
    * @param mu The mean value parameter.
    *
    * @return True if mu is in range, false otherwise.
    */
    virtual bool valid_mu(const arma::vec &mu) const = 0;

    /**
     * The variance of each observation given the mean value
     * parameter.
     *
     * @param mu The mean value parameter.
     *
     * @return The variance of each observation.
     */
    virtual arma::vec var(const arma::vec &mu) const = 0;

    /**
     * Estimate the dispersion of the model.
     *
     * @param mu The mean value parameter.
     * @param y The observations.
     * @param missing Indicates missing samples by 1 and not missing by 0.
     * @param k The number of estimated betas.
     *
     * @return The estimated dispersion.
     */
    virtual double dispersion(const arma::vec &mu, const arma::vec &y, const arma::uvec &missing, float k) const = 0;

    /**
     * Compute the log likelihood for the parameters.
     *
     * @param mu The mean value parameter.
     * @param y The observations.
     * @param missing Indicates missing samples by 1 and not missing by 0.
     * @param dispersion Estimated dispersion (only used for final likelihood).
     *
     * @return The log likelihood.
     */
    virtual double likelihood(const arma::vec &mu, const arma::vec &y, const arma::uvec &missing, float dispersion = 1.0) const = 0;

    /**
     * Returns true if the phenotype is binary.
     *
     * @return true if the phenotype is binary.
     */
    virtual bool is_binary() const = 0;

private:
    /**
     * Name of the model.
     */
    std::string m_model;

    /**
     * The link function.
     */
    glm_link *m_link;
};

#endif /* End of __GLM_MODEL_H__ */


#ifndef __GLM_METHOD_H__
#define __GLM_METHOD_H__

#include <string>
#include <vector>

#include <armadillo>

#include <glm/glm.hpp>
#include <besiq/method/method.hpp>
#include <besiq/stats/log_scale.hpp>
#include <besiq/model_matrix.hpp>

/**
 * This class is responsible for initializing and repeatedly
 * executing the glm regression on pairs of snps.
 */
class glm_method
: public method_type
{
public:
    /**
     * Constructor.
     *
     * @param data Additional data required by all methods.
     */
    glm_method(method_data_ptr data, const glm_model &model, model_matrix &model_matrix);

    /**
     * @see method_type::init.
     */
    virtual std::vector<std::string> init();

    /**
     * @see method_type::run.
     */
    virtual double run(const snp_row &row1, const snp_row &row2, float *output);

private:
    /**
     * The glm model used, in this case a binomial model with logit link.
     */
    const glm_model &m_model;

    /**
     * The model matrix that is used.
     */
    model_matrix &m_model_matrix;
};

#endif /* End of __GLM_METHOD_H__ */


#include "ntl/map.hpp"
#include "parser.hpp"
#include "firewall.hpp"

#include <functional>

#include <boost/operators.hpp>

void firewall::step(axi_data_stream& in, axi_data_stream& data_out, bool_stream& classify_out, gateway_registers& g)
{
#pragma HLS inline
#pragma HLS stream variable=invalid_udp depth=16
    gateway.gateway(g, [&](int addr, gateway_data& data) -> int { return rpc(addr, data); });

    dup_data.step(in);
    parse.step(dup_data._streams[0]);
    dup_metadata.step(parse.out);

    ntl::map(dup_metadata._streams[0], hash.lookups, [](const metadata& m) {
        return hash_tag{m.ip_source, m.ip_dest, m.udp_source, m.udp_dest};
    });

    hash.hash_table();

    ntl::map(dup_metadata._streams[1], invalid_udp, [](const metadata& m) {
        return !m.valid_udp();
    });

    ntl::map(hash.results, result_with_default, [](const ntl::maybe<std::tuple<uint32_t, hash_t::mapped_type> >& val) {
        return val.valid() ? std::get<1>(val.value()) : ap_uint<1>(0);
    });

    merge_hash_results.step(std::logical_or<ap_uint<1> >(),
                            result_with_default, invalid_udp);

    link(merge_hash_results.out, classify_out);
    link(dup_data._streams[1], data_out);
}

int firewall::rpc(int addr, gateway_data& data)
{
    switch (addr) {
    case FIREWALL_ADD:
        return hash.gateway_add_entry(std::make_tuple(data.tag, data.result), &data.status);
    case FIREWALL_DEL:
        return hash.gateway_delete_entry(data.tag, &data.status);
    default:
        return ntl::GW_FAIL;
    }
}

void firewall_top(ntl::stream<ntl::raw_axi_data, ntl::axi_stream_tag>& in,
                  ntl::stream<ntl::raw_axi_data, ntl::axi_stream_tag>& data_out,
                  bool_stream& classify_out, gateway_registers& g)
{
#pragma HLS dataflow
#pragma HLS interface ap_ctrl_none port=return
#pragma HLS interface axis port=in._stream
#pragma HLS interface axis port=data_out._stream

    GATEWAY_OFFSET(g, 0x100, 0x118, 0xfc)
    static firewall f;
    static axi_data_stream in_fifo, out_fifo;
    ntl::link(in, in_fifo);
#pragma HLS stream variable=out_fifo depth=16
    ntl::link(out_fifo, data_out);
    f.step(in_fifo, out_fifo, classify_out, g);
}