#include <tradelayer/operators_algo_clearing.h>

#include <tradelayer/externfns.h>
#include <tradelayer/log.h>
#include <tradelayer/mdex.h>
#include <tradelayer/parse_string.h>
#include <tradelayer/tradelayer.h>
#include <tradelayer/tradelayer_matrices.h>
#include <tradelayer/uint256_extensions.h>

#include <amount.h>
#include <iostream>
#include <limits>
#include <unordered_set>

#include <boost/multiprecision/cpp_dec_float.hpp>

int n_cols = 10;
int N = 2;
int M = 4;

typedef boost::multiprecision::cpp_dec_float_100 dec_float;
MatrixTLS *pt_ndatabase;

// initialization using new class constructors
VectorTLS *pt_open_incr_long  = new VectorTLS(N, "OpenLongPosition", "LongPosIncreased");
VectorTLS *pt_open_incr_short  =  new VectorTLS(N, "OpenShortPosition", "ShortPosIncreased");
VectorTLS *pt_netted_npartly_long  = new VectorTLS(N,"LongPosNetted", "LongPosNettedPartly");
VectorTLS *pt_netted_npartly_short  =  new VectorTLS(N, "ShortPosNetted", "ShortPosNettedPartly");
VectorTLS *pt_open_incr_anypos =  new VectorTLS(M,"OpenLongPosition", "LongPosIncreased", "OpenShortPosition", "ShortPosIncreased");
VectorTLS *pt_netted_npartly_anypos = new VectorTLS(M, "LongPosNetted", "LongPosNettedPartly", "ShortPosNetted", "ShortPosNettedPartly");
VectorTLS *pt_changepos_status = new VectorTLS(2, "OpenLongPosByShortPosNetted", "OpenShortPosByLongPosNetted");

// NOTE: better approach: use the template class to initialize!
// VectorTLS *pt_expiration_dates = new VectorTLS(12*NYears);
// VectorTLS &expiration_dates = *pt_expiration_dates;
// for ( int i = 0; i < NYears; i++ )
//   {
//     expiration_dates[i+11*i]    = "ALL F" + std::to_string(i+initYear);
//     expiration_dates[i+1+11*i]  = "ALL G" + std::to_string(i+initYear);
//     expiration_dates[i+2+11*i]  = "ALL H" + std::to_string(i+initYear);
//     expiration_dates[i+3+11*i]  = "ALL J" + std::to_string(i+initYear);
//     expiration_dates[i+4+11*i]  = "ALL K" + std::to_string(i+initYear);
//     expiration_dates[i+5+11*i]  = "ALL M" + std::to_string(i+initYear);
//     expiration_dates[i+6+11*i]  = "ALL N" + std::to_string(i+initYear);
//     expiration_dates[i+7+11*i]  = "ALL Q" + std::to_string(i+initYear);
//     expiration_dates[i+8+11*i]  = "ALL U" + std::to_string(i+initYear);
//     expiration_dates[i+9+11*i]  = "ALL V" + std::to_string(i+initYear);
//     expiration_dates[i+10+11*i] = "ALL X" + std::to_string(i+initYear);
//     expiration_dates[i+11+11*i] = "ALL Z" + std::to_string(i+initYear);
//   }
/**********************************************************/
/** Functions for Settlement Algorithm */
struct status_amounts *get_status_amounts_open_incr(VectorTLS &v, int q)
{
  struct status_amounts *pt_status = new status_amounts;
  VectorTLS status_z(2);
  VectorTLS status_q = status_open_incr(status_z, q);

  if ( finding(v[1], status_q) )
    {
      pt_status->addrs_trk  = v[0];
      pt_status->status_trk = v[1];
      pt_status->lives_trk  = stol(v[2].c_str());
      pt_status->addrs_src  = v[3];
      pt_status->status_src = v[4];
      pt_status->lives_src  = stol(v[5].c_str());
      pt_status->nlives_trk = stol(v[8].c_str());
      pt_status->nlives_src = stol(v[9].c_str());
    }
  else
    {
      pt_status->addrs_src  = v[0];
      pt_status->status_src = v[1];
      pt_status->lives_src  = stol(v[2].c_str());
      pt_status->addrs_trk  = v[3];
      pt_status->status_trk = v[4];
      pt_status->lives_trk  = stol(v[5].c_str());
      pt_status->nlives_src = stol(v[8].c_str());
      pt_status->nlives_trk = stol(v[9].c_str());
    }
  pt_status->amount_trd    = stol(v[6].c_str());
  pt_status->matched_price = stod(v[7].c_str());
  return pt_status;
}

struct status_amounts *get_status_amounts_byaddrs(VectorTLS &v, std::string addrs)
{
  struct status_amounts *pt_status = new status_amounts;

  if ( addrs == v[0] )
    {
      pt_status->addrs_trk  = v[0];
      pt_status->status_trk = v[1];
      pt_status->lives_trk  = stol(v[2].c_str());
      pt_status->addrs_src  = v[3];
      pt_status->status_src = v[4];
      pt_status->lives_src  = stol(v[5].c_str());
      pt_status->nlives_trk = stol(v[8].c_str());
      pt_status->nlives_src = stol(v[9].c_str());
    }
  else
    {
      pt_status->addrs_src  = v[0];
      pt_status->status_src = v[1];
      pt_status->lives_src  = stol(v[2].c_str());
      pt_status->addrs_trk  = v[3];
      pt_status->status_trk = v[4];
      pt_status->lives_trk  = stol(v[5].c_str());
      pt_status->nlives_src = stol(v[8].c_str());
      pt_status->nlives_trk = stol(v[9].c_str());
    }
  pt_status->amount_trd    = stol(v[6].c_str());
  pt_status->matched_price = stod(v[7].c_str());

  return pt_status;
}

struct status_amounts_edge *get_status_byedge(std::map<std::string, std::string> &edge)
{
  struct status_amounts_edge *pt_status = new status_amounts_edge;

  pt_status->addrs_src   = edge["addrs_src"];
  pt_status->addrs_trk   = edge["addrs_trk"];
  pt_status->status_src  = edge["status_src"];
  pt_status->status_trk  = edge["status_trk"];
  pt_status->entry_price = stod(edge["entry_price"]);
  pt_status->exit_price  = stod(edge["exit_price"]);
  pt_status->lives_src   = stol(edge["lives_src"]);
  pt_status->lives_trk   = stol(edge["lives_trk"]);
  pt_status->amount_trd  = stol(edge["amount_trd"]);
  pt_status->edge_row    = stol(edge["edge_row"]);
  pt_status->path_number = stol(edge["path_number"]);
  pt_status->ghost_edge  = stol(edge["ghost_edge"]);

  return pt_status;
}

struct EdgeInfo *GetEdgeInfo(std::map<std::string, std::string> &edge)
{
  struct EdgeInfo *pt_status = new EdgeInfo;

  pt_status->addrs_src       = edge["addrs_src"];
  pt_status->addrs_trk       = edge["addrs_trk"];
  pt_status->status_src      = edge["status_src"];
  pt_status->status_trk      = edge["status_trk"];
  pt_status->entry_price     = stod(edge["entry_price"]);
  pt_status->exit_price      = stod(edge["exit_price"]);
  pt_status->lives_src       = stol(edge["lives_src"]);
  pt_status->lives_trk       = stol(edge["lives_trk"]);
  pt_status->amount_trd_src  = stol(edge["amount_trd_src"]);
  pt_status->amount_trd_trk  = stol(edge["amount_trd_trk"]);
  pt_status->edge_row        = stol(edge["edge_row"]);
  pt_status->path_number     = stol(edge["path_number"]);
  pt_status->ghost_edge      = stol(edge["ghost_edge"]);

  return pt_status;
}

struct status_lives_edge *get_status_bylivesedge(std::map<std::string, std::string> &edge)
{
  struct status_lives_edge *pt_status = new status_lives_edge;

  pt_status->addrs       = edge["addrs"];
  pt_status->status      = edge["status"];
  pt_status->lives       = stol(edge["lives"]);
  pt_status->entry_price = stod(edge["entry_price"]);
  pt_status->edge_row    = stol(edge["edge_row"]);
  pt_status->path_number = stol(edge["path_number"]);

  return pt_status;
}

VectorTLS status_open_incr(VectorTLS &status_q, int q)
{
  VectorTLS &open_incr_long = *pt_open_incr_long;
  VectorTLS &open_incr_short = *pt_open_incr_short;

  if (q == 0)
  {
      status_q[0] = open_incr_long[0];
      status_q[1] = open_incr_long[1];
  } else {
      status_q[0] = open_incr_short[0];
      status_q[1] = open_incr_short[1];
  }

  return status_q;
}

VectorTLS status_netted_npartly(VectorTLS &status_q, int q)
{
    VectorTLS &netted_npartly_long  = *pt_netted_npartly_long;
    VectorTLS &netted_npartly_short = *pt_netted_npartly_short;

    if (q == 0)
    {
        status_q[0] = netted_npartly_long[0];
        status_q[1] = netted_npartly_long[1];
    } else {
        status_q[0] = netted_npartly_short[0];
        status_q[1] = netted_npartly_short[1];
    }

    return status_q;
}

void adding_newtwocols_trdamount(MatrixTLS &M_file, MatrixTLS &database)
{
    int N =  size(database, 1);
    for (int i = 0; i < size(database, 0); ++i)
    {
        for (int j = 0; j < N; ++j) M_file[i][j] = database[i][j];
        M_file[i][N] = database[i][6].c_str();
        M_file[i][N+1] = database[i][6].c_str();
    }
}

void settlement_algorithm_fifo(MatrixTLS &M_file, int64_t interest, int64_t twap_price)
{
  VectorTLS &open_incr_long  = *pt_open_incr_long;
  VectorTLS &open_incr_short = *pt_open_incr_short;

  std::vector<std::vector<std::map<std::string, std::string>>> path_main;
  std::vector<std::vector<std::map<std::string, std::string>>>::iterator it_path_main;
  std::vector<std::map<std::string, std::string>>::iterator it_path_maini;
  std::map<std::string, std::string> edge_source;

  int path_number = 0;
  VectorTLS vdata(n_cols);

  for (int i = 0; i < n_rows; ++i)
  {
    for (int j = 0; j < n_cols; ++j) vdata[j] = M_file[i][j];

    struct status_amounts *pt_vdata_long  = get_status_amounts_open_incr(vdata, 0);
    struct status_amounts *pt_vdata_short = get_status_amounts_open_incr(vdata, 1);

 		std::vector<std::map<std::string, std::string>> path_maini; /** ith Path element for Main Graph **/
 		int idx_b = 0; /** Branch Index: idx_b = 1 (Branch) idx_b = 0 (Source Edge) **/
    if (finding(pt_vdata_long->status_trk, open_incr_long) || finding(pt_vdata_short->status_trk, open_incr_short))
		{
      path_number += 1;

      if(msc_debug_settlement_algorithm_fifo) PrintToLog("\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
	  	/** First Edge for this new Path/Branch **/

	  	building_edge(edge_source,
                    pt_vdata_long->addrs_src, pt_vdata_long->addrs_trk,
                    pt_vdata_long->status_src, pt_vdata_long->status_trk,
                    pt_vdata_long->matched_price, pt_vdata_long->matched_price,
                    pt_vdata_long->lives_src, pt_vdata_long->lives_trk,
                    i, path_number, pt_vdata_long->amount_trd, 0);

	  	if (finding(pt_vdata_long->status_trk, open_incr_long) && finding(pt_vdata_short->status_trk, open_incr_short)){
          if(msc_debug_settlement_algorithm_fifo) PrintToLog("New Edge Source: Row #%d\n\n", i);
	  	} else {
        idx_b = 1;
	  		if(msc_debug_settlement_algorithm_fifo) PrintToLog("Posible Branch Source: Row #%d\n\n", i);
	  	}

	  	path_maini.push_back(edge_source);
	  	if(msc_debug_settlement_algorithm_fifo) PrintingEdge(edge_source);

	  	if (finding(pt_vdata_long->status_trk, open_incr_long))
	  	{
        int counting_netted_long = 0;
	    	long int amount_trd_sum_long = 0;

        if(msc_debug_settlement_algorithm_fifo) {
	    	    PrintToLog("\n*************************************************");
	    	    PrintToLog("\nTracking Long Position for: %s", pt_vdata_long->addrs_trk);
        }

	    	clearing_operator_fifo(vdata, M_file, i, pt_vdata_long, 0, counting_netted_long, amount_trd_sum_long, path_maini,
					     			           path_number, pt_vdata_long->nlives_trk, idx_b);
	  	}

	  	if(finding(pt_vdata_short->status_trk, open_incr_short))
	  	{
        int counting_netted_short = 0;
	    	long int amount_trd_sum_short = 0;
        if(msc_debug_settlement_algorithm_fifo) {
	    	    PrintToLog("\n*************************************************");
	    	    PrintToLog("\nTracking Short Position for: %s", pt_vdata_short->addrs_trk);
        }

	    	clearing_operator_fifo(vdata, M_file, i, pt_vdata_short, 1, counting_netted_short, amount_trd_sum_short, path_maini,
	    							           path_number, pt_vdata_short->nlives_trk, idx_b);
	  	}

      if (path_maini.size() != 0)
	  	{
        if(msc_debug_settlement_algorithm_fifo) PrintToLog("\nPath Element #%d of the Main Graph:\n\n", path_number);
  			for (it_path_maini = path_maini.begin(); it_path_maini != path_maini.end(); ++it_path_maini)
          if(msc_debug_settlement_algorithm_fifo) PrintingEdge(*it_path_maini);
      }

      path_main.push_back(path_maini);
		}
  }

  if(msc_debug_settlement_algorithm_fifo) PrintToLog("\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
  if (path_main.size() != 0)
  {
    if(msc_debug_settlement_algorithm_fifo) PrintToLog("\n\nMain Graph for Settlement:\n\n");
  	for (it_path_main = path_main.begin(); it_path_main != path_main.end(); ++it_path_main)
      PrintingGraph(*it_path_main);
  }

  if(msc_debug_settlement_algorithm_fifo) {
      PrintToLog("\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
      PrintToLog("Second Part: Lives Vectors and Ghost Nodes\n\n");

      PrintToLog("\n*************************************************");
      PrintToLog("\nAddresses with Lives Non Zero\n\n");
  }

  std::vector<std::string> AddrsV = AddressesList(path_main);
  std::vector<std::string> AddrsLivesNonZero = LivesNonZero(path_main, AddrsV);
  for (std::vector<std::string>::iterator it = AddrsLivesNonZero.begin(); it != AddrsLivesNonZero.end(); ++it)
    if(msc_debug_settlement_algorithm_fifo) PrintToLog("%s\n", *it);

  if(msc_debug_settlement_algorithm_fifo) {
      PrintToLog("\n*************************************************");
      PrintToLog("\nComputing Lives contracts in the Main Graph\n");
  }

  std::vector<std::map<std::string, std::string>> LivesLongs;
  std::vector<std::map<std::string, std::string>> LivesShorts;
  std::map<std::string, std::string> LivesLongsEle;
  std::map<std::string, std::string> LivesShortsEle;

  std::vector<std::vector<std::map<std::string, std::string>>>::reverse_iterator rit_path_main;
  std::vector<std::map<std::string, std::string>>::reverse_iterator rit_path_maini;

  for (std::vector<std::string>::iterator it_addrs = AddrsV.begin(); it_addrs != AddrsV.end(); ++it_addrs)
    {
      std::string &AddrsLives = *it_addrs;

      std::string Status  = "None";
      long int NLives 	  = 0;
      long int EdgeRow 	  = 0;
      long int PathNumber = 0;
      double EntryPrice   = 0;
      int IdPosition 	  = 0;
      int NEvents         = 0;
      bool Loop = true;

      for (rit_path_main = path_main.rbegin(); rit_path_main != path_main.rend() && Loop; ++rit_path_main)
	{
	  for (rit_path_maini = (*rit_path_main).rbegin(); rit_path_maini != (*rit_path_main).rend() && Loop; ++rit_path_maini)
	    {
	      std::map<std::string, std::string> &GraphEdge = *rit_path_maini;
	      struct EdgeInfo *PtStatusByEdge = GetEdgeInfo(GraphEdge);

	      if (PtStatusByEdge->addrs_src == AddrsLives)
		{
		  NEvents += 1;
		  IdPosition = finding_string("Long", PtStatusByEdge->status_src) ? 0 : 1;
		  Status = PtStatusByEdge->status_src;
		  NLives = PtStatusByEdge->lives_src;
		  EntryPrice = PtStatusByEdge->entry_price;
		  EdgeRow = PtStatusByEdge->edge_row;
		  PathNumber = PtStatusByEdge->path_number;

		  if (finding_string("Open", PtStatusByEdge->status_src))
		    {
		      // PrintToLog("\n* Open. Last Action for: %s = %s; Edge Row = %d\n", AddrsLives, Status, EdgeRow);
		      PushBackLives(IdPosition, AddrsLives, Status, NLives, EntryPrice, EdgeRow, PathNumber, LivesLongsEle,
				    LivesLongs, LivesShortsEle, LivesShorts);
		      Loop = false;
		    }
		  else if (finding_string("Increased", PtStatusByEdge->status_src))
		    {
		      // PrintToLog("\n* Increased. Last Action for: %s = %s; Edge Row = %d\n", AddrsLives, Status, EdgeRow);
		      IncreasedLastPos(path_main, EdgeRow, AddrsLives, LivesLongsEle, LivesLongs, LivesShortsEle, LivesShorts);
		      Loop =  false;
		    }
		  else if (finding_string("NettedPartly", PtStatusByEdge->status_src))
		    {
		      // PrintToLog("\n* NettedPartly. Last Action for: %s = %s; Edge Row = %d\n", AddrsLives, Status, EdgeRow);
		      PushBackLives(IdPosition, AddrsLives, Status, NLives, EntryPrice, EdgeRow, PathNumber, LivesLongsEle,
				    LivesLongs, LivesShortsEle, LivesShorts);
		      Loop =  false;
		    }
		}
	      else if (PtStatusByEdge->addrs_trk == AddrsLives)
		{
		  NEvents += 1;
		  IdPosition = finding_string("Long", PtStatusByEdge->status_trk) ? 0 : 1;
		  Status = PtStatusByEdge->status_trk;
		  NLives = PtStatusByEdge->lives_trk;
		  EntryPrice = PtStatusByEdge->entry_price;
		  EdgeRow = PtStatusByEdge->edge_row;
		  PathNumber = PtStatusByEdge->path_number;

		  if (finding_string("Open", PtStatusByEdge->status_trk))
		    {
		      // PrintToLog("\n* Open. Last Action for: %s = %s; Edge Row = %d\n", AddrsLives, Status, EdgeRow);
		      PushBackLives(IdPosition, AddrsLives, Status, NLives, EntryPrice, EdgeRow, PathNumber, LivesLongsEle,
				    LivesLongs, LivesShortsEle, LivesShorts);
		      Loop = false;
		    }
		  else if (finding_string("Increased", PtStatusByEdge->status_trk))
		    {
		      IncreasedLastPos(path_main, EdgeRow, AddrsLives, LivesLongsEle, LivesLongs, LivesShortsEle, LivesShorts);
		      // PrintToLog("\n* Increased. Last Action for: %s = %s; Edge Row = %d\n", AddrsLives, Status, EdgeRow);
		      Loop =  false;
		    }
		  else if (finding_string("NettedPartly", PtStatusByEdge->status_trk))
		    {
		      // PrintToLog("\n* NettedPartly. Last Action for: %s = %s; Edge Row = %d\n", AddrsLives, Status, EdgeRow);
		      PushBackLives(IdPosition, AddrsLives, Status, NLives, EntryPrice, EdgeRow, PathNumber, LivesLongsEle,
				    LivesLongs, LivesShortsEle, LivesShorts);
		      Loop =  false;
		    }
		}
	      else
		continue;
	    }
	  }
  }

  if(msc_debug_settlement_algorithm_fifo) PrintToLog("\n*************************************************\n");
  double exit_price_desired = 0;
  long int sum_oflives 	    = 0;
  double PNL_total 	    = 0;
  double gamma_p 	    = 0;
  double gamma_q 	    = 0;
  double sum_gamma_p 	    = 0;
  double sum_gamma_q 	    = 0;

  for (it_path_main = path_main.begin(); it_path_main != path_main.end(); ++it_path_main)
  {
    computing_settlement_exitprice(*it_path_main, sum_oflives, PNL_total, gamma_p, gamma_q, interest, twap_price);
    sum_gamma_p += gamma_p;
    sum_gamma_q += gamma_q;
  }

  exit_price_desired = sum_gamma_p/sum_gamma_q;
  if(msc_debug_settlement_algorithm_fifo) PrintToLog("\nexit_price_desired = %d\n", exit_price_desired);
  counting_lives_longshorts(LivesLongs, LivesShorts);

  if(msc_debug_settlement_algorithm_fifo) {
      PrintToLog("\n*************************************************");
      PrintToLog("\nGhost Edges Vector:\n\n");
  }

  std::vector<std::map<std::string, std::string>> GhostEdgesArray;
  GhostEdgesComputing(LivesLongs, LivesShorts, exit_price_desired, GhostEdgesArray);

  std::vector<std::map<std::string, std::string>>::iterator it_ghost;
  for (it_ghost = GhostEdgesArray.begin(); it_ghost != GhostEdgesArray.end(); ++it_ghost) PrintingGhostEdge(*it_ghost);

  if(msc_debug_settlement_algorithm_fifo) PrintToLog("\n*************************************************");

  std::unordered_set<std::string> addrs_set;
  std::vector<std::string> addrsv;
  int k = 0;
  long int nonzero_lives = 0;
  double PNL_totalit = 0;

  /** Total PNL for Main Path **/
  for (it_path_main = path_main.begin(); it_path_main != path_main.end(); ++it_path_main)
  {
    k += 1;
    if(msc_debug_settlement_algorithm_fifo) {
        PrintToLog("\nPath #%d: PNL computation for the main path\n", k);
        printing_path_maini(*it_path_main);
    }

    nonzero_lives = checkpath_livesnonzero(*it_path_main);
    listof_addresses_bypath(*it_path_main, addrsv);

    if(msc_debug_settlement_algorithm_fifo) PrintToLog("\nComputing PNL in this Path\n");
    calculate_pnltrk_bypath(*it_path_main, PNL_totalit, addrs_set, addrsv, interest, twap_price);
    PNL_total += PNL_totalit;
    if(msc_debug_settlement_algorithm_fifo) PrintToLog("\nPNL_total_main sum: %f\n", PNL_total);
    addrs_set.clear();
  }

  /** Total PNL for Ghost Nodes **/
  if(msc_debug_settlement_algorithm_fifo) {
      PrintToLog("\n*************************************************");
      PrintToLog("\nChecking PNL for Ghost Nodes:\n");
  }

  calculate_pnl_forghost(GhostEdgesArray, PNL_total);

  if(msc_debug_settlement_algorithm_fifo) {
      PrintToLog("\nPNL_total_main = %f", PNL_total);
      PrintToLog("\n\n");
  }

  if(msc_debug_settlement_algorithm_fifo) PrintToLog("\nnonzero_lives : %d\n",nonzero_lives);

}

void PushBackLives(int IdPosition, std::string AddrsLives, std::string Status, long int NLives, double EntryPrice, long int EdgeRow, long int PathNumber, std::map<std::string, std::string> LivesLongsEle, std::vector<std::map<std::string, std::string>> &LivesLongs, std::map<std::string, std::string> LivesShortsEle, std::vector<std::map<std::string, std::string>> &LivesShorts)
{
  if (IdPosition == 0)
    {
      building_lives_edges(LivesLongsEle, AddrsLives, Status, NLives, EntryPrice, EdgeRow, PathNumber);
      LivesLongs.push_back(LivesLongsEle);
    }
  else
    {
      building_lives_edges(LivesShortsEle, AddrsLives, Status, NLives, EntryPrice, EdgeRow, PathNumber);
      LivesShorts.push_back(LivesShortsEle);
    }
}

void IncreasedLastPos(std::vector<std::vector<std::map<std::string, std::string>>> path_main, long int LastEdgeRow, std::string AddrsLives, std::map<std::string, std::string> LivesLongsEle, std::vector<std::map<std::string, std::string>> &LivesLongs, std::map<std::string, std::string> LivesShortsEle, std::vector<std::map<std::string, std::string>> &LivesShorts)
{
  std::vector<std::vector<std::map<std::string, std::string>>>::reverse_iterator rit_path_main;
  std::vector<std::map<std::string, std::string>>::reverse_iterator rit_path_maini;

  std::string Status  = "None";
  long int EdgeRow    = 0;
  long int PathNumber = 0;
  long int AmountTrd  = 0;
  double EntryPrice   = 0;
  int IdPosition      = 0;
  bool Loop = true;

  for (rit_path_main = path_main.rbegin(); rit_path_main != path_main.rend() && Loop; ++rit_path_main)
    {
      for (rit_path_maini = (*rit_path_main).rbegin(); rit_path_maini != (*rit_path_main).rend() && Loop; ++rit_path_maini)
	{
	  std::map<std::string, std::string> &GraphEdge = *rit_path_maini;
	  struct EdgeInfo *PtStatusByEdge = GetEdgeInfo(GraphEdge);

	  EdgeRow = PtStatusByEdge->edge_row;

	  /*******************************************************/
	  /** Look back for Increase Events Until Open is Found **/
	  if (EdgeRow <= LastEdgeRow)
	    {
	      if (PtStatusByEdge->addrs_src == AddrsLives)
		{
		  // PrintToLog("\nEdgeRow = %d\n", EdgeRow);
		  // PrintingEdge(GraphEdge);

		  IdPosition = finding_string("Long", PtStatusByEdge->status_src) ? 0 : 1;
		  Status     = PtStatusByEdge->status_src;
		  AmountTrd  = PtStatusByEdge->amount_trd_src; /** AmountTrd Represent Lives Increased Events **/
		  EntryPrice = PtStatusByEdge->entry_price;
		  EdgeRow    = PtStatusByEdge->edge_row;
		  PathNumber = PtStatusByEdge->path_number;
		  Loop = finding_string("Open", PtStatusByEdge->status_src) ? false : true; /** When Open appear Finish**/

		  PushBackLives(IdPosition, AddrsLives, Status, AmountTrd, EntryPrice, EdgeRow, PathNumber, LivesLongsEle,
				LivesLongs, LivesShortsEle, LivesShorts);
		}
	      else if (PtStatusByEdge->addrs_trk == AddrsLives)
		{
		  // PrintToLog("\nEdgeRow = %d\n", EdgeRow);
		  // PrintingEdge(GraphEdge);

		  IdPosition = finding_string("Long", PtStatusByEdge->status_trk) ? 0 : 1;
		  Status     = PtStatusByEdge->status_trk;
		  AmountTrd  = PtStatusByEdge->amount_trd_trk; /** AmountTrd Represent Lives Increased Events **/
		  EntryPrice = PtStatusByEdge->entry_price;
		  EdgeRow    = PtStatusByEdge->edge_row;
		  PathNumber = PtStatusByEdge->path_number;
		  Loop = finding_string("Open", PtStatusByEdge->status_trk) ? false : true; /** When Open appear Finish**/

		  PushBackLives(IdPosition, AddrsLives, Status, AmountTrd, EntryPrice, EdgeRow, PathNumber, LivesLongsEle,
				LivesLongs, LivesShortsEle, LivesShorts);
		}
	    }
	}
    }
}

void clearing_operator_fifo(VectorTLS &vdata, MatrixTLS &M_file, int index_init, struct status_amounts *pt_pos, int idx_long_short, int &counting_netted, long int amount_trd_sum, std::vector<std::map<std::string, std::string>> &path_main, int path_number, long int opened_contracts, int idx_b)
{
  VectorTLS status_z(2);
  std::string addrs_opening = pt_pos->addrs_trk;
  long int d_amounts = 0;
  std::map<std::string, std::string> path_first;

  if(msc_debug_clearing_operator_fifo) {
      PrintToLog("\n---------------------------------------------------\n");
      PrintToLog("LAUNCHING FIFO CLEARING\n");
      PrintToLog("...\n");
  }

  for (int i = index_init + 1; i < n_rows; ++i)
  {
    VectorTLS jrow_database(n_cols);
    sub_row(jrow_database, M_file, i);

    if ( finding(addrs_opening, jrow_database) )
    {
	  	struct status_amounts *pt_status_addrs_trk = get_status_amounts_byaddrs(jrow_database, addrs_opening);
	  	VectorTLS status_netted = status_netted_npartly(status_z, idx_long_short);

	  	if ( finding(pt_status_addrs_trk->status_trk, status_netted) && pt_status_addrs_trk->nlives_trk != 0 )
	    {
        counting_netted +=1;
	      amount_trd_sum += pt_status_addrs_trk->nlives_trk;

        if(msc_debug_clearing_operator_fifo) {
            PrintToLog("\n\nNetted Event in the Row #%d\t Address Tracked: %s\n", i, addrs_opening.c_str());
	          printing_vector(jrow_database);
	          PrintToLog("\n\nopened_contracts = %d, nlives_trk = %d, amount_trd_sum = %d\n", opened_contracts, pt_status_addrs_trk->nlives_trk, amount_trd_sum);
        }

	      d_amounts = opened_contracts - amount_trd_sum;

        if(msc_debug_clearing_operator_fifo) PrintToLog("\n\nReview of d_amounts before cases:\t%ld", d_amounts);

	      if ( d_amounts > 0 )
				{
          if(msc_debug_clearing_operator_fifo) PrintToLog("\n\nd_amounts = %ld > 0", d_amounts);
		  		updating_lasttwocols_fromdatabase(addrs_opening, M_file, i, 0);

          if(msc_debug_clearing_operator_fifo) {
		  		    PrintToLog("\n\nCheck last two columns Updated: Row #%d \n %s \t %s", i, M_file[i][8], M_file[i][9]);
		  		    PrintToLog("\nOpened contrats: %ld > Sum amounts traded: %ld\n", opened_contracts, amount_trd_sum);
          }

		  		building_edge(path_first,
                        pt_status_addrs_trk->addrs_src, pt_status_addrs_trk->addrs_trk,
					              pt_status_addrs_trk->status_src, pt_status_addrs_trk->status_trk,
                        pt_pos->matched_price, pt_status_addrs_trk->matched_price,
                        pt_status_addrs_trk->lives_src, d_amounts,
                        i, path_number, pt_status_addrs_trk->nlives_trk, 0);

          path_main.push_back(path_first);

          if(msc_debug_clearing_operator_fifo) {
		  		    PrintToLog("\nEdge:\n");
		  		    PrintingEdge(path_first);
          }
		  		continue;
				}

	      if ( d_amounts < 0)
				{
		  		if(msc_debug_clearing_operator_fifo) PrintToLog("\n\nd_amounts = %ld < 0", d_amounts);
		  		updating_lasttwocols_fromdatabase(addrs_opening, M_file, i, labs(d_amounts));

          if(msc_debug_clearing_operator_fifo) {
		  		      PrintToLog("\n\nCheck last two columns Updated: Row #%d \n %s \t %s", i, M_file[i][8], M_file[i][9]);
		  		      PrintToLog("\nOpened contrats: %ld < Sum amounts traded: %ld\n", opened_contracts, amount_trd_sum);
          }

		  		building_edge(path_first,
                        pt_status_addrs_trk->addrs_src, pt_status_addrs_trk->addrs_trk,
                        pt_status_addrs_trk->status_src, pt_status_addrs_trk->status_trk,
                        pt_pos->matched_price, pt_status_addrs_trk->matched_price,
                        pt_status_addrs_trk->lives_src, 0,
                        i, path_number, pt_status_addrs_trk->nlives_trk-labs(d_amounts), 0);
		  		path_main.push_back(path_first);

          if(msc_debug_clearing_operator_fifo) {
		  		    PrintToLog("\nEdge:\n");
		  		    PrintingEdge(path_first);
          }

		  		break;
				}

	      if ( d_amounts == 0)
				{
		  		if(msc_debug_clearing_operator_fifo) PrintToLog("\n\nd_amounts = %ld = 0", d_amounts);
		  		updating_lasttwocols_fromdatabase(addrs_opening, M_file, i, 0);

          if(msc_debug_clearing_operator_fifo) {
		  		    PrintToLog("\n\nCheck last two columns Updated: Row #%d \n %s \t %s", i, M_file[i][8], M_file[i][9]);
		  		    PrintToLog("\nOpened contrats: %ld = Sum amounts traded: %ld\n", opened_contracts, amount_trd_sum);
          }

		  		building_edge(path_first,
                        pt_status_addrs_trk->addrs_src, pt_status_addrs_trk->addrs_trk,
					              pt_status_addrs_trk->status_src, pt_status_addrs_trk->status_trk,
                        pt_pos->matched_price, pt_status_addrs_trk->matched_price,
                        pt_status_addrs_trk->lives_src, 0,
                        i, path_number, pt_status_addrs_trk->nlives_trk, 0);
		  		path_main.push_back(path_first);

          if(msc_debug_clearing_operator_fifo) {
              PrintToLog("\nEdge:\n");
		  		    PrintingEdge(path_first);
          }

		  		break;
				}
	   	}
    }
  }

  if (idx_b == 1) path_main.erase(path_main.begin());;

  if(msc_debug_clearing_operator_fifo) {
      PrintToLog("\nFINISHING FIFO SEARCH");
      PrintToLog("\n---------------------------------------------------\n");
  }

}

void updating_lasttwocols_fromdatabase(std::string addrs, MatrixTLS &M_file, int i, long int live_updated)
{
  if ( addrs == M_file[i][0] )
    M_file[i][8] = std::to_string(live_updated);
  else
    M_file[i][9] = std::to_string(live_updated);
}

void building_edge(std::map<std::string, std::string> &path_first, std::string addrs_src, std::string addrs_trk, std::string status_src, std::string status_trk, double entry_price, double exit_price, long int lives, int index_row, int path_number, long int amount_path, int ghost_edge)
{
  path_first["addrs_src"]   = addrs_src;
  path_first["addrs_trk"]   = addrs_trk;
  path_first["status_src"]  = status_src;
  path_first["status_trk"]  = status_trk;
  path_first["entry_price"] = std::to_string(entry_price);
  path_first["exit_price"]  = std::to_string(exit_price);
  path_first["lives_src"]   = std::to_string(lives);
  path_first["lives_trk"]   = std::to_string(lives);
  path_first["amount_trd"]  = std::to_string(amount_path);
  path_first["edge_row"]    = std::to_string(index_row);
  path_first["path_number"] = std::to_string(path_number);
  path_first["ghost_edge"]  = std::to_string(ghost_edge);
}

void building_edge(std::map<std::string, std::string> &path_first, std::string addrs_src, std::string addrs_trk, std::string status_src, std::string status_trk, double entry_price, double exit_price, long int lives_src, long int lives_trk, int index_row, int path_number, long int amount_path, int ghost_edge)
{
  path_first["addrs_src"]      = addrs_src;
  path_first["addrs_trk"]      = addrs_trk;
  path_first["status_src"]     = status_src;
  path_first["status_trk"]     = status_trk;
  path_first["entry_price"]    = std::to_string(entry_price);
  path_first["exit_price"]     = std::to_string(exit_price);
  path_first["lives_src"]      = std::to_string(lives_src);
  path_first["lives_trk"]      = std::to_string(lives_trk);
  path_first["amount_trd_src"] = std::to_string(amount_path);
  path_first["amount_trd_trk"] = std::to_string(amount_path);
  path_first["edge_row"]       = std::to_string(index_row);
  path_first["path_number"]    = std::to_string(path_number);
  path_first["ghost_edge"]     = std::to_string(ghost_edge);
}

void BuildingGhostEdges(std::map<std::string, std::string> &path_first, std::string addrs_src, std::string addrs_trk, std::string status_src, std::string status_trk, string entry_price_src, string entry_price_trk, double exit_price, long int amount_path)
{
  path_first["addrs_src"]       = addrs_src;
  path_first["addrs_trk"]       = addrs_trk;
  path_first["status_src"]      = status_src;
  path_first["status_trk"]      = status_trk;
  path_first["entry_price_src"] = entry_price_src;
  path_first["entry_price_trk"] = entry_price_trk;
  path_first["exit_price"]      = std::to_string(exit_price);
  path_first["amount_trd"]      = std::to_string(amount_path);
}

void building_lives_edges(std::map<std::string, std::string> &path_first, std::string addrs, std::string status, long int lives, double entry_price, struct status_amounts_edge *pt_status_byedge)
{
  path_first["addrs"] 	    = addrs;
  path_first["status"] 	    = status;
  path_first["lives"] 	    = std::to_string(lives);
  path_first["entry_price"] = std::to_string(entry_price);
  path_first["edge_row"]    = std::to_string(pt_status_byedge->edge_row);
  path_first["path_number"] = std::to_string(pt_status_byedge->path_number);
}

void building_lives_edges(std::map<std::string, std::string> &path_first, std::string addrs, std::string status, long int lives, double entry_price, long int edge_row, long int path_number)
{
  path_first["addrs"] 	    = addrs;
  path_first["status"] 	    = status;
  path_first["lives"]  	    = std::to_string(lives);
  path_first["entry_price"] = std::to_string(entry_price);
  path_first["edge_row"]    = std::to_string(edge_row);
  path_first["path_number"] = std::to_string(path_number);
}

void printing_edges(std::map<std::string, std::string> &path_first)
{
  PrintToLog("{ addrs_src : %s , status_src : %s, lives_src : %d, addrs_trk : %s , status_trk : %s, lives_trk : %d, entry_price : %d, exit_price : %d, amount_trd : %d, edge_row : %d, path_number : %d, ghost_edge : %d }\n", path_first["addrs_src"], path_first["status_src"], path_first["lives_src"], path_first["addrs_trk"], path_first["status_trk"], path_first["lives_trk"], path_first["entry_price"], path_first["exit_price"], path_first["amount_trd"], path_first["edge_row"], path_first["path_number"], path_first["ghost_edge"]);
}

void PrintingEdge(std::map<std::string, std::string> &path_first)
{
  PrintToLog("{ addrs_src : %s , status_src : %s, lives_src : %d, amount_trd_src : %d, addrs_trk : %s , status_trk : %s, lives_trk : %d, amount_trd_trk : %d, entry_price : %d, exit_price : %d, edge_row : %d, path_number : %d, ghost_edge : %d }\n", path_first["addrs_src"], path_first["status_src"], path_first["lives_src"], path_first["amount_trd_src"], path_first["addrs_trk"], path_first["status_trk"], path_first["lives_trk"], path_first["amount_trd_trk"], path_first["entry_price"], path_first["exit_price"], path_first["edge_row"], path_first["path_number"], path_first["ghost_edge"]);
}

void PrintingGhostEdge(std::map<std::string, std::string> &path_first)
{
  PrintToLog("{ addrs_src : %s , status_src : %s, entry_price_src : %d, addrs_trk : %s , status_trk : %s, entry_price_trk : %d, exit_price : %d, amount_trd : %d}\n", path_first["addrs_src"], path_first["status_src"], path_first["entry_price_src"], path_first["addrs_trk"], path_first["status_trk"], path_first["entry_price_trk"], path_first["exit_price"], path_first["amount_trd"]);
}

void printing_edges_lives(std::map<std::string, std::string> &path_first)
{
  PrintToLog("{ addrs : %s , status : %s, lives : %d, entry_price : %d, edge_row : %d, path_number : %d }\n", path_first["addrs"], path_first["status"], path_first["lives"], path_first["entry_price"], path_first["edge_row"], path_first["path_number"]);
}

bool find_open_incr_anypos(std::string &s, VectorTLS *v)
{
  VectorTLS open_incr_anypos(4);
  VectorTLS &u = *v;
  for (int i = 0; i < length(open_incr_anypos); i++) open_incr_anypos[i] = u[i];
  return finding(s, open_incr_anypos);
}

bool find_netted_npartly_anypos(std::string &s, VectorTLS *v)
{
  VectorTLS netted_npartly_anypos(4);
  VectorTLS &u = *v;
  for (int i = 0; i < length(netted_npartly_anypos); i++) netted_npartly_anypos[i] = u[i];
  return finding(s, netted_npartly_anypos);
}

bool find_open_incr_long(std::string &s, VectorTLS *v)
{
  VectorTLS open_incr_long(2);
  VectorTLS &u = *v;
  for (int i = 0; i < length(open_incr_long); i++) open_incr_long[i] = u[i];
  return finding(s, open_incr_long);
}

void looking_netted_events(std::string &addrs_obj, std::vector<std::map<std::string, std::string>> &it_path_main, int q, long int amount_opened, int index_src_trk, std::string status_opening)
{
  long int sum_amount_trd = 0;
  int netted_counter = 0;
  unsigned index_netted = 0;

  for (unsigned i = q; i < it_path_main.size(); ++i)
    {
      struct status_amounts_edge *pt_status_byedge = get_status_byedge(it_path_main[i]);

      if ( sum_amount_trd > amount_opened ) break;

      if ( pt_status_byedge->addrs_trk == addrs_obj )
      	{
	  if ( finding_string("Long", status_opening) && finding_string("Long", pt_status_byedge->status_trk) )
	    {
	      index_netted = i;
	      netted_counter += 1;
	      sum_amount_trd += pt_status_byedge->amount_trd;
	    }
	  if ( finding_string("Short", status_opening) && finding_string("Short", pt_status_byedge->status_trk) )
	    {
	      index_netted = i;
	      netted_counter += 1;
	      sum_amount_trd += pt_status_byedge->amount_trd;
	    }
	}
    }

  bool check = sum_amount_trd <= amount_opened;
  if ( netted_counter != 0 )
    it_path_main[index_netted]["lives_trk"] = check ? std::to_string(amount_opened - sum_amount_trd) : std::to_string(0);
  else
    {
      PrintToLog("\nindex q = %d\n", q);
      if ( index_src_trk == 0 )
	it_path_main[q-1]["lives_src"] = std::to_string(amount_opened);
      else if ( index_src_trk == 1 )
	it_path_main[q-1]["lives_trk"] = std::to_string(amount_opened);
    }
}

void printing_path_maini(std::vector<std::map<std::string, std::string>> &it_path_maini)
{
  for (std::vector<std::map<std::string, std::string>>::iterator it = it_path_maini.begin(); it != it_path_maini.end(); ++it)
    printing_edges(*it);
}

void PrintingGraph(std::vector<std::map<std::string, std::string>> &it_path_maini)
{
  for (std::vector<std::map<std::string, std::string>>::iterator it = it_path_maini.begin(); it != it_path_maini.end(); ++it)
    PrintingEdge(*it);
}

void checking_zeronetted_bypath(std::vector<std::map<std::string, std::string>> path_maini)
{
  long int contracts_closed = 0;
  long int contracts_opened = 0;

  VectorTLS &open_incr_anypos = *pt_open_incr_anypos;

  if (finding(path_maini[0]["status_src"], open_incr_anypos) && finding(path_maini[0]["status_trk"], open_incr_anypos))
    contracts_opened = 2*std::stol(path_maini[0]["amount_trd"]);
  else
    contracts_opened = std::stol(path_maini[0]["amount_trd"]);

  long int contracts_lives = std::stol(path_maini[0]["lives_src"])+std::stol(path_maini[0]["lives_trk"]);

  int idx_q = 0;
  // Counting closed contracts. Netted eventes are followed by addrs_trk!!
  for (std::vector<std::map<std::string, std::string>>::iterator it = path_maini.begin()+1; it != path_maini.end(); ++it)
    {
      idx_q += 1;
      struct status_amounts_edge *pt_status_byedge = get_status_byedge(*it);
      if (path_maini[0]["addrs_trk"] == pt_status_byedge->addrs_trk)
	{
	  if (find_netted_npartly_anypos(pt_status_byedge->status_trk, pt_netted_npartly_anypos))
	    {
	      path_maini[idx_q]["addrs_src"]   = std::string();
	      path_maini[idx_q]["addrs_trk"]   = std::string();
	      path_maini[idx_q]["status_src"]  = std::string();
	      path_maini[idx_q]["status_trk"]  = std::string();
	      path_maini[idx_q]["entry_price"] = std::to_string(0);
	      path_maini[idx_q]["exit_price"]  = std::to_string(0);
	      path_maini[idx_q]["lives_src"]   = std::to_string(0);
	      path_maini[idx_q]["lives_trk"]   = std::to_string(0);
	      path_maini[idx_q]["amount_trd"]  = std::to_string(0);
	      path_maini[idx_q]["edge_row"]    = std::to_string(0);
	      path_maini[idx_q]["path_number"] = std::to_string(0);
	      path_maini[idx_q]["ghost_edge"]  = std::to_string(0);
	      contracts_closed += pt_status_byedge->amount_trd;
	    }
	}
    }

  for (std::vector<std::map<std::string, std::string>>::iterator it = path_maini.begin()+1; it != path_maini.end(); ++it)
    {
      struct status_amounts_edge *pt_status_byedge = get_status_byedge(*it);
      if (path_maini[0]["addrs_src"] == pt_status_byedge->addrs_trk)
      	{
      	  if (find_netted_npartly_anypos(pt_status_byedge->status_trk, pt_netted_npartly_anypos))
	    contracts_closed += pt_status_byedge->amount_trd;
      	}
    }

  PrintToLog("\ncontracts_opened = %d, contracts_closed = %d, contracts_lives = %d\n",
  	     contracts_opened, contracts_closed, contracts_lives);
  if ( (contracts_opened - contracts_closed)-contracts_lives == 0 )
    PrintToLog("\nChecking Zero Netted by Path:\n(contracts_opened - contracts_closed)-contracts_lives = %d\n",
  	       (contracts_opened - contracts_closed)-contracts_lives);
  else
    PrintToLog("¡¡Warning!! There is no zero netted event in the path");
}

void computing_livesvectors_forlongshort(std::vector<std::map<std::string, std::string>> &it_path_main, std::vector<std::map<std::string, std::string>> &lives_longs, std::vector<std::map<std::string, std::string>> &lives_shorts)
{
	std::map<std::string, std::string> path_ele;

  	for (std::vector<std::map<std::string, std::string>>::iterator it = it_path_main.begin(); it != it_path_main.end(); ++it)
    {
    	struct status_amounts_edge *pt_status_byedge = get_status_byedge(*it);
      	if ( pt_status_byedge->lives_src != 0 )
		{
	  		building_lives_edges(path_ele, pt_status_byedge->addrs_src, pt_status_byedge->status_src, pt_status_byedge->lives_src, pt_status_byedge->exit_price, pt_status_byedge);

	  		if ( finding_string("Long", pt_status_byedge->status_src) )
	    		lives_longs.push_back(path_ele);
	  		else if ( finding_string("Short", pt_status_byedge->status_src) )
	    		lives_shorts.push_back(path_ele);
		}
      	if ( pt_status_byedge->lives_trk != 0 )
		{
	  		building_lives_edges(path_ele, pt_status_byedge->addrs_trk, pt_status_byedge->status_trk, pt_status_byedge->lives_trk, pt_status_byedge->entry_price, pt_status_byedge);

	  		if ( finding_string("Long", pt_status_byedge->status_trk) )
	    		lives_longs.push_back(path_ele);
	  		else if ( finding_string("Short", pt_status_byedge->status_trk) )
	    		lives_shorts.push_back(path_ele);
		}
    }
}

void counting_lives_longshorts(std::vector<std::map<std::string, std::string>> &lives_longs, std::vector<std::map<std::string, std::string>> &lives_shorts)
{
  long int nlives_longs = 0;
  long int nlives_shorts = 0;

  if(msc_debug_counting_lives_longshorts) PrintToLog("\nList of Long Lives:\n");
  for (std::vector<std::map<std::string, std::string>>::iterator it = lives_longs.begin(); it != lives_longs.end(); ++it)
    {
      if(msc_debug_counting_lives_longshorts) printing_edges_lives(*it);
      nlives_longs += stol((*it)["lives"]);
    }
  if(msc_debug_counting_lives_longshorts) PrintToLog("\nList of Short Lives:\n");
  for (std::vector<std::map<std::string, std::string>>::iterator it = lives_shorts.begin(); it != lives_shorts.end(); ++it)
    {
      if(msc_debug_counting_lives_longshorts) printing_edges_lives(*it);
      nlives_shorts += stol((*it)["lives"]);
    }
  if(msc_debug_counting_lives_longshorts) PrintToLog("\n|nlives_longs| : %d, \n|nlives_shorts| : %d\n", nlives_longs, nlives_shorts);
  if (nlives_longs != nlives_shorts) PrintToLog("\n\nWarning!! Lives Longs sould be equal to Lives Shorts\n\n");
}

void computing_livesvector_global(std::vector<std::map<std::string, std::string>> lives_longs, std::vector<std::map<std::string, std::string>> lives_shorts, std::vector<std::map<std::string, std::string>> &lives_longs_vg, std::vector<std::map<std::string, std::string>> &lives_shorts_vg)
{
  getting_globallives_long_short(lives_longs, lives_longs_vg);
  getting_globallives_long_short(lives_shorts, lives_shorts_vg);
}

void printing_lives_vector(std::vector<std::map<std::string, std::string>> lives)
{
  for (std::vector<std::map<std::string, std::string>>::iterator it = lives.begin(); it != lives.end(); ++it)
    {
      printing_edges_lives(*it);
    }
}

void getting_globallives_long_short(std::vector<std::map<std::string, std::string>> lives, std::vector<std::map<std::string, std::string>> &lives_vg)
{
  for (std::vector<std::map<std::string, std::string>>::iterator it = lives.begin(); it != lives.end(); ++it)
    lives_vg.push_back(*it);
}

bool find_address_lives_vector(std::vector<std::map<std::string, std::string>> lives_v, std::string address)
{
  for (std::vector<std::map<std::string, std::string>>::iterator it = lives_v.begin(); it != lives_v.end(); ++it)
    {
      struct status_lives_edge *pt_status_bylivesedge = get_status_bylivesedge(*it);
      if (pt_status_bylivesedge->addrs == address)
	{
	  return true;
	}
    }
  return false;
}

int find_posaddress_lives_vector(std::vector<std::map<std::string, std::string>> lives_v, std::string address)
{
  int idx_q = 0;
  for (std::vector<std::map<std::string, std::string>>::iterator it = lives_v.begin(); it != lives_v.end(); ++it)
    {
      idx_q += 1;
      struct status_lives_edge *pt_status_bylivesedge = get_status_bylivesedge(*it);
      if (pt_status_bylivesedge->addrs == address)
	{
	  return idx_q-1;
	}
    }
  return idx_q-1;
}

void computing_settlement_exitprice(std::vector<std::map<std::string, std::string>> &it_path_main, long int &sum_oflives, double &PNL_total, double &gamma_p, double &gamma_q, int64_t interest, int64_t twap_price)
{
  long int sum_oflivesh = 0;
  std::unordered_set<std::string> addrs_set;
  std::vector<std::string> addrsv;

  for (std::vector<std::map<std::string, std::string>>::iterator it = it_path_main.begin(); it != it_path_main.end(); ++it)
    {
      std::map<std::string, std::string> &it_ele = *it;
      sum_oflivesh += stol(it_ele["lives_src"]) + stol(it_ele["lives_trk"]);
    }
  sum_oflives = sum_oflivesh;

  if (sum_oflives == 0) PrintToLog("\nThis path does not have lives contracts!!\n");
  else
    {
      listof_addresses_bypath(it_path_main, addrsv);
      calculate_pnltrk_bypath(it_path_main, PNL_total, addrs_set, addrsv, interest, twap_price);
      getting_gammapq_bypath(it_path_main, PNL_total, gamma_p, gamma_q, addrs_set);
      addrs_set.clear();
    }
}

void calculate_pnltrk_bypath(std::vector<std::map<std::string, std::string>> &path_main, double &PNL_total, std::unordered_set<std::string> &addrs_set, std::vector<std::string> addrsv, int64_t interest, int64_t twap_price)
{
  std::vector<std::map<std::string, std::string>>::iterator it_path;
  std::string addrsit;
  double PNL_trk;
  double sumPNL_trk = 0;
  MatrixTLS &ndatabase = *pt_ndatabase;
  VectorTLS jrow_database(n_cols);

  for (std::vector<std::string>::iterator it_addrs = addrsv.begin(); it_addrs != addrsv.end(); ++it_addrs)
    {
      addrsit = *it_addrs;
      for (it_path = path_main.begin()+1; it_path != path_main.end(); ++it_path)
	{
	  std::map<std::string, std::string> &edge_path = *it_path;
	  if ( addrsit == edge_path["addrs_trk"] )
	    {
	      sub_row(jrow_database, ndatabase, stol(edge_path["edge_row"]));
	      struct status_amounts *pt_jrow_database = get_status_amounts_byaddrs(jrow_database, addrsit);
	      addrs_set.insert(addrsit);

	      PNL_trk = PNL_function(stod(edge_path["entry_price"]), stod(edge_path["exit_price"]), stol(edge_path["amount_trd"]),
				     pt_jrow_database);
	      sumPNL_trk += PNL_trk;

	      std::string addrssr = edge_path["addrs_src"];
	      int64_t PNL_trkInt64 = mastercore::DoubleToInt64(PNL_trk);

	      struct FutureContractObject *pfuture = getFutureContractObject("ALL F18");
	      uint32_t NotionalSize = pfuture->fco_notional_size;

	      arith_uint256 volumeALL256_t = mastercore::ConvertTo256(NotionalSize)*mastercore::ConvertTo256(PNL_trkInt64)/COIN;
	      int64_t volumeALL64_t = mastercore::ConvertTo64(volumeALL256_t);

	      struct TokenDataByName *pfuture_ALL = getTokenDataByName("ALL");
	      uint32_t ALLId = pfuture_ALL->data_propertyId;

	      // int64_t entry_priceh =  mastercore::StrToInt64(edge_path["entry_price"], true);
	      // PrintToLog("\nInterest Payment: Entry Price = %s, Twap Price = %s\n",
	      // 		 FormatDivisibleMP(entry_priceh), FormatDivisibleMP(twap_price));

	      if (volumeALL64_t != 0 && volumeALL64_t < getMPbalance(addrssr, ALLId, BALANCE))
		{
		  PrintToLog("\nRebalancing Settlement\n");
		  assert(mastercore::update_tally_map(addrssr, ALLId, -volumeALL64_t, BALANCE)); /** Change in testnet **/
		  assert(mastercore::update_tally_map(addrsit, ALLId,  volumeALL64_t, BALANCE)); /** Change in testnet**/
	       	}
	    }
	}
    }
  PNL_total = sumPNL_trk;
}

void calculate_pnltrk_bypath(std::vector<std::map<std::string, std::string>> path_main, double &PNL_total)
{
  std::vector<std::map<std::string, std::string>>::iterator it_path;
  double sumPNL_trk = 0;
  double PNL_trk;
  MatrixTLS &ndatabase = *pt_ndatabase;
  VectorTLS jrow_database(n_cols);

  //PrintToLog("\nChecking PNL in this Path:\n");
  for (it_path = path_main.begin(); it_path != path_main.end(); ++it_path)
    {
      sub_row(jrow_database, ndatabase, stol((*it_path)["edge_row"]));
      struct status_amounts *pt_jrow_database = get_status_amounts_byaddrs(jrow_database, (*it_path)["addrs_trk"]);
      PNL_trk = PNL_function(stod((*it_path)["entry_price"]), stod((*it_path)["exit_price"]), stol((*it_path)["amount_trd"]), pt_jrow_database);
      //PrintToLog("\nPNL_trk = %f\n", PNL_trk);
      sumPNL_trk += PNL_trk;
    }
  PNL_total = sumPNL_trk;
  //PrintToLog("\nPNL_total_thispath = %f\n", PNL_total);
}

void calculate_pnl_forghost(std::vector<std::map<std::string, std::string>> path_ghost, double &PNL_total)
{
  std::vector<std::map<std::string, std::string>>::iterator it_path;
  double sumPNL_trk = 0;
  double PNL_src;
  double PNL_trk;

  if(msc_debug_calculate_pnl_forghost) PrintToLog("\nChecking PNL for Ghosts\n");
  for (it_path = path_ghost.begin(); it_path != path_ghost.end(); ++it_path)
  {
    PNL_src = PNL_ghosts(stod((*it_path)["entry_price_src"]), stod((*it_path)["exit_price"]), stol((*it_path)["amount_trd"]), (*it_path)["status_src"]);
    PNL_trk = PNL_ghosts(stod((*it_path)["entry_price_trk"]), stod((*it_path)["exit_price"]), stol((*it_path)["amount_trd"]), (*it_path)["status_trk"]);
    sumPNL_trk += PNL_src + PNL_trk;
    if(msc_debug_calculate_pnl_forghost) PrintToLog("\nPNL_src = %f\t PNL_trk = %f\t sumPNL_trk = %f\n", PNL_src, PNL_trk, sumPNL_trk);
  }
}

void listof_addresses_lives(std::vector<std::map<std::string, std::string>> lives, std::vector<std::string> &addrsv)
{
	std::vector<std::string> addrsvh;
	for (std::vector<std::map<std::string, std::string>>::iterator it = lives.begin(); it != lives.end(); ++it)
    {
    	std::map<std::string, std::string> &it_ele = *it;
    	if ( !find_string_strv(it_ele["addrs"], addrsvh) )
    		addrsvh.push_back(it_ele["addrs"]);
    	else
    		continue;
    }
    addrsv = addrsvh;
}

std::vector<std::string> AddressesList(std::vector<std::vector<std::map<std::string, std::string>>> &path_main)
{
	std::vector<std::string> AddrsV;
	std::vector<std::map<std::string, std::string>>::iterator it_in;
	std::vector<std::vector<std::map<std::string, std::string>>>::iterator it_out;

	for (it_out = path_main.begin(); it_out != path_main.end(); ++it_out)
	{
		for (it_in = (*it_out).begin(); it_in != (*it_out).end(); ++it_in)
		{
			std::map<std::string, std::string> &it_ele = *it_in;

			if ( !find_string_strv(it_ele["addrs_src"], AddrsV))
			{
				AddrsV.push_back(it_ele["addrs_src"]);
			}

			if ( !find_string_strv(it_ele["addrs_trk"], AddrsV) )
			{
				AddrsV.push_back(it_ele["addrs_trk"]);
			}
		}
	}

  return AddrsV;
}

std::vector<std::string> LivesNonZero(std::vector<std::vector<std::map<std::string, std::string>>> &path_main, std::vector<std::string> AddrsV)
{
  std::vector<std::string> Result;
  std::vector<std::map<std::string, std::string>>::reverse_iterator it_in;
  std::vector<std::vector<std::map<std::string, std::string>>>::reverse_iterator it_out;

  for (std::vector<std::string>::iterator it_addrs = AddrsV.begin(); it_addrs != AddrsV.end(); ++it_addrs)
    {
      bool Loop = true;
      std::string &Addrsh = *it_addrs;

      for (it_out = path_main.rbegin(); it_out != path_main.rend() && Loop; ++it_out)
	{
	  for (it_in = (*it_out).rbegin(); it_in != (*it_out).rend() && Loop; ++it_in)
	    {
	      std::map<std::string, std::string> &it_ele = *it_in;
	      LivesNotZeroHelper(it_ele["addrs_src"], Addrsh, it_ele["lives_src"], Result, Loop);
	      LivesNotZeroHelper(it_ele["addrs_trk"], Addrsh, it_ele["lives_trk"], Result, Loop);
	    }
	}
    }

  return Result;
}

void LivesNotZeroHelper(std::string AddrsTrk, std::string AddrsI, std::string LivesTrk, std::vector<std::string> &Result, bool &Loop)
{
  if (finding_string(AddrsTrk, AddrsI))
    {
      if (stol(LivesTrk) != 0)
	{
	  Result.push_back(AddrsI);
	  Loop = false;
	}
      else Loop = false;
    }
}

void listof_addresses_bypath(std::vector<std::map<std::string, std::string>> &it_path_main, std::vector<std::string> &addrsv)
{
	std::vector<std::string> addrsvh;
	for (std::vector<std::map<std::string, std::string>>::iterator it = it_path_main.begin(); it != it_path_main.end(); ++it)
	{
		std::map<std::string, std::string> &it_ele = *it;
		if ( !find_string_strv(it_ele["addrs_src"], addrsv) )
		{
			addrsv.push_back(it_ele["addrs_src"]);
		}
		if ( !find_string_strv(it_ele["addrs_trk"], addrsv) )
		{
			addrsv.push_back(it_ele["addrs_trk"]);
		}
	}
	addrsv = addrsvh;
	for (std::vector<std::string>::iterator it = addrsvh.begin(); it != addrsvh.end(); ++it)
	{
		PrintToLog("\nAddress %s\n", *it);
	}
}

double PNL_function(double entry_price, double exit_price, long int amount_trd, struct status_amounts *pt_jrow_database)
{
  double PNL = 0;

  if ( finding_string("Long", pt_jrow_database->status_trk) )
    PNL = (double)amount_trd*(1/entry_price-1/exit_price);
  else if ( finding_string("Short", pt_jrow_database->status_trk) )
    PNL = (double)amount_trd*(1/exit_price-1/entry_price);

  return PNL;
}

double PNL_ghosts(double entry_price, double exit_price, long int amount_trd, std::string status)
{
  double PNL = 0;

  if (finding_string("Long", status))
    PNL = (double)amount_trd*(1/entry_price-1/exit_price);
  else if (finding_string("Short", status))
    PNL = (double)amount_trd*(1/exit_price-1/entry_price);

  return PNL;
}

void getting_gammapq_bypath(std::vector<std::map<std::string, std::string>> &path_main, double PNL_total, double &gamma_p, double &gamma_q, std::unordered_set<std::string> addrs_set)
{
  MatrixTLS &ndatabase = *pt_ndatabase;
  VectorTLS jrow_database(n_cols);
  long int sum_alpha_beta_i = 0;
  long int sum_alpha_i = 0;
  long int sum_alpha_beta_j = 0;
  long int sum_alpha_j = 0;

  std::vector<std::map<std::string, std::string>>::iterator it_path_main;
  for (it_path_main = path_main.begin(); it_path_main != path_main.end(); ++it_path_main)
    {
      std::map<std::string, std::string> &edge_ele = *it_path_main;
      if ( stol(edge_ele["lives_src"]) != 0 )
	{
	  sub_row(jrow_database, ndatabase, stol(edge_ele["edge_row"]));
	  struct status_amounts *pt_jrow_database = get_status_amounts_byaddrs(jrow_database, edge_ele["addrs_src"]);
	  if ( finding_string("Long", pt_jrow_database->status_trk) )
	    {
	      sum_alpha_beta_i += stol(edge_ele["lives_src"])*stod(edge_ele["exit_price"]);
	      sum_alpha_i += stol(edge_ele["lives_src"]);
	    }
	  else
	    {
	      sum_alpha_beta_j += stol(edge_ele["lives_src"])*stod(edge_ele["exit_price"]);
	      sum_alpha_j += stol(edge_ele["lives_src"]);
	    }
	}
      if ( stol(edge_ele["lives_trk"]) != 0 )
	{
	  sub_row(jrow_database, ndatabase, stol(edge_ele["edge_row"]));
	  struct status_amounts *pt_jrow_database = get_status_amounts_byaddrs(jrow_database, edge_ele["addrs_trk"]);
	  if ( finding_string("Long", pt_jrow_database->status_trk) )
	    {
	      sum_alpha_beta_i += stol(edge_ele["lives_trk"])*stod(edge_ele["entry_price"]);
	      sum_alpha_i += stol(edge_ele["lives_trk"]);
	    }
	  else
	    {
	      sum_alpha_beta_j += stol(edge_ele["lives_trk"])*stod(edge_ele["entry_price"]);
	      sum_alpha_j += stol(edge_ele["lives_trk"]);
	    }
	}
    }
  gamma_p = PNL_total-sum_alpha_beta_i+sum_alpha_beta_j;
  gamma_q = sum_alpha_j-sum_alpha_i;
}

void GhostEdgesComputing(std::vector<std::map<std::string, std::string>> lives_longs, std::vector<std::map<std::string, std::string>> lives_shorts, double exit_price_desired, std::vector<std::map<std::string, std::string>> &GhostEdgesArray)
{
  long int amount_itlongs  = 0;
  long int amount_itshorts = 0;
  unsigned index_start = 0;

  std::vector<std::map<std::string, std::string>>::iterator it_longs;
  std::vector<std::map<std::string, std::string>>::iterator it_shorts;
  std::map<std::string, std::string> short_ele;
  std::map<std::string, std::string> long_ele;
  std::map<std::string, std::string> edge_ele;

  for (unsigned i = 0; i < lives_longs.size(); i++)
  {
    long_ele = lives_longs[i];
    amount_itlongs = stol(long_ele["lives"]);

    for (unsigned j = index_start; j < lives_shorts.size(); j++)
    {
      short_ele = lives_shorts[j];
	    amount_itshorts = stol(short_ele["lives"]);

      if (amount_itlongs > amount_itshorts)
      {
        amount_itlongs = amount_itlongs - amount_itshorts;
	      // PrintToLog("\namount_itshorts = %d\t amount_itlongs = %d\n", amount_itshorts, amount_itlongs);

        BuildingGhostEdges(edge_ele, long_ele["addrs"], short_ele["addrs"], long_ele["status"], short_ele["status"], long_ele["entry_price"], short_ele["entry_price"], exit_price_desired, amount_itlongs);
	      GhostEdgesArray.push_back(edge_ele);

        continue;
      }
      if (amount_itlongs < amount_itshorts)
      {
        index_start = j;
	      lives_shorts[j]["lives"] = std::to_string(amount_itshorts - amount_itlongs);
	      // PrintToLog("\namount_itshorts = %d\t amount_itlongs = %d\n", amount_itshorts - amount_itlongs, amount_itlongs);

        BuildingGhostEdges(edge_ele, long_ele["addrs"], short_ele["addrs"], long_ele["status"], short_ele["status"], long_ele["entry_price"], short_ele["entry_price"], exit_price_desired, amount_itlongs);
	      GhostEdgesArray.push_back(edge_ele);

        break;
      }
	    if ( amount_itlongs == amount_itshorts )
      {
        index_start = j + 1;
	      // PrintToLog("\namount_itshorts = %d\t amount_itlongs = %d\n", amount_itshorts, amount_itlongs);

        BuildingGhostEdges(edge_ele, long_ele["addrs"], short_ele["addrs"], long_ele["status"], short_ele["status"], long_ele["entry_price"], short_ele["entry_price"], exit_price_desired, amount_itlongs);
	      GhostEdgesArray.push_back(edge_ele);

        break;
      }
    }
  }
}

void updating_lives_tozero(std::vector<std::vector<std::map<std::string, std::string>>> &path_main)
{
  std::vector<std::vector<std::map<std::string, std::string>>>::iterator it_path_main;
  std::vector<std::map<std::string, std::string>>::iterator it_path_maini;

  for (it_path_main = path_main.begin(); it_path_main != path_main.end(); ++it_path_main)
    {
      for (it_path_maini = (*it_path_main).begin(); it_path_maini != (*it_path_main).end(); ++it_path_maini)
	{
	  std::map<std::string, std::string> &edge_ele = *it_path_maini;
	  edge_ele["lives_src"] = std::to_string(0);
	  edge_ele["lives_trk"] = std::to_string(0);
	  printing_edges(*it_path_maini);
	}
    }
}

void joining_pathmain_ghostedges(std::vector<std::vector<std::map<std::string, std::string>>> &path_main, std::vector<std::map<std::string, std::string>> GhostEdgesArray)
{
  std::vector<std::vector<std::map<std::string, std::string>>>::iterator it_path_main;
  std::vector<std::map<std::string, std::string>>::iterator it_ghosts;
  long int index_path = 0;

  for (it_path_main = path_main.begin(); it_path_main != path_main.end(); ++it_path_main)
    {
      index_path = stol((*it_path_main).front()["path_number"]);
      for (it_ghosts = GhostEdgesArray.begin(); it_ghosts != GhostEdgesArray.end(); ++it_ghosts)
	{
	  if ( stol((*it_ghosts)["path_number"]) == index_path )
	    {
	      (*it_path_main).push_back(*it_ghosts);
	    }
	}
    }
}

void checkzeronetted_bypath_ghostedges(std::vector<std::map<std::string, std::string>> path_maini, long int nonzero_lives)
{
  std::vector<std::map<std::string, std::string>>::iterator it_path_maini;
  long int lives_settled = 0;

  if ( nonzero_lives != 0 )
    {
      for (it_path_maini = path_maini.begin(); it_path_maini != path_maini.end(); ++it_path_maini)
	{
	  if ( stol((*it_path_maini)["ghost_edge"]) == 1 )
	    {
	      lives_settled += stol((*it_path_maini)["amount_trd"]);
	    }
	}
    }
  //PrintToLog("\nLives in the Path - Lives settled : (%ld - %ld) = %ld\n", nonzero_lives, lives_settled, nonzero_lives - lives_settled);
}

long int checkpath_livesnonzero(std::vector<std::map<std::string, std::string>> path_maini)
{
  std::vector<std::map<std::string, std::string>>::iterator it_path_maini;
  long int lives = 0;

  for (it_path_maini = path_maini.begin(); it_path_maini != path_maini.end(); ++it_path_maini)
    {
      lives += stol((*it_path_maini)["lives_src"])+stol((*it_path_maini)["lives_trk"]);
    }
  return lives;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson, John B. Mains
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef BENCHMARK__BENCH_TYPES_HPP_
#define BENCHMARK__BENCH_TYPES_HPP_

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <smooth/feedback/qp.hpp>

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>

/**
 * @brief Create a random quadratic program of dimension m x n.
 * @param m number on inequalities
 * @param n number of variables
 * @param density approximate proportion of nonzeros in P and A
 * @param RNG random number generator
 */
template<typename RNG>
smooth::feedback::QuadraticProgram<-1, -1> random_qp(int m, int n, double density, RNG & rng)
{
  std::bernoulli_distribution bdist(density);
  std::uniform_real_distribution<double> udist(-1, 1);

  Eigen::MatrixXd A =
    Eigen::MatrixXd::NullaryExpr(m, n, [&](int, int) { return bdist(rng) ? udist(rng) : 0.; });
  Eigen::MatrixXd Lrand =
    Eigen::MatrixXd::NullaryExpr(n, n, [&](int, int) { return bdist(rng) ? udist(rng) : 0.; });
  Eigen::MatrixXd L                         = Eigen::MatrixXd::Zero(n, n);
  L.template triangularView<Eigen::Lower>() = Lrand.template triangularView<Eigen::Lower>();
  for (int i = 0; i < n; ++i) { L(i, i) = std::max({L(i, i), -L(i, i), 0.05}); }

  Eigen::VectorXd v     = Eigen::VectorXd::NullaryExpr(n, [&](int) { return udist(rng); });
  Eigen::VectorXd delta = Eigen::VectorXd::NullaryExpr(m, [&](int) { return udist(rng); });

  return smooth::feedback::QuadraticProgram<-1, -1>{
    .P = L * L.transpose(),
    .q = Eigen::VectorXd::NullaryExpr(n, [&](int) { return udist(rng); }),
    .A = A,
    .l = Eigen::VectorXd::Constant(m, -std::numeric_limits<double>::infinity()),
    .u = A * v + delta,
  };
}

/**
 * @brief Create a random sparse quadratic program of dimension m x n.
 */
inline smooth::feedback::QuadraticProgramSparse<double>
qp_dense_to_sparse(const smooth::feedback::QuadraticProgram<-1, -1, double> & qp)
{
  smooth::feedback::QuadraticProgramSparse<double> qps;

  qps.P = qp.P.sparseView();
  qps.P.prune(1e-6);
  qps.P.makeCompressed();
  qps.q = qp.q;
  qps.A = qp.A.sparseView();
  qps.A.prune(1e-6);
  qps.A.makeCompressed();
  qps.l = qp.l;
  qps.u = qp.u;

  return qps;
}

struct BenchResult
{
  smooth::feedback::QPSolutionStatus status;
  std::chrono::high_resolution_clock::duration dt;
  uint64_t iter;
  Eigen::VectorXd solution;
  double objective;
};

struct BatchResult
{
  std::vector<BenchResult> results{};

  std::size_t num_optimal{0};

  double total_duration_success{0};
  double total_duration_fail{0};

  double avg_duration_success{0};
  double avg_duration_fail{0};
};

template<typename SolverWrapper, std::ranges::range R>
BatchResult solve_batch(const R & qps, const smooth::feedback::QPSolverParams & prm)
{
  auto work_range =
    std::views::transform(qps, [&prm](const auto & qp) { return SolverWrapper(qp, prm); });

  BatchResult ret{};

  for (const auto & work : work_range) { ret.results.push_back(work()); }

  // calculate accuracies
  for (const auto & result : ret.results) {
    if (result.status == smooth::feedback::QPSolutionStatus::Optimal) {
      ++ret.num_optimal;
      ret.total_duration_success += std::chrono::duration<double>(result.dt).count();
    } else {
      ret.total_duration_fail += std::chrono::duration<double>(result.dt).count();
    }
  }

  std::size_t num_fail = ret.results.size() - ret.num_optimal;

  ret.avg_duration_success =
    ret.num_optimal > 0 ? ret.total_duration_success / ret.num_optimal : -1;
  ret.avg_duration_fail = num_fail > 0 ? ret.total_duration_fail / num_fail : -1;

  return ret;
}

#endif  // BENCHMARK__BENCH_TYPES_HPP_


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_ACTUATION_BASE_HPP_
#define CROCODDYL_CORE_ACTUATION_BASE_HPP_

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

/**
 * @brief Abstract class for the actuation-mapping model
 *
 * The generalized torques \f$\boldsymbol{\tau}\in\mathbb{R}^{nv}\f$ can by any nonlinear function of the
 * control inputs \f$\mathbf{u}\in\mathbb{R}^{nu}\f$, and state point \f$\mathbf{x}\in\mathbb{R}^{nx}\f$, where
 * `nv`, `nu`, and `ndx` are the number of joints, dimension of the control input and state manifold,
 * respectively. Additionally, the generalized torques are also named as the actuation signals of our system.
 *
 * The main computations are carrying out in `calc()`, and `calcDiff()`, where the former computes actuation signal,
 * and the latter computes the Jacobians of the actuation-mapping function, i.e.,
 * \f$\frac{\partial\boldsymbol{\tau}}{\partial\mathbf{x}}\f$ and
 * \f$\frac{\partial\boldsymbol{\tau}}{\partial\mathbf{u}}\f$. Note that `calcDiff()` requires to run `calc()` first.
 *
 * \sa `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class ActuationModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialize the actuation model
   *
   * @param[in] state  State description
   * @param[in] nu     Dimension of control vector
   */
  ActuationModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu);
  virtual ~ActuationModelAbstractTpl();

  /**
   * @brief Compute the actuation signal from the state point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ and control input
   * \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   *
   * @param[in] data  Actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Ignore the computation of the actuation signal
   *
   * It does not update the actuation signal as this function is used in the terminal nodes of an optimal
   * control problem.
   *
   * @param[in] data  Actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   */
  void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Compute the Jacobians of the actuation function
   *
   * @param[in] data  Actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Ignore the computation of the Jacobians of the actuation function
   *
   * It does not update the Jacobians of the actuation function as this function is used in the terminal
   * nodes of an optimal control problem.
   *
   * @param[in] data  Actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   */
  void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Create the actuation data
   *
   * @return the actuation data
   */
  virtual boost::shared_ptr<ActuationDataAbstract> createData();

  /**
   * @brief Return the dimension of the control input
   */
  std::size_t get_nu() const;

  /**
   * @brief Return the state
   */
  const boost::shared_ptr<StateAbstract>& get_state() const;

  /**
   * @brief Print information on the residual model
   */
  template <class Scalar>
  friend std::ostream& operator<<(std::ostream& os, const ResidualModelAbstractTpl<Scalar>& model);

  /**
   * @brief Print relevant information of the residual model
   *
   * @param[out] os  Output stream object
   */
  virtual void print(std::ostream& os) const;

 protected:
  std::size_t nu_;                          //!< Control dimension
  boost::shared_ptr<StateAbstract> state_;  //!< Model of the state
};

template <typename _Scalar>
struct ActuationDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  explicit ActuationDataAbstractTpl(Model<Scalar>* const model)
      : tau(model->get_state()->get_nv()),
        dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
        dtau_du(model->get_state()->get_nv(), model->get_nu()) {
    tau.setZero();
    dtau_dx.setZero();
    dtau_du.setZero();
  }
  virtual ~ActuationDataAbstractTpl() {}

  VectorXs tau;      //!< Actuation (generalized force) signal
  MatrixXs dtau_dx;  //!< Partial derivatives of the actuation model w.r.t. the state point
  MatrixXs dtau_du;  //!< Partial derivatives of the actuation model w.r.t. the control input
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/actuation-base.hxx"

#endif  // CROCODDYL_CORE_ACTUATION_BASE_HPP_


#pragma once

#include <mona/rect.hpp>
#include <mona/mesh.hpp>

#include "line_renderer.hpp"
#include "colors.hpp"
#include "mona/internal/shader.hpp"

#include <armadillo>

namespace mona
{
    class dots: public mesh
    {
    private:
        mona::rect span_area;
        uint vbo = 0, vao = 0;
        int size = 0;
        mutable shader s;
        std::vector<glm::vec3> vertices;
        float radius = 10;

    public:
        dots(const arma::fvec& x, const arma::fvec& y, const std::vector<glm::vec4>& colors);
        dots(const arma::fvec& x, const arma::fvec& y, glm::vec4 color = colors::black);

        // returns the area that this series cover
        // this is to be used to calculate the axes tick values
        auto span() const -> mona::rect override
        {
            return span_area;
        }

        //auto reset(const arma::fvec& x, const arma::fvec y) -> void;
        auto draw(const glm::mat4& mvp) const -> void override;

        auto set_radius(float r) -> void
        {
            radius = r;
        }
    };
};


#include <omp.h>
#include <blitz.h>

using namespace blitz;

// N C H W
Shape input_shape(4);
// K C R S
Shape filter_shape(4);
// N K P Q
Shape output_shape(4);
// cpu workspace
Shape workspace_shape_cpu(1);

void set_input_shape_nchw(size_t N, size_t C, size_t H, size_t W, BLITZ_DATA_LAYOUT data_layout) {
  input_shape[0] = N;
  input_shape[1] = C;
  input_shape[2] = H;
  input_shape[3] = W;
  input_shape.set_data_layout(data_layout);
}

void set_input_shape_nhwc(size_t N, size_t C, size_t H, size_t W, BLITZ_DATA_LAYOUT data_layout) {
  input_shape[0] = N;
  input_shape[1] = H;
  input_shape[2] = W;
  input_shape[3] = C;
  input_shape.set_data_layout(data_layout);
}

void set_filter_shape_kcrs(size_t K, size_t C, size_t R, size_t S, BLITZ_DATA_LAYOUT data_layout) {
  filter_shape[0] = K;
  filter_shape[1] = C;
  filter_shape[2] = R;
  filter_shape[3] = S;
  filter_shape.set_data_layout(data_layout);
}

void set_output_shape_nkpq(size_t N, size_t K, size_t P, size_t Q, BLITZ_DATA_LAYOUT data_layout) {
  output_shape[0] = N;
  output_shape[1] = K;
  output_shape[2] = P;
  output_shape[3] = Q;
  output_shape.set_data_layout(data_layout);
}

void compare(const float* algo1, const float* algo2, size_t size) {
  for (size_t i = 0; i < size; ++i) {
    if (algo1[i] >= algo2[i] + 1e-5 || algo1[i] <= algo2[i] - 1e-5) {
      LOG(FATAL) << "index: " << i << " value1: " << algo1[i] << " value2: " << algo2[i];
    }
  }
}

void unpack_base(
  const float* input,
  float* unpack,
  size_t channel,
  size_t input_height,
  size_t input_width,
  size_t filter_height,
  size_t filter_width,
  size_t output_height,
  size_t output_width,
  size_t padding_height,
  size_t padding_width,
  size_t stride_height,
  size_t stride_width) {
  // base line impl borrow from caffe2
  size_t channels = channel * filter_height * filter_width;
  for (size_t c = 0; c < channels; ++c) {
    size_t w_offset = c % filter_width;
    size_t h_offset = (c / filter_width) % filter_height;
    size_t c_im = c / filter_height / filter_width;
    for (size_t h = 0; h < output_height; ++h) {
      for (size_t w = 0; w < output_width; ++w) {
        int h_pad = h * stride_height - padding_height + h_offset;
        int w_pad = w * stride_width - padding_width + w_offset;
        if (h_pad >= 0 && h_pad < static_cast<int>(input_height) &&
          w_pad >= 0 && w_pad < static_cast<int>(input_width))
          unpack[(c * output_height + h) * output_width + w] =
            input[(c_im * input_height + h_pad) * input_width + w_pad];
        else
          unpack[(c * output_height + h) * output_width + w] = 0;
      }
    }
  }
}

void pack_base(
  const float* unpack,
  float* input,
  size_t channel,
  size_t input_height,
  size_t input_width,
  size_t filter_height,
  size_t filter_width,
  size_t output_height,
  size_t output_width,
  size_t padding_height,
  size_t padding_width,
  size_t stride_height,
  size_t stride_width) {
  // base line impl borrow from caffe2
  size_t channels = channel * filter_height * filter_width;
  for (size_t c = 0; c < channels; ++c) {
    size_t w_offset = c % filter_width;
    size_t h_offset = (c / filter_width) % filter_height;
    size_t c_im = c / filter_height / filter_width;
    for (size_t h = 0; h < output_height; ++h) {
      for (size_t w = 0; w < output_width; ++w) {
        int h_pad = h * stride_height - padding_height + h_offset;
        int w_pad = w * stride_width - padding_width + w_offset;
        if (h_pad >= 0 && h_pad < static_cast<int>(input_height) &&
          w_pad >= 0 && w_pad < static_cast<int>(input_width)) {
          input[(c_im * input_height + h_pad) * input_width + w_pad] +=
            unpack[(c * output_height + h) * output_width + w];
        }
      }
    }
  }
}

void input_hwc2chw(const float* hwc, float* chw, size_t channel, size_t input_height, size_t input_width) {
  for (size_t i = 0; i < channel; ++i) {
    for (size_t j = 0; j < input_height; ++j) {
      for (size_t k = 0; k < input_width; ++k) {
        chw[i * input_height * input_width + j * input_width + k] = hwc[j * input_width * channel + k * channel + i];
      }
    }
  }
}

void input_chw2hwc(const float* chw, float* hwc, size_t channel, size_t input_height, size_t input_width) {
  for (size_t i = 0; i < channel; ++i) {
    for (size_t j = 0; j < input_height; ++j) {
      for (size_t k = 0; k < input_width; ++k) {
        hwc[j * input_width * channel + k * channel + i] = chw[i * input_height * input_width + j * input_width + k];
      }
    }
  }
}

void workspace_hwc2chw(const float* hwc, float* chw, size_t output_height, size_t output_width,
  size_t channel, size_t filter_height, size_t filter_width) {
  for (size_t i = 0; i < output_height * output_width; ++i) {
    for (size_t j = 0; j < filter_height; ++j) {
      for (size_t k = 0; k < filter_width; ++k) {
        for (size_t v = 0; v < channel; ++v) {
          chw[i * filter_height * filter_width * channel + v * filter_height * filter_width + j * filter_width + k] =
            hwc[i * filter_height * filter_width * channel + j * filter_width * channel + k * channel + v];
        }
      }
    }
  }
}

void unpack(size_t pad_h, size_t pad_w, size_t str_h, size_t str_w, size_t iterations) {
  // shape decode
  size_t N, H, W, C, R, S, K, P, Q;
  Blitz2DBuffer(input_shape, &N, &C, &H, &W);
  Blitz2DFilter(filter_shape, &K, &C, &R, &S);
  Blitz2DBuffer(output_shape, &N, &K, &P, &Q);
  // set up cpu
  CPUTensor<float> input_cpu(input_shape);
  CPUTensor<float> input_cpu_transform(input_shape);
  CPUTensor<float> workspace_cpu(workspace_shape_cpu);
  CPUTensor<float> workspace_cpu_optimize(workspace_shape_cpu);
  CPUTensor<float> workspace_cpu_transform(workspace_shape_cpu);
  Backend<CPUTensor, float>::UniformDistributionFunc(&input_cpu, 0.0, 1.0);

  if (input_shape.data_layout() == BLITZ_BUFFER_NHWC) {
    input_hwc2chw(input_cpu.data(), input_cpu_transform.data(), C, H, W);
  } else {
    memcpy(input_cpu_transform.data(), input_cpu.data(), sizeof(float) * input_cpu.size());
  }
  for (size_t i = 0; i < iterations; ++i) {
    unpack_base(
      input_cpu_transform.data(), workspace_cpu.data(),
      C, H, W, R, S, P, Q,
      pad_h, pad_w, str_h, str_w);
  }

  for (size_t i = 0; i < iterations; ++i) {
    Backend<CPUTensor, float>::Unpack2DFunc(
      &input_cpu, &workspace_cpu_optimize,
      R, S,
      pad_h, pad_w,
      str_h, str_w);
  }

  bool hwc = false;
  bool transform = false;

  if (input_shape.data_layout() == BLITZ_BUFFER_NHWC) {
    hwc = true;
    transform = true;
  }

  if (hwc == true) {
    workspace_hwc2chw(workspace_cpu_optimize.data(), workspace_cpu_transform.data(), P, Q, C, R, S);
    memcpy(workspace_cpu_optimize.data(), workspace_cpu_transform.data(), sizeof(float) * workspace_cpu_optimize.size());
  }

  if (transform == true) {
    Shape shape(2);
    shape[0] = P * Q;
    shape[1] = workspace_shape_cpu[0] / (P * Q);
    workspace_cpu_optimize.set_shape(shape);
    shape[0] = workspace_shape_cpu[0] / (P * Q);
    shape[1] = P * Q;
    workspace_cpu_transform.set_shape(shape);
    Backend<CPUTensor, float>::Transpose2DFunc(&workspace_cpu_optimize, &workspace_cpu_transform);
    memcpy(workspace_cpu_optimize.data(), workspace_cpu_transform.data(), sizeof(float) * workspace_cpu_optimize.size());
  }

  compare(workspace_cpu.data(), workspace_cpu_optimize.data(), workspace_shape_cpu.size());
}

void pack(size_t pad_h, size_t pad_w, size_t str_h, size_t str_w, size_t iterations) {
  // shape decode
  size_t N, H, W, C, R, S, K, P, Q;
  Blitz2DBuffer(input_shape, &N, &C, &H, &W);
  Blitz2DFilter(filter_shape, &K, &C, &R, &S);
  Blitz2DBuffer(output_shape, &N, &K, &P, &Q);
  // set up cpu
  CPUTensor<float> input_cpu(input_shape);
  CPUTensor<float> input_cpu_transform(input_shape);
  CPUTensor<float> input_cpu_optimize(input_shape);
  CPUTensor<float> workspace_cpu(workspace_shape_cpu);
  CPUTensor<float> workspace_cpu_optimize(workspace_shape_cpu);
  CPUTensor<float> workspace_cpu_transform(workspace_shape_cpu);
  Backend<CPUTensor, float>::UniformDistributionFunc(&workspace_cpu_optimize, 0.0, 1.0);
  memcpy(workspace_cpu.data(), workspace_cpu_optimize.data(), sizeof(float) * workspace_cpu.size());

  bool hwc = false;
  bool transform = false;

  if (input_shape.data_layout() == BLITZ_BUFFER_NHWC) { // PQRSC to CRSPQ
    hwc = true;
    transform = true;
  }

  if (hwc == true) { // PQRSC to PQCRS
    workspace_hwc2chw(workspace_cpu_optimize.data(), workspace_cpu_transform.data(), P, Q, C, R, S);
    memcpy(workspace_cpu.data(), workspace_cpu_transform.data(), sizeof(float) * workspace_cpu.size());
  }

  if (transform == true) { // CRSPQ
    Shape shape(2);
    shape[0] = P * Q;
    shape[1] = workspace_shape_cpu[0] / (P * Q);
    workspace_cpu.set_shape(shape); // PQCRS
    shape[0] = workspace_shape_cpu[0] / (P * Q);
    shape[1] = P * Q;
    workspace_cpu_transform.set_shape(shape); // CRSPQ
    Backend<CPUTensor, float>::Transpose2DFunc(&workspace_cpu, &workspace_cpu_transform);
    memcpy(workspace_cpu.data(), workspace_cpu_transform.data(), sizeof(float) * workspace_cpu.size());
  }

  for (size_t i = 0; i < iterations; ++i) {
    pack_base(
      workspace_cpu.data(), input_cpu.data(),
      C, H, W, R, S, P, Q,
      pad_h, pad_w, str_h, str_w);
  }

  for (size_t i = 0; i < iterations; ++i) {
    Backend<CPUTensor, float>::Pack2DFunc(
      &workspace_cpu_optimize, &input_cpu_optimize,
      R, S,
      pad_h, pad_w,
      str_h, str_w);
  }

  if (input_shape.data_layout() == BLITZ_BUFFER_NHWC) {
    input_hwc2chw(input_cpu_optimize.data(), input_cpu_transform.data(), C, H, W);
    memcpy(input_cpu_optimize.data(), input_cpu_transform.data(), sizeof(float) * input_cpu.size());
  }

  compare(input_cpu.data(), input_cpu_optimize.data(), input_cpu.size());
}

int main(int argc, char** argv) {
  const size_t NUM_ARGS = 17;
  // phase C H W R S K P Q pad_h pad_w str_h str_w iterations
  if (argc != NUM_ARGS + 1) {
    std::cerr << "Not matchable args!" << std::endl;
    exit(1);
  }
  // get args
  const std::string phase = std::string(argv[1]);
  const std::string input_data_layout = std::string(argv[2]);
  const std::string output_data_layout = std::string(argv[3]);
  const std::string filter_data_layout = std::string(argv[4]);
  const size_t C = atoi(argv[5]);
  const size_t H = atoi(argv[6]);
  const size_t W = atoi(argv[7]);
  const size_t R = atoi(argv[8]);
  const size_t S = atoi(argv[9]);
  const size_t K = atoi(argv[10]);
  const size_t P = atoi(argv[11]);
  const size_t Q = atoi(argv[12]);
  const size_t pad_h = atoi(argv[13]);
  const size_t pad_w = atoi(argv[14]);
  const size_t str_h = atoi(argv[15]);
  const size_t str_w = atoi(argv[16]);
  const size_t iterations = atoi(argv[17]);
  // set shapes
  if (BlitzParseShape(input_data_layout) == BLITZ_BUFFER_NCHW) {
    set_input_shape_nchw(1, C, H, W, BlitzParseShape(input_data_layout));
  } else {
    set_input_shape_nhwc(1, C, H, W, BlitzParseShape(input_data_layout));
  }
  set_filter_shape_kcrs(K, C, R, S, BlitzParseShape(filter_data_layout));
  set_output_shape_nkpq(1, K, P, Q, BlitzParseShape(output_data_layout));
  // set workspace shape
  workspace_shape_cpu[0] = C * R * S * P * Q;
  // run pack
  if (phase == "pack") {
    pack(pad_h, pad_w, str_h, str_w, iterations);
  } else if (phase == "unpack") {
    unpack(pad_h, pad_w, str_h, str_w, iterations);
  }
  return 0;
}


/**
 * \file QuaternionConvertFilter.cpp
 */

#include <complex>
#include <cstdint>

#include <simdpp/simd.h>

#include <ATK/Core/SIMD/QuaternionConvertFilter.h>

#include <ATK/Mock/TriangleCheckerFilter.h>
#include <ATK/Mock/TriangleGeneratorFilter.h>
#include <ATK/Tools/SumFilter.h>

#include <ATK/Core/PipelineGlobalSinkFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (1024*1024)

BOOST_AUTO_TEST_CASE(BOOST_PP_CAT(SIMDPP_ARCH_NAMESPACE, QuaternionBasedFilter_triangle_real_to_real_test))
{
  ATK::TriangleGeneratorFilter<float> generator1;
  generator1.set_output_sampling_rate(48000);
  generator1.set_frequency(1000);
  generator1.set_amplitude(1);
  ATK::TriangleGeneratorFilter<float> generator2;
  generator2.set_output_sampling_rate(48000);
  generator2.set_frequency(10000);
  generator2.set_amplitude(.5);
  ATK::TriangleGeneratorFilter<float> generator3;
  generator3.set_output_sampling_rate(48000);
  generator3.set_frequency(100);
  generator3.set_amplitude(5);
  ATK::TriangleGeneratorFilter<float> generator4;
  generator4.set_output_sampling_rate(48000);
  generator4.set_frequency(1000);
  generator4.set_amplitude(10);

  ATK::RealToQuaternionFilter<float, simdpp::float32<4> > converter1;
  converter1.set_input_sampling_rate(48000);
  converter1.set_output_sampling_rate(48000);

  ATK::QuaternionToRealFilter<simdpp::float32<4>, float > converter2;
  converter2.set_input_sampling_rate(48000);
  converter2.set_output_sampling_rate(48000);

  ATK::TriangleCheckerFilter<float> checker1;
  checker1.set_input_sampling_rate(48000);
  checker1.set_frequency(1000);
  checker1.set_amplitude(1);
  ATK::TriangleCheckerFilter<float> checker2;
  checker2.set_input_sampling_rate(48000);
  checker2.set_frequency(10000);
  checker2.set_amplitude(.5);
  ATK::TriangleCheckerFilter<float> checker3;
  checker3.set_input_sampling_rate(48000);
  checker3.set_frequency(100);
  checker3.set_amplitude(5);
  ATK::TriangleCheckerFilter<float> checker4;
  checker4.set_input_sampling_rate(48000);
  checker4.set_frequency(1000);
  checker4.set_amplitude(10);

  converter1.set_input_port(0, &generator1, 0);
  converter1.set_input_port(1, &generator2, 0);
  converter1.set_input_port(2, &generator3, 0);
  converter1.set_input_port(3, &generator4, 0);

  converter2.set_input_port(0, &converter1, 0);

  checker1.set_input_port(0, &converter2, 0);
  checker2.set_input_port(0, &converter2, 1);
  checker3.set_input_port(0, &converter2, 2);
  checker4.set_input_port(0, &converter2, 3);

  ATK::PipelineGlobalSinkFilter globalsink;
  globalsink.set_input_sampling_rate(48000);
  globalsink.add_filter(&checker1);
  globalsink.add_filter(&checker2);
  globalsink.add_filter(&checker3);
  globalsink.add_filter(&checker4);
  globalsink.process(PROCESSSIZE);
}

#ifdef SIMDPP_EMIT_DISPATCHER
BOOST_AUTO_TEST_CASE(QuaternionBasedFilter_triangle_real_to_real_test)
{
  ATK::TriangleGeneratorFilter<float> generator1;
  generator1.set_output_sampling_rate(48000);
  generator1.set_frequency(1000);
  generator1.set_amplitude(1);
  ATK::TriangleGeneratorFilter<float> generator2;
  generator2.set_output_sampling_rate(48000);
  generator2.set_frequency(10000);
  generator2.set_amplitude(.5);
  ATK::TriangleGeneratorFilter<float> generator3;
  generator3.set_output_sampling_rate(48000);
  generator3.set_frequency(100);
  generator3.set_amplitude(5);
  ATK::TriangleGeneratorFilter<float> generator4;
  generator4.set_output_sampling_rate(48000);
  generator4.set_frequency(1000);
  generator4.set_amplitude(10);

  std::unique_ptr<ATK::BaseFilter> converter1 = ATK::createRealToQuaternionFilter<float>();
  converter1->set_input_sampling_rate(48000);
  converter1->set_output_sampling_rate(48000);

  std::unique_ptr<ATK::BaseFilter> converter2 = ATK::createQuaternionToRealFilter<float>();
  converter2->set_input_sampling_rate(48000);
  converter2->set_output_sampling_rate(48000);

  ATK::TriangleCheckerFilter<float> checker1;
  checker1.set_input_sampling_rate(48000);
  checker1.set_frequency(1000);
  checker1.set_amplitude(1);
  ATK::TriangleCheckerFilter<float> checker2;
  checker2.set_input_sampling_rate(48000);
  checker2.set_frequency(10000);
  checker2.set_amplitude(.5);
  ATK::TriangleCheckerFilter<float> checker3;
  checker3.set_input_sampling_rate(48000);
  checker3.set_frequency(100);
  checker3.set_amplitude(5);
  ATK::TriangleCheckerFilter<float> checker4;
  checker4.set_input_sampling_rate(48000);
  checker4.set_frequency(1000);
  checker4.set_amplitude(10);

  converter1->set_input_port(0, &generator1, 0);
  converter1->set_input_port(1, &generator2, 0);
  converter1->set_input_port(2, &generator3, 0);
  converter1->set_input_port(3, &generator4, 0);

  converter2->set_input_port(0, converter1.get(), 0);

  checker1.set_input_port(0, converter2.get(), 0);
  checker2.set_input_port(0, converter2.get(), 1);
  checker3.set_input_port(0, converter2.get(), 2);
  checker4.set_input_port(0, converter2.get(), 3);

  ATK::PipelineGlobalSinkFilter globalsink;
  globalsink.set_input_sampling_rate(48000);
  globalsink.add_filter(&checker1);
  globalsink.add_filter(&checker2);
  globalsink.add_filter(&checker3);
  globalsink.add_filter(&checker4);
  globalsink.process(PROCESSSIZE);
}
#endif


/* boost random/geometric_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: geometric_distribution.hpp,v 1.13 2002/12/22 22:03:10 jmaurer Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP
#define BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP

#include <cmath>          // std::log
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::log;
#endif

// geometric distribution: p(i) = (1-p) * pow(p, i-1)   (integer)
template<class UniformRandomNumberGenerator, class IntType = int,
         class RealType = double,
         class Adaptor = uniform_01<UniformRandomNumberGenerator, RealType> >
class geometric_distribution
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  explicit geometric_distribution(base_type & rng,
                                  const RealType& p = RealType(0.5))
    : _rng(rng), _p(p)
  {
    assert(RealType(0) < p && p < RealType(1));
    init();
  }

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

  RealType p() const { return _p; }
  adaptor_type& adaptor() { return _rng; }
  base_type& base() const { return _rng.base(); }
  void reset() { _rng.reset(); }

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
    using std::floor;
#endif
    return IntType(floor(log(RealType(1)-_rng()) / _log_p)) + IntType(1);
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const geometric_distribution& x,
                         const geometric_distribution& y)
  { return x._log_p == y._log_p && x._rng == y._rng; }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const geometric_distribution& gd)
  {
    os << gd._p;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, geometric_distribution& gd)
  {
    is >> std::ws >> gd._p;
    gd.init();
    return is;
  }
#endif

#else
  // Use a member function
  bool operator==(const geometric_distribution& rhs) const
  { return _log_p == rhs._log_p && _rng == rhs._rng;  }
#endif
private:
  void init()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
#endif
    _log_p = log(_p);
  }

  adaptor_type _rng;
  RealType _p;
  RealType _log_p;
};

} // namespace boost

#endif // BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP


/*
 * @Description: 点云畸变补偿
 * @Author: Ren Qian
 * @Date: 2020-02-25 14:38:12
 */

#ifndef LIDAR_LOCALIZATION_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_
#define LIDAR_LOCALIZATION_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_

#include <pcl/common/transforms.h>
#include <Eigen/Dense>
#include "glog/logging.h"

#include "lidar_localization/models/scan_adjust/distortion_adjust.hpp"
#include "lidar_localization/sensor_data/velocity_data.hpp"
#include "lidar_localization/sensor_data/cloud_data.hpp"

namespace lidar_localization {
class DistortionAdjust {
  public:
    void SetMotionInfo(float scan_period, VelocityData velocity_data);
    bool AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr);

  private:
    inline Eigen::Matrix3f UpdateMatrix(float real_time);

  private:
    float scan_period_;
    Eigen::Vector3f velocity_;
    Eigen::Vector3f angular_rate_;
};
} // namespace lidar_slam
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.
//
// Algorithm inspired by Nick Vannieuwenhoven, written by Cornelius Steinhardt

#define MTL_VPT_LEVEL 3

#include <iostream>
#include <boost/timer.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/mtl/io/read_el_matrix.hpp>

int main(int, char** argv)
{
    mtl::vampir_trace<9999> tracer;
    typedef double value_type;

    std::string program_dir= mtl::io::directory_name(argv[0]),
  	        matrix_file= mtl::io::join(program_dir, "../../mtl/test/matrix_market/square3.mtx");
// 		matrix_file= mtl::io::join(program_dir, "../../mtl/test/matrix_market/obstacle_small.mtx");
// 	        matrix_file= mtl::io::join(program_dir, "../../../../../branches/data/matrix_market/obstacle_q1q1_e64/obstacle_q1q1_e64_r00800.mtx");

    mtl::mat::element_structure<value_type> A;
    read_el_matrix(matrix_file, A);
    A.make_compact();

    const int size= A.get_total_vars();
    mtl::dense_vector<value_type>              x(size, 1), b(size), ident(size);
    iota(ident);
    b= 7.0;

    for (int i= 0; i < 100; i++) {
	b+= A * x;
	if (size <= 100)
	    std::cout << "b = " << b << '\n';
    }

    if (size <= 100)
	std::cout << "b = " << b << '\n';

    return 0;
}