// Copyright 2004-2006 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_PLOD_GENERATOR_HPP
#define BOOST_GRAPH_PLOD_GENERATOR_HPP

#include <iterator>
#include <utility>
#include <boost/random/uniform_int.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/graph/graph_traits.hpp>
#include <vector>
#include <map>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/mpl/if.hpp>

namespace boost {
  template<typename RandomGenerator>
  class out_directed_plod_iterator
  {
  public:
    typedef std::forward_iterator_tag            iterator_category;
    typedef std::pair<std::size_t, std::size_t>  value_type;
    typedef const value_type&                    reference;
    typedef const value_type*                    pointer;
    typedef std::ptrdiff_t                       difference_type;

    out_directed_plod_iterator() : gen(0), at_end(true) { }

    out_directed_plod_iterator(RandomGenerator& gen, std::size_t n,
                               double alpha, double beta,
                               bool allow_self_loops)
      : gen(&gen), n(n), alpha(alpha), beta(beta),
        allow_self_loops(allow_self_loops), at_end(false), degree(0),
        current(0, 0)
    {
      using std::pow;

      uniform_int<std::size_t> x(0, n-1);
      std::size_t xv = x(gen);
      degree = (xv == 0? 0 : std::size_t(beta * pow(xv, -alpha)));
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    out_directed_plod_iterator& operator++()
    {
      using std::pow;

      uniform_int<std::size_t> x(0, n-1);

      // Continue stepping through source nodes until the
      // (out)degree is > 0
      while (degree == 0) {
        // Step to the next source node. If we've gone past the
        // number of nodes we're responsible for, we're done.
        if (++current.first >= n) {
          at_end = true;
          return *this;
        }

        std::size_t xv = x(*gen);
        degree = (xv == 0? 0 : std::size_t(beta * pow(xv, -alpha)));
      }

      do {
        current.second = x(*gen);
      } while (current.first == current.second && !allow_self_loops);
      --degree;

      return *this;
    }

    out_directed_plod_iterator operator++(int)
    {
      out_directed_plod_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const out_directed_plod_iterator& other) const
    {
      return at_end == other.at_end;
    }

    bool operator!=(const out_directed_plod_iterator& other) const
    {
      return !(*this == other);
    }

  private:
    RandomGenerator* gen;
    std::size_t n;
    double alpha;
    double beta;
    bool allow_self_loops;
    bool at_end;
    std::size_t degree;
    value_type current;
  };

  template<typename RandomGenerator>
  class undirected_plod_iterator
  {
    typedef std::vector<std::pair<std::size_t, std::size_t> > out_degrees_t;

  public:
    typedef std::input_iterator_tag              iterator_category;
    typedef std::pair<std::size_t, std::size_t>  value_type;
    typedef const value_type&                    reference;
    typedef const value_type*                    pointer;
    typedef std::ptrdiff_t                       difference_type;

    undirected_plod_iterator()
      : gen(0), out_degrees(), degrees_left(0), allow_self_loops(false) { }

    undirected_plod_iterator(RandomGenerator& gen, std::size_t n,
                             double alpha, double beta,
                             bool allow_self_loops = false)
      : gen(&gen), n(n), out_degrees(new out_degrees_t),
        degrees_left(0), allow_self_loops(allow_self_loops)
    {
      using std::pow;

      uniform_int<std::size_t> x(0, n-1);
      for (std::size_t i = 0; i != n; ++i) {
        std::size_t xv = x(gen);
        std::size_t degree = (xv == 0? 0 : std::size_t(beta * pow(xv, -alpha)));
        if (degree == 0) degree = 1;
        else if (degree >= n) degree = n-1;
        out_degrees->push_back(std::make_pair(i, degree));
        degrees_left += degree;
      }

      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    undirected_plod_iterator& operator++()
    {
      next();
      return *this;
    }

    undirected_plod_iterator operator++(int)
    {
      undirected_plod_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const undirected_plod_iterator& other) const
    {
      return degrees_left == other.degrees_left;
    }

    bool operator!=(const undirected_plod_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      std::size_t source, target;
      while (true) {
        /* We may get to the point where we can't actually find any
           new edges, so we just add some random edge and set the
           degrees left = 0 to signal termination. */
        if (out_degrees->size() < 2) {
          uniform_int<std::size_t> x(0, n-1);
          current.first  = x(*gen);
          do {
            current.second = x(*gen);
          } while (current.first == current.second && !allow_self_loops);
          degrees_left = 0;
          out_degrees->clear();
          return;
        }

        uniform_int<std::size_t> x(0, out_degrees->size()-1);

        // Select source vertex
        source = x(*gen);
        if ((*out_degrees)[source].second == 0) {
          (*out_degrees)[source] = out_degrees->back();
          out_degrees->pop_back();
          continue;
        }

        // Select target vertex
        target = x(*gen);
        if ((*out_degrees)[target].second == 0) {
          (*out_degrees)[target] = out_degrees->back();
          out_degrees->pop_back();
          continue;
        } else if (source != target
                   || (allow_self_loops && (*out_degrees)[source].second > 2)) {
          break;
        }
      }

      // Update degree counts
      --(*out_degrees)[source].second;
      --degrees_left;
      --(*out_degrees)[target].second;
      --degrees_left;
      current.first  = (*out_degrees)[source].first;
      current.second = (*out_degrees)[target].first;
    }

    RandomGenerator* gen;
    std::size_t n;
    shared_ptr<out_degrees_t> out_degrees;
    std::size_t degrees_left;
    bool allow_self_loops;
    value_type current;
  };

  template<typename RandomGenerator, typename Graph>
  class plod_iterator
    : public mpl::if_<is_convertible<
                        typename graph_traits<Graph>::directed_category,
                        directed_tag>,
                      out_directed_plod_iterator<RandomGenerator>,
                      undirected_plod_iterator<RandomGenerator> >::type
  {
    typedef typename mpl::if_<
                       is_convertible<
                         typename graph_traits<Graph>::directed_category,
                         directed_tag>,
                        out_directed_plod_iterator<RandomGenerator>,
                        undirected_plod_iterator<RandomGenerator> >::type
      inherited;

  public:
    plod_iterator() : inherited() { }

    plod_iterator(RandomGenerator& gen, std::size_t n,
                  double alpha, double beta, bool allow_self_loops = false)
      : inherited(gen, n, alpha, beta, allow_self_loops) { }
  };

} // end namespace boost

#endif // BOOST_GRAPH_PLOD_GENERATOR_HPP


#include "common.hpp"

#include <cstdio>
#include <cstdlib>
#include <string>
#include <cassert>
#include <random>
#include <vector>
#include <utility>
#include <map>
#include <cmath>
#include <algorithm>

#define rep(i,n) for(int (i)=0;(i)<(n);(i)++)

// eigen
#include <Eigen/Core>

using namespace Eigen;
using namespace std;

constexpr int BUF_SIZE = 1000;

using p_t = pair<int, int>;
using r_t = vector<map<p_t, double> >;
using mat_t = MatrixXd;
using vec_t = VectorXd;
using vec_mat_t = vector<mat_t>;

std::mt19937_64 engine;//(seed_gen());

vector<int> seq(int n){
  vector<int> v(n);
  for(int i = 0; i < n; ++i){v[i] = i;}
  return v;
}

vector<int> random_index(int n){
  vector<int> res = seq(n);
  shuffle(res.begin(), res.end(), engine);
  return res;
}

void seeding(unsigned seed = 0){
  //std::random_device seed_gen;
  //engine = std::mt19937_64(seed_gen());
  engine = std::mt19937_64(seed);
}

string to_str(int x){
  char buf[BUF_SIZE];
  sprintf(buf, "%d", x);
  return buf;
}

mat_t load_mat(const string& infile){
  vector<vector<double> > mat;
  ifstream ifs(infile);
  string line;
  while(getline(ifs, line)){
    vector<double> row;
    for(const auto& field : split(line)){
      double x;
      sscanf(field.c_str(), "%lf", &x);
      row.push_back(x);
    }
    mat.push_back(row);
  }
  int m = mat.size();
  assert(m > 0);
  int n = mat[0].size();
  mat_t res = mat_t::Zero(m, n);
  for(int i = 0; i < m; ++i){
    assert(n == mat[i].size());
    for(int j = 0; j < n; ++j){
      res(i, j) = mat[i][j];
    }
  }
  return res;
}

mat_t ReadModelParameter(const string& prefix, int K, int ItrNum, const string& name){
  char buf[BUF_SIZE];
  sprintf(buf, "%s_Itr%d_%s.csv", prefix.c_str(), ItrNum, name.c_str());
  //read_file(buf);

  //mat_t tmp = load_mat(buf);
  //fprintf(stderr, "[%ld, %ld]\n", tmp.rows(), tmp.cols());

  return load_mat(buf);
}
/*
  ########################### Read model parameters #############################
  # [output1]: A (N x K matrix)
  # [output2]: B (M x K matrix)
  # [output3]: C (M x K matrix)
  # [output4]: D (T x K matrix)
*/
tuple<mat_t, mat_t, mat_t, mat_t> ReadModelParameters(const string& prefix, int K, int ItrNum){
  mat_t A = ReadModelParameter(prefix, K, ItrNum, "A");
  mat_t B = ReadModelParameter(prefix, K, ItrNum, "B");
  mat_t C = ReadModelParameter(prefix, K, ItrNum, "C");
  mat_t D = ReadModelParameter(prefix, K, ItrNum, "D");
  return forward_as_tuple(A, B, C, D);
}

/*
######################### Plausible deniability test ##########################
# [input1]: A (N x K matrix)
# [input2]: B (M x K matrix)
# [input3]: C (M x K matrix)
# [input4]: D (T x K matrix)
# [input5]: N -- Number of users
# [input6]: M -- Number of POIs
# [input7]: T -- Number of time periods
# [output1]: pass_test (N-dim vector)
# [output2]: loglikeli_train (N*TraceNum-dim vector)
# [output3]: loglikeli_verify (N*TraceNum x VUserNum matrix)
 */

using pass_test_t = vector<int>;

void PDTest_out1(const string& _outfile,
		 int N,
		 int TraceNum,
		 const pass_test_t& pass_test){
  //  string outfile = _outfile + "_";
  string outfile = _outfile;
  puts(outfile.c_str());
  FILE* fp = fopen(outfile.c_str(), "w");
  rep(user_index, N){
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
//      fprintf(fp, "%lf\n", static_cast<double>(pass_test[user_trace_no]));
      fprintf(fp, "%d,%d\n", pass_test[user_trace_no + N * TraceNum], pass_test[user_trace_no]);
    }
  }
  int pass_test_sum = 0;
  for(int i = 0; i < N * TraceNum; i++){
    pass_test_sum += pass_test[i];
  }
  fprintf(fp, "-,-,%d,%d,%lf\n", pass_test_sum, N * TraceNum, float(pass_test_sum) / float(N * TraceNum));
  fclose(fp);
}
void PDTest_out2(const string& _outfile,
		 int N,
		 int TraceNum,
		 int VUserNum,
		 const mat_t& loglikeli_train,
		 const mat_t& loglikeli_verify){
  //  string outfile = _outfile + "_";
  string outfile = _outfile;
  puts(outfile.c_str());
  FILE* fp = fopen(outfile.c_str(), "w");
  rep(user_index, N){
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
      fprintf(fp, "%lf", loglikeli_train(user_trace_no, 0));
      rep(n, VUserNum){
	fprintf(fp, ",%lf", loglikeli_verify(user_trace_no,n));
      }
      fprintf(fp, "\n");
    }
  }
  fclose(fp);
}

pass_test_t perform_pd_test(int N,
			    int TraceNum,
			    int VUserNum,
			    const vector<int>& verify_user,
			    const mat_t& loglikeli_train,
			    const mat_t& loglikeli_verify,
			    double ReqEps,
			    int Reqk){
  /*
  pass_test_t pass_test(N * TraceNum, 0);
  // # For each training user
  rep(user_index, N){
    // # For each trace
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
      // # Initialization
      int k = 0;
      // # For each verifying user
      rep(n, VUserNum){
	// # Continue if the training user is the same with the verifying user
	if(user_index == verify_user[n]){continue;}
	// # Increase k if the inequality holds with ReqEps
	if((loglikeli_train(user_trace_no, 0) >= loglikeli_verify(user_trace_no,n) - ReqEps) &&
	   (loglikeli_train(user_trace_no, 0) <= loglikeli_verify(user_trace_no,n) + ReqEps)){
	  ++k;
	}
	// # Break if k reaches Reqk
	if(k == Reqk){
	  pass_test[user_trace_no] = 1;
	  break;
	}
      }
    }
  }
  */
  pass_test_t pass_test(N * TraceNum * 2, 0);
  // # For each training user
  int i_tra, i_ver;
  rep(user_index, N){
    // # For each trace
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
      pass_test[user_trace_no + N * TraceNum] += 1;
      // # Find i s.t. [-(i+1)ReqEps < loglikeli_train[user_trace_no] <= -i ReqEps] --> i_tra
      i_tra = int(loglikeli_train(user_trace_no, 0) / ReqEps);
      // # For each verifying user
      rep(n, VUserNum){
	// # Continue if the training user is the same with the verifying user
	if(user_index == verify_user[n]){continue;}
	// # Find i s.t. [-(i+1)ReqEps < loglikeli_verify[user_trace_no,n] <= -i ReqEps] --> i_ver
	i_ver = int(loglikeli_verify(user_trace_no,n) / ReqEps);
	// # Increase k if the inequality holds with ReqEps (i_tra == i_ver)
	if(i_tra == i_ver){
	  pass_test[user_trace_no + N * TraceNum] += 1;
	}
	// # Pass test (and break (optional)) if k reaches Reqk
	if(pass_test[user_trace_no + N * TraceNum] == Reqk){
	  pass_test[user_trace_no] = 1;
//	  break;
	}
      }
    }
  }
  return pass_test;
}

int str2i(const string& s){
  int x;
  sscanf(s.c_str(), "%d", &x);
  return x;
}

double str2f(const string& s){
  double x;
  sscanf(s.c_str(), "%lf", &x);
  return x;
}

using trans_t = vector<vector<pair<int, int> > >;
using first_visit_t = vector<int>;

tuple<trans_t, first_visit_t, mat_t> read_syn_traces(const string& infile,
					     int TraceNum,
					     int T,
					     int TimInsNum,
					     int N){
  // init
  trans_t trans(N * TraceNum);
  first_visit_t first_visit(N * TraceNum, 0);
  mat_t loglikeli_train = mat_t::Zero(N * TraceNum, 1);
  int poi_index_prev = 0;
  //printf("%s\n", infile.c_str());
  ifstream ifs(infile);
  string line;
  getline(ifs, line); // header
  while(getline(ifs, line)){
    auto lst = split(line);
    int user_index = str2i(lst[0]);
    int trace_no = str2i(lst[1]);
    int time_slot = str2i(lst[2]);
    int time_ins = str2i(lst[3]);
    int poi_index = str2i(lst[4]);
    double ll = str2f(lst[6]);

    // # (user,trace)-pair no. --> user_trace_no
    int user_trace_no = user_index * TraceNum + trace_no;
    if(time_slot == 0 && time_ins == 0){
      // # Visited POI at the first time --> first_visit[user_trace_no]
      first_visit[user_trace_no] = poi_index;
    }else{
      // # Transition from the previous POI to the current POI --> trans[user_trace_no]
      trans[user_trace_no].push_back(make_pair(poi_index_prev,poi_index));
    }
    // # Log-likelihood for the training trace --> loglikeli_train[user_trace_no]
    if(time_slot == T-1 && time_ins == TimInsNum-1){
      loglikeli_train(user_trace_no, 0) = ll;
    }
    poi_index_prev = poi_index;
  }
  return forward_as_tuple(trans, first_visit, loglikeli_train);
}

mat_t calc_ll(int VUserNum,
	      const vector<int>& verify_user,
	      int T, int M,
	      double VisThr,
	      double VisitDelta,
	      const mat_t& A, // N * K
	      const mat_t& B, // M * K
	      const mat_t& C, // M * K
	      const mat_t& D, // T * K
	      double TransDelta,
	      int N,
	      int TraceNum,
	      int TimInsNum,
	      const first_visit_t& first_visit,
	      const trans_t& trans){
  mat_t loglikeli_verify = mat_t::Zero(N*TraceNum,VUserNum);
  rep(n, VUserNum){
    int vn = verify_user[n];
    printf("%d, %d\n", n, vn);
    // # Initialization
    mat_t time_poi_dist = mat_t::Zero(T, M);
    mat_t time_poi_dist_sum = mat_t::Zero(T, 1);
    mat_t prop_mat = mat_t::Zero(M, M);
    mat_t same_trans = mat_t::Zero(T, M); //np.full((T,M), -1.0);
    for(int i = 0; i < T; ++i)for(int j = 0; j < M; ++j){same_trans(i, j) = -1.0;}
    mat_t trans_vec = mat_t::Zero(M, 1);

    // ################### Calculate the POI distributions ###################
    puts("POI distribution");
    rep(t, T){
      mat_t ad = A.row(vn).array() * D.row(t).array();
      rep(i, M){
	// # Elements in a sampled visit tensor --> time_poi_dist
	time_poi_dist(t,i) = (ad.array() * B.row(i).array()).sum();
	// # Assign VisitDelta for an element whose value is less than VisThr
	if(time_poi_dist(t,i) < VisThr){
	  time_poi_dist(t,i) = VisitDelta;
	}
      }
    }
    // # Normalize time_poi_dist
    rep(t, T){
      time_poi_dist_sum(t) = time_poi_dist.row(t).sum();
      if(time_poi_dist_sum(t) > 0){
	time_poi_dist.row(t) /= time_poi_dist_sum(t);
      }else{
	printf("Error: All probabilities are 0 for user %d and time %d\n", n, t);
	exit(1);
      }
    }
    // #################### Calculate the proposal matrix ####################
    puts("Proposal matrix");
    rep(i, M){
      mat_t ab = A.row(vn).array() * B.row(i).array();
      // # Elements in a sampled transition tensor (assign TransDelta for a small transition count) --> prop_mat
      rep(j, M){
	prop_mat(i,j) = max((ab.array() * C.row(j).array()).sum(), TransDelta);
      }
      // # Normalize prop_mat
      double row_sum = prop_mat.row(i).sum();
      prop_mat.row(i) /= row_sum;
    }
    // #################### Calculate the log-likelihood #####################
    puts("Calculating the log-likelihood");
    // # For each training user
    rep(user_index, N){
      // # Continue if the training user is the same with the verifying user
      if(user_index == vn){continue;}
      // # For each trace
      rep(trace_no, TraceNum){
	int user_trace_no = user_index * TraceNum + trace_no;
	// # For each time slot
	rep(t, T){
	  // # For each time instant
	  rep(ins, TimInsNum){
	    if(t == 0 && ins == 0){
	      // # Add the log-likelihood for the first POI
	      int poi_index = first_visit[user_trace_no];
	      loglikeli_verify(user_trace_no, n) = log(time_poi_dist(0,poi_index));
	    }else{
	      // # Add the log-likelihood for the subsequent POIs
	      int tim = t * TimInsNum + ins - 1;
	      int poi_index_pre = trans[user_trace_no][tim].first;
	      int poi_index = trans[user_trace_no][tim].second;
	      double trans_prob;
	      // # If the current POI is different from the previous POI
	      if(poi_index_pre != poi_index){
		// # Calculate the transition probability --> trans_prob
		double alpha = (time_poi_dist(t, poi_index) * prop_mat(poi_index, poi_index_pre))
		  / (time_poi_dist(t, poi_index_pre) * prop_mat(poi_index_pre, poi_index));
		trans_prob = prop_mat(poi_index_pre,poi_index) * min(1.0, alpha);
	      // # If the self-transition probability for the POI at time slot t has been computed
	      }else if(same_trans(t,poi_index_pre) != -1.0){
		// # Use the self-transition probability --> trans_prob
		trans_prob = same_trans(t,poi_index_pre);
	      // # If the self-transition probability for the POI at time slot t has NOT been computed
	      }else{
		// # Compute the self-transition probability for the POI --> same_trans[t,poi_index_pre]
		trans_vec(poi_index_pre, 0) = 0;
		rep(j, M){
		  if(poi_index_pre != j){
		    double alpha = (time_poi_dist(t, j) * prop_mat(j, poi_index_pre))
		      / (time_poi_dist(t, poi_index_pre) * prop_mat(poi_index_pre,j));
		    trans_vec(j, 0) = prop_mat(poi_index_pre,j) * min(1.0, alpha);
		  }
		}
		double row_sum = trans_vec.sum();
		same_trans(t,poi_index_pre) = 1 - row_sum;
		// # Use the self-transition probability --> trans_prob
		trans_prob = same_trans(t,poi_index_pre);
	      }
	      // # Add the log of the transition probability
	      loglikeli_verify(user_trace_no,n) += log(trans_prob);
	      //
	    }
	    //
	  }
	}
      }
    }
    //
  }
  return loglikeli_verify;
}

void PDTest(mat_t& A,
	    mat_t& B,
	    mat_t& C,
	    mat_t& D,
	    int N,
	    int M,
	    int T,
	    int VUserNum,
	    const string& PDTestResFile,
	    int K,
	    int ItrNum,
	    int TraceNum,
	    double ReqEps,
	    int Reqk,
	    const string& SynTraceFile,
	    int TimInsNum,
	    double VisThr, double VisitDelta, double TransDelta){
  // # Initialization

  // # Randomly assign verifying users --> verify_user
  auto verify_user = random_index(VUserNum);

  // # Read synthesized traces --> trans, first_visit, loglikeli_train
  trans_t trans;
  first_visit_t first_visit;
  mat_t loglikeli_train;
  tie(trans, first_visit, loglikeli_train) =
    read_syn_traces(SynTraceFile + "_Itr" + to_str(ItrNum) + ".csv",
		    TraceNum, T, TimInsNum, N);

  // # Calculate the log-likelihood for each verifying user --> loglikeli_verify
  mat_t loglikeli_verify = calc_ll(VUserNum, verify_user, T, M, VisThr, VisitDelta, A, B, C, D, TransDelta, N, TraceNum, TimInsNum, first_visit, trans);

  // # Perform the PD test --> pass_test
  pass_test_t pass_test = perform_pd_test(N,TraceNum, VUserNum,
				    verify_user, loglikeli_train, loglikeli_verify,
				    ReqEps, Reqk);

  // # Output the PD test results (pass_test)
  PDTest_out1(PDTestResFile + "_Itr" + to_str(ItrNum) + ".csv",
	      N, TraceNum, pass_test);
  /*
  // # Output the PD test results (loglikeli_train, loglikeli_verify)
  PDTest_out2(PDTestResFile + "_Itr" + to_str(ItrNum) + "_loglikeli.csv",
	      N, TraceNum, VUserNum,
	      loglikeli_train, loglikeli_verify);
  */
}

int main(int argc, char* argv[]){
  if(argc < 3){
    fprintf(stderr, "Usage: %s [Dataset] [City] ([ReqEps (default:1.0)] [Reqk (default:10)] [VUserNum (default:-1)] [TraceNum (default:10)] [Alp (default:200)] [MaxNumTrans (default:100)] [MaxNumVisit (default:100)] [ItrNum (default:100)])\n", argv[0]);
    exit(1);
  }
  // # Dataset (PF/FS)
  string DataSet = argv[1];
  // # City
  string City = argv[2];

  // # Required epsilon in plausible deniability
  double ReqEps = 1.0;
  if(argc >= 4){
    sscanf(argv[3], "%lf", &ReqEps);
  }

  // # Required k in plausible deniability
  int Reqk = 10;
  if(argc >= 5){
    sscanf(argv[4], "%d", &Reqk);
  }

  // # Number of users for verifying PD (-1: all)
  int VUserNum = -1;
  if(argc >= 6){
    sscanf(argv[5], "%d", &VUserNum);
  }

  // # Number of traces per user
  int TraceNum = 10;
  if(argc >= 7){
    sscanf(argv[6], "%d", &TraceNum);
  }

  // # Hyper-hyper parameter alpha
  double Alp = 200;
  string AlpStr = "200";
  if(argc >= 8){
    sscanf(argv[7], "%lf", &Alp);
    AlpStr = argv[7];
  }

  // # Maximum number of transitions per user (-1: infinity)
  int MaxNumTrans = 100;
  if(argc >= 9){
    sscanf(argv[8], "%d", &MaxNumTrans);
  }

  // # Maximum number of POI visits per user (-1: infinity)
  int MaxNumVisit = 100;
  if(argc >= 10){
    sscanf(argv[9], "%d", &MaxNumVisit);
  }

  // # Number of iterations in Gibbs sampling
  int ItrNum = 100;
  if(argc >= 11){
    sscanf(argv[10], "%d", &ItrNum);
  }

  // # Data directory
  string DataDir = "../data/" + DataSet + "/";

  // # Training user index file (input)
  string TUserIndexFile = DataDir + "tuserindex_%s.csv";
  // # POI index file (input)
  string POIIndexFile = DataDir + "POIindex_%s.csv";

  // # Model parameter directory
//  string ModelParameterDir = DataDir + "PPMTF_" + City + "_alp" + AlpStr + "_mnt" + to_str(MaxNumTrans) + "_mnv" + to_str(MaxNumVisit) + "_py/";
  string ModelParameterDir = DataDir + "PPMTF_" + City + "_alp" + AlpStr + "_mnt" + to_str(MaxNumTrans) + "_mnv" + to_str(MaxNumVisit) + "/";
  // # Prefix of the model parameter file (input)
  string ModelParameterFile = ModelParameterDir + "modelparameter";
  // # Prefix of the synthesized trace file (input)
  string SynTraceFile = ModelParameterDir + "syntraces";

  // # Prefix of the PD test result file (output)
  string PDTestResFile = ModelParameterDir + "pdtest_res";

  // # Name of the model parameter A
  //string ParamA = "A";

  // # Number of time slots
  int T = -1;
  if(DataSet.find("PF") == 0){T = 30;}
  if(DataSet.find("FS") == 0){T = 12;}
  if(T == -1){
    fprintf(stderr, "Wrong Dataset\n");
    exit(1);
  }

  // # Number of time instants per time slot
  int TimInsNum = -1;
  if(DataSet.find("PF") == 0){TimInsNum = 1;}
  if(DataSet.find("FS") == 0){TimInsNum = 2;}
  assert(TimInsNum >= 0);

  // # Number of columns in model parameters (A, B, C)
  int K = 16;
  // # Threshold for a visit count
  double VisThr = 0;
  // # Minimum value of a visit count
  double VisitDelta = 0.00000001;
  // # Minimum value of a transition count
  double TransDelta = 0.00000001;

  // #################################### Main #####################################
  // # Fix a seed
  seeding(1);

  // # Replace XX with City
  TUserIndexFile = string_replace(TUserIndexFile, City);
  POIIndexFile = string_replace(POIIndexFile, City);

  // # Number of training users --> N
  int N = line_num(TUserIndexFile) - 1;
  // # Number of POIs --> M
  int M = line_num(POIIndexFile) - 1;

  // # Number of users for verifying PD (-1: all)
  if(VUserNum == -1){VUserNum = N-1;}

  // # Read model parameters
  mat_t A, B, C, D;
  tie(A, B, C, D) = ReadModelParameters(ModelParameterFile, K, ItrNum);

  // # Plausible deniability test
  PDTest(A, B, C, D, N, M, T,
	 VUserNum, PDTestResFile,
	 K, ItrNum, TraceNum,
	 ReqEps, Reqk,
	 SynTraceFile,
	 TimInsNum,
	 VisThr, VisitDelta, TransDelta);
  //////////////////pass_test, loglikeli_train, loglikeli_verify = PDTest(A, B, C, D, N, M, T)

//  printf("%s\n", DataSet.c_str());
//  printf("%s\n", City.c_str());
  return 0;
}


//
// Created by david on 2018-10-29.
//

#include "matrix_recast.h"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iterator>

/*! \brief Prints the content of a vector nicely */
//template<typename Scalar_>
//std::ostream &operator<<(std::ostream &out, const std::vector<Scalar_> &v) {
//    if (!v.empty()) {
////        out << "[ ";
//        std::copy(v.begin(), v.end(), std::ostream_iterator<Scalar_>(out, "  "));
//        out << '\n';
//    }
//    return out;
//}

template<typename Scalar>
matrix_recast<Scalar>::matrix_recast(const Scalar *matrix_ptr_, int L_):matrix_ptr(matrix_ptr_), L(L_){
    recheck_all();
}

template<typename Scalar>
void matrix_recast<Scalar>::prune(double threshold){
    matrix_pruned.resize(L*L);
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matmap   (matrix_ptr,L,L);
    Eigen::Map<Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matpruned(matrix_pruned.data(),L,L);
    matpruned = (matmap.array().cwiseAbs() < threshold).select(Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>::Zero(L,L), matmap);
    recheck_all();
    pruned = true;
}

template<typename Scalar>
void matrix_recast<Scalar>:: recheck_all(){
    check_if_sparse();
    check_if_hermitian();
    check_if_real();
}

template<typename Scalar>
void matrix_recast<Scalar>::check_if_real() {
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matrix (matrix_ptr,L,L);
    if constexpr (std::is_same<Scalar, double>::value){isReal = true;}
    else {isReal = matrix.imag().isZero(1e-14);}
}

template<typename Scalar>
void matrix_recast<Scalar>::check_if_hermitian() {
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matrix (matrix_ptr,L,L);
    isHermitian = matrix.isApprox(matrix.adjoint(), 1e-14);
}

template<typename Scalar>
void matrix_recast<Scalar>::check_if_sparse() {
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matrix (matrix_ptr,L,L);
    sparcity = (matrix.array().cwiseAbs() > 1e-14 )
                       .select(Eigen::MatrixXd::Ones(L,L),0).sum() / static_cast<double>(matrix.size());
    isSparse =  sparcity < 0.1;
}

template<typename Scalar>
DenseMatrixProduct<double> matrix_recast<Scalar>::get_as_real_dense() {
    if constexpr(std::is_same<Scalar, double>::value){
        if (pruned){return DenseMatrixProduct<double>(matrix_pruned.data(),L,true);}
        else       {return DenseMatrixProduct<double>(matrix_ptr,L,true);}
    }else{
//        assert(is_real and "ERROR: The given matrix has a nonzero imaginary part. Can't convert to real.");
        if (not isReal){
            double sum = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).imag().cwiseAbs().sum();
            std::cerr << "WARNING: The given matrix has a nonzero imaginary part, yet converting to real. Imag sum: " << sum << std::endl;
        }        Eigen::MatrixXd matrix_recast;
        if (pruned){matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L).real();}
        else       {matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).real();}
        return DenseMatrixProduct<double>(matrix_recast.data(),L,true);

    }
}

template<typename Scalar>
DenseMatrixProduct<std::complex<double>> matrix_recast<Scalar>::get_as_cplx_dense() {
    if constexpr(std::is_same<Scalar, double>::value){
        Eigen::MatrixXcd matrix_recast(L,L);
        matrix_recast.setZero();
        if (pruned){matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L);}
        else       {matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L);}
        return DenseMatrixProduct<std::complex<double>>(matrix_recast.data(),L,true);
    }else{
        return DenseMatrixProduct<std::complex<double>>(matrix_ptr,L,true);
    }
}

template<typename Scalar>
SparseMatrixProduct<double> matrix_recast<Scalar>::get_as_real_sparse() {
    if constexpr(std::is_same<Scalar, double>::value){
        if(pruned){return SparseMatrixProduct<double>(matrix_pruned.data(),L,true);}
        else      {return SparseMatrixProduct<double>(matrix_ptr,L,true);}
    }else{
        if (not isReal){
            double sum = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).imag().cwiseAbs().sum();
            std::cerr << "WARNING: The given matrix has a nonzero imaginary part, yet converting to real. Imag sum: " << sum << std::endl;
        }
        Eigen::MatrixXd matrix_recast;
        if(pruned){matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L).real();}
        else      {matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).real();}
        return SparseMatrixProduct<double>(matrix_recast.data(),L,true);
    }
}

template<typename Scalar>
SparseMatrixProduct<std::complex<double>> matrix_recast<Scalar>::get_as_cplx_sparse() {
    if constexpr(std::is_same<Scalar, double>::value){
        Eigen::MatrixXcd matrix_recast(L,L);
        matrix_recast.setZero();
        if(pruned){matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L);}
        else      {matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L);}
        return SparseMatrixProduct<std::complex<double>>(matrix_recast.data(),L,true);
    }else{
        if(pruned){return SparseMatrixProduct<std::complex<double>>(matrix_pruned.data(),L,true);}
        else      {return SparseMatrixProduct<std::complex<double>>(matrix_ptr,L,true);}
    }
}

//
//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_real_dense(){
//    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html
//
//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
////    matrix_real_sparse.clear();
////    matrix_cplx_sparse.clear();
//
//
////    matrix_real_dense.L    = L;
////    matrix_real_dense.N    = L*L;
//    if constexpr(std::is_same<Scalar, double>::value){
//        matrix_real_dense = DenseMatrixProduct<double>(matrix_ptr,L);
//    }else{
//        Eigen::MatrixXd matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).real();
//        matrix_real_dense = DenseMatrixProduct<double>(matrix_recast.data(),L);
//    }
//
//}
//
//
//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_cplx_dense(){
//    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html
//
//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
////    matrix_real_sparse.clear();
////    matrix_cplx_sparse.clear();
//
//
////    matrix_cplx_dense.L    = L;
////    matrix_cplx_dense.N    = L*L;
//    if constexpr(std::is_same<Scalar, double>::value){
//        Eigen::MatrixXcd matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L);
//        matrix_cplx_dense =  DenseMatrixProduct<std::complex<double>>(matrix_recast.data(),L);
//    }else{
//        matrix_cplx_dense = DenseMatrixProduct<std::complex<double>>(matrix_ptr,L);
//    }
//}

//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_real_sparse(){
    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html

//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
//    matrix_real_sparse.clear();
//    matrix_cplx_sparse.clear();
//
//    assert(isReal and "Matrix is not real!");
//    Eigen::SparseMatrix<double> matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>>(matrix_ptr,L,L).real().sparseView();
//    matrix_recast.makeCompressed();
//    matrix_real_sparse.nnz = matrix_recast.nonZeros();
//    matrix_real_sparse.L   = L;
//    matrix_real_sparse.N   = L*L;
//    matrix_real_sparse.irow = std::vector<int>   (matrix_recast.innerIndexPtr(), matrix_recast.innerIndexPtr() + matrix_recast.nonZeros() );
//    matrix_real_sparse.pcol = std::vector<int>   (matrix_recast.outerIndexPtr(), matrix_recast.outerIndexPtr() + matrix_recast.outerSize() +1);
//    matrix_real_sparse.vals = std::vector<double>(matrix_recast.valuePtr()     , matrix_recast.valuePtr()      + matrix_recast.nonZeros());
//

//}

//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_cplx_sparse(){
    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html

//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
//    matrix_real_sparse.clear();
//    matrix_cplx_sparse.clear();
//    assert(not isReal and "Matrix is not cplx!");
//    Eigen::SparseMatrix<std::complex<double>> matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>>(matrix_ptr,L,L).template cast<std::complex<double>>().sparseView();
//    matrix_recast.makeCompressed();
//    matrix_recast.finalize();
//    std::cout << "Matrix compressed: \n" << matrix_recast << std::endl;
//
//    matrix_cplx_sparse.nnz = matrix_recast.nonZeros();
//    matrix_cplx_sparse.L   = L;
//    matrix_cplx_sparse.N   = L*L;
//    matrix_cplx_sparse.irow = std::vector<int>   (matrix_recast.innerIndexPtr(), matrix_recast.innerIndexPtr() + matrix_recast.nonZeros() );
//    matrix_cplx_sparse.pcol = std::vector<int>   (matrix_recast.outerIndexPtr(), matrix_recast.outerIndexPtr() + matrix_recast.outerSize() +1);
//    matrix_cplx_sparse.vals = std::vector<std::complex<double>>(matrix_recast.valuePtr()       , matrix_recast.valuePtr()       + matrix_recast.nonZeros());
//
//    std::cout << "nonzeros : " << matrix_recast.nonZeros() << std::endl;
//    std::cout << "innerSize: " << matrix_recast.innerSize() << std::endl;
//    std::cout << "outerSize: " << matrix_recast.outerSize() << std::endl;
//    std::cout << "irow \n" << matrix_cplx_sparse.irow << std::endl;
//    std::cout << "pcol \n" << matrix_cplx_sparse.pcol << std::endl;
//    std::cout << "vals \n" << matrix_cplx_sparse.vals << std::endl;
//    auto innernnz = std::vector<int>   (matrix_recast.innerNonZeroPtr(), matrix_recast.innerNonZeroPtr() + matrix_recast.innerSize() );
//    auto innernnz = std::vector<int>   (matrix_recast.innerNonZeroPtr(), matrix_recast.innerIndexPtr() + matrix_recast.innerSize() );
//    std::cout << "innernnz: \n" << innernnz << std::endl;

//}

template class matrix_recast<double>;
template class matrix_recast<std::complex<double>>;


#include <iostream>
#include <vector>
#include <list>
#include <set>
#include <tuple>
#include <unordered_map>
#include <cstdio>
#include <utility>
#include <cstdlib>
#include <map>
#include <cmath>
#include <ctgmath>
#include <cstring>
#include <functional>
#include <cassert>
#include <algorithm>
#include <unordered_map>
#include <fstream>
#include <stdio.h>

#include <boost/algorithm/string.hpp>
#include <Eigen/Dense>
#include "parse_args.hh"
#include "omp.h"

#define KNRM  "\x1B[0m"
#define KRED  "\x1B[31m"
#define KGRN  "\x1B[32m"
#define KYEL  "\x1B[33m"
#define KBLU  "\x1B[34m"
#define KMAG  "\x1B[35m"
#define KCYN  "\x1B[36m"
#define KWHT  "\x1B[37m"

using namespace std;
using namespace Eigen;

int D; // dimensionality

unordered_map<string, VectorXd> w; // target word vectors
unordered_map<string, VectorXd> c; // context word vectors
unordered_map<string, double> bw; // target word biases
unordered_map<string, double> bc; // context word biases

unordered_map<string, VectorXd> grad_w; // Squared gradient for AdaGrad
unordered_map<string, VectorXd> grad_c;
unordered_map<string, double> grad_bw;
unordered_map<string, double> grad_bc;

struct edge {
  string w, c;
  double value;
};
vector<edge> edges;

set<string> words;
set<string> contexts;

unordered_map<string, double> pairs;

void read_edges(string fname, vector<edge> &train_data){
    ifstream train_file(fname.c_str());
    string first, second;
    double value;
    while (train_file >> first >> second >> value){
        edge e;
        e.w = first;
        e.c = second;
        e.value = value;
        //cout << e.w << "\t" << e.c << "\t" << e.value << endl;
        words.insert(first);
        contexts.insert(second);
        train_data.push_back(e);
    }
    train_file.close();
    // allocate memory for all vectors
}

void load_word_vectors(string vects_fname){
    // Initialize word vectors using pre-trained word vectors
    fprintf(stderr, "%sDimensionality of the word vectors = %d%s\n", KRED, D, KNRM);
    fprintf(stderr, "%sReading pre-trained vectors from %s%s\n", KRED, vects_fname.c_str(), KNRM);
    unordered_map<string, VectorXd> x;
    set<string> prewords;

    FILE *fp= fopen(vects_fname.c_str(), "r");
    int count = 0;
    for (char buf[262144]; fgets(buf, sizeof(buf), fp); ) {
        char curword[1024];
        sscanf(buf, "%s %[^\n]", curword, buf);
        string wstr(curword);
        if ((words.find(wstr) != words.end()) || (contexts.find(wstr) != contexts.end())){
            x[wstr] = VectorXd::Zero(D);
            count = 0;
            while (1) {
                double fval;
                bool end = (sscanf(buf, "%lf %[^\n]", &fval, buf) != 2);
                x[wstr][count] = fval;
                count += 1;
            if (end) break;
            }
            prewords.insert(wstr);
            assert(count == D);
        }
    }
    // Copy the vectors from x to w and c.
    for (auto y = x.begin(); y != x.end(); ++y){
        if (words.find(y->first) != words.end())
            w[y->first] = y->second;
        if (contexts.find(y->first) != contexts.end())
            c[y->first] = y->second;
    }
    fclose(fp);
}

void read_pairs(string pairs_fname){
    fprintf(stderr, "%sReading relational pairs from = %s%s\n", KRED, pairs_fname.c_str(), KNRM);
    ifstream pairs_file(pairs_fname.c_str());
    string first_word, second_word;
    double value;

    while (pairs_file >> first_word >> second_word >> value){
        //cout << first_word << "\t" << second_word << "\t" << value << "\t" << (2 * value) << endl;
        pairs[first_word + "<+>" + second_word] = value;
    }

    /*
    while (pairs_file >> first_word >> second_word){
        //cout << first_word << "\t" << second_word << "\t" << value << endl;
        pairs[first_word + "<+>" + second_word] = 1.0;
    }
    */

    pairs_file.close();
}

void centralize(unordered_map<string, VectorXd> &x){
    VectorXd mean = VectorXd::Zero(D);
    VectorXd squared_mean = VectorXd::Zero(D);
    for (auto w = x.begin(); w != x.end(); ++w){
        mean += w->second;
        squared_mean += (w->second).cwiseProduct(w->second);
    }
    mean = mean / ((double) x.size());
    VectorXd sd = squared_mean - mean.cwiseProduct(mean);
    for (int i = 0; i < D; ++i){
        sd[i] = sqrt(sd[i]);
    }
    for (auto w = x.begin(); w != x.end(); ++w){
        VectorXd tmp = VectorXd::Zero(D);
        for (int i = 0; i < D; ++i){
            tmp[i] = (w->second)[i] - mean[i];
            if (sd[i] != 0)
                tmp[i] /= sd[i];
        }
        w->second = tmp;
    }
}

void initialize(){
    int count_words = 0;
    for(auto e = words.begin(); e != words.end(); ++e){
        count_words++;
        w[*e] = VectorXd::Random(D);
        bw[*e] = 0;
        grad_w[*e] = VectorXd::Zero(D);
        grad_bw[*e] = 0;
    }

    int count_contexts = 0;
    for(auto e = contexts.begin(); e != contexts.end(); ++e){
        count_contexts++;
        c[*e] = VectorXd::Random(D);
        bc[*e] = 0;
        grad_c[*e] = VectorXd::Zero(D);
        grad_bc[*e] = 0;
    }

    centralize(w);
    centralize(c);
    fprintf(stderr, "%sInitialization Completed...\n%s", KYEL, KNRM);
}

double f(size_t x){
    if (x < 100)
        return pow((x / 100.0), 0.75);
    else
        return 1.0;
}

void train(int epohs, double alpha, double lambda){
    fprintf(stderr, "%s\nTotal ephos to train = %d\n%s", KGRN, epohs, KNRM);
    fprintf(stderr, "%sInitial learning rate = %f\n%s", KGRN, alpha, KNRM);
    fprintf(stderr, "%slambda = %f\n%s", KGRN, lambda, KNRM);
    fprintf(stderr, "%sDim = %d\n%s", KGRN, D, KNRM);

    double total_loss, cost;
    VectorXd gw = VectorXd::Zero(D);
    VectorXd gc = VectorXd::Zero(D);
    VectorXd diff = VectorXd::Zero(D);

    VectorXd one_vect = VectorXd::Zero(D);
    for (auto i = 0; i < D; ++i)
        one_vect[i] = 1.0;

    int found_pairs = 0;

    for (int t = 0; t < epohs; ++t){
        total_loss = 0;
        found_pairs = 0;
        for(auto e = edges.begin(); e != edges.end(); ++e){
            cost = w[e->w].dot(c[e->c]) + bw[e->w] + bc[e->c] - log(e->value);
            total_loss += f(e->value) * cost * cost;

            cost *= f(e->value);
            gw = cost * c[e->c];
            gc = cost * w[e->w];

            string pair_key = e->w + "<+>" + e->c;
            if (pairs.find(pair_key) != pairs.end()){
                diff = lambda * pairs[pair_key] * (w[e->w] - c[e->c]);
                gw += diff;
                gc -= diff;
                found_pairs++;
                }

            grad_w[e->w] += gw.cwiseProduct(gw);
            grad_c[e->c] += gc.cwiseProduct(gc);
            grad_bw[e->w] += cost * cost;
            grad_bc[e->c] += cost * cost;

            w[e->w] -= alpha * gw.cwiseProduct((grad_w[e->w] + one_vect).cwiseInverse().cwiseSqrt());
            c[e->c] -= alpha * gc.cwiseProduct((grad_c[e->c] + one_vect).cwiseInverse().cwiseSqrt());

            bw[e->w] -= (alpha * cost) / sqrt(1.0 + grad_bw[e->w]);
            bc[e->c] -= (alpha * cost) / sqrt(1.0 + grad_bc[e->c]);
        }
        fprintf(stderr, "Itr = %d, Loss = %f, foundPairs = %d\n", t, (sqrt(total_loss) / edges.size()), found_pairs);
    }
}

void write_line(ofstream &reps_file, VectorXd vec, string label){
    reps_file << label + " ";
    for (int i = 0; i < D; ++i)
        reps_file << vec[i] << " ";
    reps_file << endl;
}

void save_model(string fname){
    ofstream reps_file;
    reps_file.open(fname);
    if (!reps_file){
        fprintf(stderr, "%sFailed to write reps to %s\n%s", KRED, KNRM, fname.c_str());
        exit(1);
    }
    for (auto x = words.begin(); x != words.end(); ++x){
        if (contexts.find(*x) != contexts.end())
             write_line(reps_file, 0.5 * (w[*x] + c[*x]), *x);
        else
            write_line(reps_file, w[*x], *x);
    }

    for (auto x = contexts.begin(); x != contexts.end(); ++x){
        if (words.find(*x) == words.end())
            write_line(reps_file, c[*x], *x);
    }
    reps_file.close();
}

int main(int argc, char *argv[]){
    int no_threads = 100;
    omp_set_num_threads(no_threads);
    setNbThreads(no_threads);
    initParallel();

    if (argc == 1) {
        fprintf(stderr, "usage: ./reps --dim=dimensionality --model=model_fname \
                                --alpha=alpha --ephos=rounds --lmda=lambda --edges=edges_fname \
                                 --pretrain=pretrained_word_vectors_file (if any) \
                                 --pairs=pairs_file_name \n");
        return 0;
    }
    parse_args::init(argc, argv);
    string edges_fname = parse_args::get<string>("--edges");
    string pretrain = parse_args::get<string>("--pretrain");
    string pairs_fname = parse_args::get<string>("--pairs");

    D = parse_args::get<int>("--dim");
    int epohs = parse_args::get<int>("--epohs");
    double alpha = parse_args::get<double>("--alpha");
    string model = parse_args::get<string>("--model");
    double lambda = parse_args::get<double>("--lmda");

    read_edges(edges_fname, edges);
    fprintf(stderr, "%sTotal no. of target train instances = %d\n%s", KGRN, (int) edges.size(), KNRM);
    fprintf(stderr, "%sTotal no of words = %d\n%s", KGRN, (int) words.size(), KNRM);
    fprintf(stderr, "%sTotal no. of contexts = %d\n%s", KGRN, (int) contexts.size(), KNRM);
    read_pairs(pairs_fname);
    //test_code();
    initialize();
    if (pretrain.length() > 0){
        load_word_vectors(pretrain);
    }
    train(epohs, alpha, lambda);
    save_model(model);
    return 0;

}


/*
 boost header: numeric/odeint/gram_schmitt.hpp

 Copyright 2011-2013 Karsten Ahnert
 Copyright 2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_GRAM_SCHMITT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_GRAM_SCHMITT_HPP_INCLUDED

#include <boost/throw_exception.hpp>
#include <iterator>
#include <algorithm>
#include <numeric>

namespace boost {
namespace numeric {
namespace odeint {

template< class Iterator , class T >
void normalize( Iterator first , Iterator last , T norm )
{
    while( first != last ) *first++ /= norm;
}

template< class Iterator , class T >
void substract_vector( Iterator first1 , Iterator last1 ,
        Iterator first2 , T val )
{
    while( first1 != last1 ) *first1++ -= val * ( *first2++ );
}

template< size_t num_of_lyap , class StateType , class LyapType >
void gram_schmidt( StateType &x , LyapType &lyap , size_t n )
{
    if( !num_of_lyap ) return;
    if( ptrdiff_t( ( num_of_lyap + 1 ) * n ) != std::distance( x.begin() , x.end() ) )
        BOOST_THROW_EXCEPTION( std::domain_error( "renormalization() : size of state does not match the number of lyapunov exponents." ) );

    typedef typename StateType::value_type value_type;
    typedef typename StateType::iterator iterator;

    value_type norm[num_of_lyap];
    value_type tmp[num_of_lyap];
    iterator first = x.begin() + n;
    iterator beg1 = first , end1 = first + n ;

    std::fill( norm , norm+num_of_lyap , 0.0 );

    // normalize first vector
    norm[0] = sqrt( std::inner_product( beg1 , end1 , beg1 , 0.0 ) );
    normalize( beg1 , end1 , norm[0] );

    beg1 += n;
    end1 += n;

    for( size_t j=1 ; j<num_of_lyap ; ++j , beg1+=n , end1+=n )
    {
        for( size_t k=0 ; k<j ; ++k )
        {
            tmp[k] = std::inner_product( beg1 , end1 , first + k*n , 0.0 );
            //  clog << j << " " << k << " " << tmp[k] << "\n";
        }

        for( size_t k=0 ; k<j ; ++k )
            substract_vector( beg1 , end1 , first + k*n , tmp[k] );

        // normalize j-th vector
        norm[j] = sqrt( std::inner_product( beg1 , end1 , beg1 , 0.0 ) );
        // clog << j << " " << norm[j] << "\n";
        normalize( beg1 , end1 , norm[j] );
    }

    for( size_t j=0 ; j<num_of_lyap ; j++ )
        lyap[j] += log( norm[j] );
}

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif //BOOST_NUMERIC_ODEINT_GRAM_SCHMITT_HPP_INCLUDED


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"
#include "perspective_camera_plucker_line_tait_bryan_wc_jacobian.h"
#include "perspective_camera_tait_bryan_wc_jacobian.h"
#include "perspective_camera_plucker_line_rodrigues_wc_jacobian.h"
#include "perspective_camera_plucker_line_quaternion_wc_jacobian.h"
#include "quaternion_constraint_jacobian.h"

struct LinePoint{
	double u;
	double v;
	int index_to_line;
};

struct Camera{
	Eigen::Affine3d pose;
	std::vector<std::pair<LinePoint, LinePoint>> uv;
};

typedef Eigen::Matrix<double, 6, 1> PLine;

std::vector<Camera> cameras;
PerspectiveCameraParams cam_params;
std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> lines;

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = -215, rotate_y = 273;
float translate_z = -68.0;
float translate_x = 4, translate_y = 30.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();
PLine get_plucker_line(const Eigen::Vector3d &from, const Eigen::Vector3d &to);

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

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	cam_params.fx = 2000;
	cam_params.fy = 1000;
	cam_params.cx = 1000;
	cam_params.cy = 500;

	for(int i = -50 ; i < 50; i+=20){
		Camera c;
		c.pose = Eigen::Affine3d::Identity();
		c.pose(0,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		c.pose(1,3) = i;
		c.pose(2,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		cameras.push_back(c);
	}

	for(float x = 0; x <= 100; x += 5){
		Eigen::Vector3d a(x, -50.0, 400);
		Eigen::Vector3d b(x,  50.0, 400);
		lines.emplace_back(a,b);
	}
	for(float y = -50; y <= 50; y += 5){
		Eigen::Vector3d a(0, y, 400);
		Eigen::Vector3d b(100.0, y, 400);
		lines.emplace_back(a,b);
	}

	for(float x = -100; x <= 0; x += 5){
		Eigen::Vector3d a(x, -50.0, 440);
		Eigen::Vector3d b(x,  50.0, 440);
		lines.emplace_back(a,b);
	}
	for(float y = -50; y <= 50; y += 5){
		Eigen::Vector3d a(-100, y, 440);
		Eigen::Vector3d b(0.0, y, 440);
		lines.emplace_back(a,b);
	}

	for(size_t i = 0; i < cameras.size(); i++){
		for(size_t j = 0 ; j < lines.size(); j++){
			std::pair<LinePoint, LinePoint> uv;
			LinePoint kp;
			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);

			projection_perspective_camera_tait_bryan_wc(kp.u, kp.v, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, lines[j].first.x(), lines[j].first.y(), lines[j].first.z());

			uv.first.u = kp.u;
			uv.first.v = kp.v;
			uv.first.index_to_line = j;

			projection_perspective_camera_tait_bryan_wc(kp.u, kp.v, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, lines[j].second.x(), lines[j].second.y(), lines[j].second.z());

			uv.second.u = kp.u;
			uv.second.v = kp.v;
			uv.second.index_to_line = j;

			cameras[i].uv.push_back(uv);
		}
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("perspective_camera_external_orientation_plucker_line");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	for(size_t i = 0 ; i < cameras.size(); i++){
		Eigen::Affine3d m = cameras[i].pose;

		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

			glColor3f(0.0f, 1.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

			glColor3f(0.0f, 0.0f, 1.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
		glEnd();
	}

	glColor3f(0,1,0);
	glBegin(GL_LINES);
		for(size_t i = 0; i < lines.size(); i++){
			glVertex3f(lines[i].first.x(), lines[i].first.y(), lines[i].first.z());
			glVertex3f(lines[i].second.x(), lines[i].second.y(), lines[i].second.z());
		}
	glEnd();

	glColor3f(0.8,0.8,0.8);
	glBegin(GL_LINE_STRIP);
	for(size_t i = 0 ; i < cameras.size(); i++){
		Eigen::Affine3d m = cameras[i].pose;
		glVertex3f(m(0,3), m(1,3), m(2,3));
	}
	glEnd();

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'c':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose pose;
				pose.px = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.py = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.pz = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.om = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.fi = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.ka = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;

				Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose);
				cameras[i].pose = cameras[i].pose * m;
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].uv.size(); j++){

					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);

					PLine pl = get_plucker_line(lines[cameras[i].uv[j].first.index_to_line].first, lines[cameras[i].uv[j].first.index_to_line].second);
					Eigen::Matrix<double, 2, 1> delta;
					observation_equation_perspective_camera_plucker_line_tait_bryan_wc(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					Eigen::Matrix<double, 2, 6, Eigen::RowMajor> jacobian;
					observation_equation_perspective_camera_plucker_line_tait_bryan_wc_jacobian(jacobian, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					int ir = tripletListB.size();
					int ic_camera = i * 6;
					tripletListA.emplace_back(ir     , ic_camera    , -jacobian(0,0));
					tripletListA.emplace_back(ir     , ic_camera + 1, -jacobian(0,1));
					tripletListA.emplace_back(ir     , ic_camera + 2, -jacobian(0,2));
					tripletListA.emplace_back(ir     , ic_camera + 3, -jacobian(0,3));
					tripletListA.emplace_back(ir     , ic_camera + 4, -jacobian(0,4));
					tripletListA.emplace_back(ir     , ic_camera + 5, -jacobian(0,5));

					tripletListA.emplace_back(ir + 1 , ic_camera    , -jacobian(1,0));
					tripletListA.emplace_back(ir + 1 , ic_camera + 1, -jacobian(1,1));
					tripletListA.emplace_back(ir + 1 , ic_camera + 2, -jacobian(1,2));
					tripletListA.emplace_back(ir + 1 , ic_camera + 3, -jacobian(1,3));
					tripletListA.emplace_back(ir + 1 , ic_camera + 4, -jacobian(1,4));
					tripletListA.emplace_back(ir + 1 , ic_camera + 5, -jacobian(1,5));

					tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));
					tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta(1,0), 1));

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 6, cameras.size() * 6);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < cameras.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}

			break;
		}
		case 'r':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.px += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.py += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.pz += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].uv.size(); j++){

					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose);
					Eigen::Matrix<double, 2, 1> delta;
					PLine pl = get_plucker_line(lines[cameras[i].uv[j].first.index_to_line].first, lines[cameras[i].uv[j].first.index_to_line].second);
					observation_equation_perspective_camera_plucker_line_rodrigues_wc(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					Eigen::Matrix<double, 2, 6, Eigen::RowMajor> jacobian;
					observation_equation_perspective_camera_plucker_line_rodrigues_wc_jacobian(jacobian, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					int ir = tripletListB.size();
					int ic_camera = i * 6;
					tripletListA.emplace_back(ir     , ic_camera    , -jacobian(0,0));
					tripletListA.emplace_back(ir     , ic_camera + 1, -jacobian(0,1));
					tripletListA.emplace_back(ir     , ic_camera + 2, -jacobian(0,2));
					tripletListA.emplace_back(ir     , ic_camera + 3, -jacobian(0,3));
					tripletListA.emplace_back(ir     , ic_camera + 4, -jacobian(0,4));
					tripletListA.emplace_back(ir     , ic_camera + 5, -jacobian(0,5));

					tripletListA.emplace_back(ir + 1 , ic_camera    , -jacobian(1,0));
					tripletListA.emplace_back(ir + 1 , ic_camera + 1, -jacobian(1,1));
					tripletListA.emplace_back(ir + 1 , ic_camera + 2, -jacobian(1,2));
					tripletListA.emplace_back(ir + 1 , ic_camera + 3, -jacobian(1,3));
					tripletListA.emplace_back(ir + 1 , ic_camera + 4, -jacobian(1,4));
					tripletListA.emplace_back(ir + 1 , ic_camera + 5, -jacobian(1,5));

					tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));
					tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta(1,0), 1));

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 6, cameras.size() * 6);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < cameras.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_rodrigues(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].uv.size(); j++){

					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);
					Eigen::Matrix<double, 2, 1> delta;
					PLine pl = get_plucker_line(lines[cameras[i].uv[j].first.index_to_line].first, lines[cameras[i].uv[j].first.index_to_line].second);
					observation_equation_perspective_camera_plucker_line_quaternion_wc(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					Eigen::Matrix<double, 2, 7, Eigen::RowMajor> jacobian;
					observation_equation_perspective_camera_plucker_line_quaternion_wc_jacobian(jacobian, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					int ir = tripletListB.size();
					int ic_camera = i * 7;
					tripletListA.emplace_back(ir     , ic_camera    , -jacobian(0,0));
					tripletListA.emplace_back(ir     , ic_camera + 1, -jacobian(0,1));
					tripletListA.emplace_back(ir     , ic_camera + 2, -jacobian(0,2));
					tripletListA.emplace_back(ir     , ic_camera + 3, -jacobian(0,3));
					tripletListA.emplace_back(ir     , ic_camera + 4, -jacobian(0,4));
					tripletListA.emplace_back(ir     , ic_camera + 5, -jacobian(0,5));
					tripletListA.emplace_back(ir     , ic_camera + 6, -jacobian(0,6));

					tripletListA.emplace_back(ir + 1 , ic_camera    , -jacobian(1,0));
					tripletListA.emplace_back(ir + 1 , ic_camera + 1, -jacobian(1,1));
					tripletListA.emplace_back(ir + 1 , ic_camera + 2, -jacobian(1,2));
					tripletListA.emplace_back(ir + 1 , ic_camera + 3, -jacobian(1,3));
					tripletListA.emplace_back(ir + 1 , ic_camera + 4, -jacobian(1,4));
					tripletListA.emplace_back(ir + 1 , ic_camera + 5, -jacobian(1,5));
					tripletListA.emplace_back(ir + 1 , ic_camera + 6, -jacobian(1,6));

					tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));
					tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta(1,0), 1));

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			for(size_t i = 0 ; i < cameras.size(); i++){
				int ic = i * 7;
				int ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);

				double delta;
				quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

				Eigen::Matrix<double, 1, 4> jacobian;
				quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

				tripletListA.emplace_back(ir, ic + 3 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 4 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 5 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 6 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

				tripletListB.emplace_back(ir, 0, delta);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 7);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 7, cameras.size() * 7);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 7, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 7){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < cameras.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_quaternion(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "c: add noise to cameras" << std::endl;
	std::cout << "t: optimize cameras external orientation (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize cameras external orientation (Rodrigues)" << std::endl;
	std::cout << "q: optimize cameras external orientation (Quaternion)" << std::endl;
}

PLine get_plucker_line(const Eigen::Vector3d &from, const Eigen::Vector3d &to)
{
	PLine plucker_line;

	Eigen::Vector3d direction = (to-from);
	direction/=direction.norm();
	Eigen::Vector3d moment = from.cross(direction);

	plucker_line(0,0) = moment.x();
	plucker_line(1,0) = moment.y();
	plucker_line(2,0) = moment.z();
	plucker_line(3,0) = direction.x();
	plucker_line(4,0) = direction.y();
	plucker_line(5,0) = direction.z();

	return plucker_line;
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2018, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Zachary Kingston */

#include <ompl/util/PPM.h>
#include <boost/filesystem.hpp>

#include "ConstrainedPlanningCommon.h"

static const double pi2 = 2 * boost::math::constants::pi<double>();

/** Torus manifold. */
class TorusConstraint : public ompl::base::Constraint
{
public:
    const double outer;
    const double inner;

    TorusConstraint(const double outer, const double inner, const std::string &maze)
      : ompl::base::Constraint(3, 1), outer(outer), inner(inner), file_(maze)
    {
        ppm_.loadFile(maze.c_str());
    }

    void getStartAndGoalStates(Eigen::Ref<Eigen::VectorXd> start, Eigen::Ref<Eigen::VectorXd> goal) const
    {
        const double h = ppm_.getHeight() - 1;
        const double w = ppm_.getWidth() - 1;

        for (unsigned int x = 0; x <= w; ++x)
            for (unsigned int y = 0; y <= h; ++y)
            {
                Eigen::Vector2d p = {x / w, y / h};

                auto &c = ppm_.getPixel(x, y);
                if (c.red == 255 && c.blue == 0 && c.green == 0)
                    mazeToAmbient(p, start);

                else if (c.green == 255 && c.blue == 0 && c.red == 0)
                    mazeToAmbient(p, goal);
            }
    }

    void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
    {
        Eigen::Vector3d c = {x[0], x[1], 0};
        out[0] = (x - outer * c.normalized()).norm() - inner;
    }

    void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
    {
        const double xySquaredNorm = x[0] * x[0] + x[1] * x[1];
        const double xyNorm = std::sqrt(xySquaredNorm);
        const double denom = std::sqrt(x[2] * x[2] + (xyNorm - outer) * (xyNorm - outer));
        const double c = (xyNorm - outer) * (xyNorm * xySquaredNorm) / (xySquaredNorm * xySquaredNorm * denom);
        out(0, 0) = x[0] * c;
        out(0, 1) = x[1] * c;
        out(0, 2) = x[2] / denom;
    }

    void ambientToMaze(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const
    {
        Eigen::Vector3d c = {x[0], x[1], 0};

        const double h = ppm_.getHeight();
        const double w = ppm_.getWidth();

        out[0] = std::atan2(x[2], c.norm() - outer) / pi2;
        out[0] += (out[0] < 0);
        out[0] *= h;
        out[1] = std::atan2(x[1], x[0]) / pi2;
        out[1] += (out[1] < 0);
        out[1] *= w;
    }

    void mazeToAmbient(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const
    {
        Eigen::Vector2d a = x * pi2;

        Eigen::Vector3d b = {std::cos(a[0]), 0, std::sin(a[0])};
        b *= inner;
        b[0] += outer;

        double norm = std::sqrt(b[0] * b[0] + b[1] * b[1]);
        out << std::cos(a[1]), std::sin(a[1]), 0;
        out *= norm;
        out[2] = b[2];
    }

    bool mazePixel(const Eigen::Ref<const Eigen::VectorXd> &x) const
    {
        const double h = ppm_.getHeight();
        const double w = ppm_.getWidth();

        if (x[0] < 0 || x[0] >= w || x[1] < 0 || x[1] >= h)
            return false;

        const ompl::PPM::Color &c = ppm_.getPixel(x[0], x[1]);
        return !(c.red == 0 && c.blue == 0 && c.green == 0);
    }

    bool isValid(const ompl::base::State *state) const
    {
        auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
        Eigen::Vector2d coords;
        ambientToMaze(x, coords);

        return mazePixel(coords);
    }

    void dump(std::ofstream &file) const
    {
        file << outer << std::endl;
        file << inner << std::endl;

        boost::filesystem::path path(file_);
        file << boost::filesystem::canonical(path).string() << std::endl;
    }

private:
    const std::string file_;
    ompl::PPM ppm_;
};

bool torusPlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
{
    cp.setPlanner(planner);

    // Solve the problem
    ob::PlannerStatus stat = cp.solveOnce(output, "torus");

    if (output)
    {
        OMPL_INFORM("Dumping problem information to `torus_info.txt`.");
        std::ofstream infofile("torus_info.txt");
        infofile << cp.type << std::endl;
        dynamic_cast<TorusConstraint *>(cp.constraint.get())->dump(infofile);
        infofile.close();
    }

    cp.atlasStats();

    if (output)
        cp.dumpGraph("torus");

    return stat;
}

bool torusPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
{
    cp.setupBenchmark(planners, "torus");
    cp.runBenchmark();
    return 0;
}

bool torusPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
                   struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench, double outer, double inner,
                   const std::string &maze)
{
    // Create the ambient space state space for the problem.
    auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);

    ob::RealVectorBounds bounds(3);
    bounds.setLow(-(outer + inner));
    bounds.setHigh(outer + inner);

    rvss->setBounds(bounds);

    // Create a shared pointer to our constraint.
    auto constraint = std::make_shared<TorusConstraint>(outer, inner, maze);

    ConstrainedProblem cp(space, rvss, constraint);
    cp.setConstrainedOptions(c_opt);
    cp.setAtlasOptions(a_opt);

    Eigen::Vector3d start, goal;
    constraint->getStartAndGoalStates(start, goal);

    cp.setStartAndGoalStates(start, goal);
    cp.ss->setStateValidityChecker(std::bind(&TorusConstraint::isValid, constraint, std::placeholders::_1));

    if (!bench)
        return torusPlanningOnce(cp, planners[0], output);
    else
        return torusPlanningBench(cp, planners);
}

auto help_msg = "Shows this help message.";
auto output_msg = "Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
                  "`torus_path.txt` and `torus_graph.graphml` respectively.";
auto bench_msg = "Do benchmarking on provided planner list.";
auto outer_msg = "Outer radius of torus.";
auto inner_msg = "Inner radius of torus.";
auto maze_msg = "Filename of maze image (in .ppm format) to use as obstacles on the surface of the torus.";

int main(int argc, char **argv)
{
    bool output, bench;
    enum SPACE_TYPE space = PJ;
    std::vector<enum PLANNER_TYPE> planners = {RRT};

    struct ConstrainedOptions c_opt;
    struct AtlasOptions a_opt;

    double outer, inner;
    boost::filesystem::path path(__FILE__);
    std::string maze = (path.parent_path() / "mazes/thick.ppm").string();

    po::options_description desc("Options");
    desc.add_options()("help,h", help_msg);
    desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
    desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
    desc.add_options()("outer", po::value<double>(&outer)->default_value(2), outer_msg);
    desc.add_options()("inner", po::value<double>(&inner)->default_value(1), inner_msg);
    desc.add_options()("maze,m", po::value<std::string>(&maze), maze_msg);

    addSpaceOption(desc, &space);
    addPlannerOption(desc, &planners);
    addConstrainedOptions(desc, &c_opt);
    addAtlasOptions(desc, &a_opt);

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 1;
    }

    if (maze == "")
    {
        OMPL_ERROR("--maze is a required.");
        return 1;
    }

    return torusPlanning(output, space, planners, c_opt, a_opt, bench, outer, inner, maze);
}