#include <iostream>
using namespace std;
#include <time.h>
#include <fstream>
#include "Graph.hpp"
#include "GraphAlg.hpp"
#include <math.h>

vector<int> x;                      //Current solution 
vector<int> xstar;                  //Current best solution vector
double      zstar;                  //Current best solution value
vector<int> weights;                //Weights of the instance 
vector< vector<double> > qp_matrix; //The profit matrix 
int belowLB=0;                      //Number of nodes below zstar 
int infeasible=0;                   //Number of infeasible nodes 
int aboveLB=0;                      //Number of nodes above zstar 
int proc=0;                         //Number of nodes processed 
double root_bound=10e30;            //The root bound 
int print_int = 20;                 //Print statistic with this interval
int line_cnt = 0;                   //How many lines have we printed 

/* ======================================================================
		         MinimumCut 
   ====================================================================== */

void MinimumCut(vector< vector<double> > mat, 
		 int dim, 
		 int source, 
		 int target, 
		 vector<int>& S, 
		 vector<int>& T, 
		 double& mincut_val){

  Sparse_Graph G(dim);
  for(size_t i=0;i<mat.size();i++) {
    for(size_t j=0;j<mat[i].size();j++){
      if (i==j) continue;
      if(mat[i][j] > 0)
	G.addEdge(i,j,mat[i][j]);
    }
  }
  Vertex s(source);
  Vertex t(target);   
  GraphAlg::MinimumSTCut(G, s, t, S,  T, mincut_val);
}

/* ======================================================================
		         ReadInstance
   ====================================================================== */

void ReadInstance(char* filename, 
		  int& dimension, 
		  int& capacity, 
		  vector<int> &weights, 
		  vector<vector<double> > &matrix) {

  // Open file
  std::ifstream file(filename);
  if(!file) {
    std::cerr << "Could not open file: " << filename << std::endl;
    exit(2);
  }

  // Print comment if present
  string comment;
  if(file.peek() == '#') {
    getline(file,comment);
    std::cout << comment << std::endl;
  }

  // Read dimension, capacity and weights
  file >> dimension;
  if(dimension <= 0) {
    std::cerr << "Error: dimension <= 0" << std::endl;
  }

  file >> capacity;
  if(capacity <= 0) {
    std::cerr << "Error: capacity <= 0" << std::endl;
  }

  weights = vector<int>(dimension);
  for(int i=0; i<dimension; i++) {
    file >> weights[i];
  }

  // Read matrix
  vector<double> row(dimension);
  for(int i=0; i<dimension; i++) {
    for(int j=0; j<dimension; j++){
      file >> row[j];
    }
    matrix.push_back(row);
  }
 
  return;
}

/* ======================================================================
		         print_matrix 
   ====================================================================== */

void print_matrix(const vector< vector<double> > matrix) {
  for(size_t i=0; i<matrix.size(); i++) {
    for(size_t j=0; j<matrix[i].size(); j++) {
      cout.width(6);
      cout.precision(4);
      std::cout << matrix[i][j] << " ";
    }
    std::cout << std::endl;  }
}

/* ======================================================================
		         lagrange_relax
   ====================================================================== */

void lagrange_relax(vector<vector<double> > &matrix,
		    vector<int> &weights, 
		    double lambda,
		    int capacity,
		    double& k) {

  for (size_t i=0; i < matrix.size(); i++){
    matrix[i][i] = matrix[i][i] - weights[i] * lambda;
  }
  k = lambda * capacity;
}


/* ======================================================================
                           copy_solution 
   ====================================================================== */
//Copy this solution to zstar 
void copy_solution(double z, vector<int> x){
  if(z < zstar) return;
  zstar = z;
  xstar = x;
  
  //  cout << "z: " << zstar << endl;

  for (int i = 0 ; i<10;i++ ){
    //    if (x[i] == 1){
    //    cout << "x" << i+1 << "= " << x[i]<<  endl; 
      //    }
  }

}


/* ======================================================================
                            print_stat_head
   ====================================================================== */
void print_stat_head(){
  cout.width(12);
  cout << "Processed";
  cout.width(12);
  cout << "Above LB";
  cout.width(12);
  cout << "Below LB";
  cout.width(12);
  cout << "Infeasible";
  cout.width(12);
  cout << "zstar";
  cout.width(12);
  cout << "current UB" << endl;
}
   
/* ======================================================================
                            print_stat_line
   ====================================================================== */
void print_stat_line(double UB){
  line_cnt++;
  if(line_cnt % print_int != 0) return;
  if((line_cnt/print_int) % 20 == 0) print_stat_head();
  cout.width(12);
  cout << proc;
  cout.width(12);
  cout << aboveLB;
  cout.width(12);
  cout << belowLB;
  cout.width(12);
  cout << infeasible;
  cout.width(12);
  cout << zstar;
  cout.width(12);
  cout << UB << endl;
}

/* ======================================================================
                        Ex6 Begins here 
   ====================================================================== */

/* ======================================================================
		         convert 
   ====================================================================== */

//Fill out this function. It should setup the problem described in exercise 5 from P2
void convert(vector<vector<double> > qp_matrix, 
	     vector<vector<double> > &max_flow_matrix, 
	     int &source_vertex, 
	     int &target_vertex) {

  source_vertex = 0;
  target_vertex = max_flow_matrix.size()-1;
  
  

  // set edge capacities for edges leaving the source_vertex
  double sum = 0;
  for (unsigned int i = 0; i < qp_matrix.size() ; i++){
    for (unsigned int j = 0; j < qp_matrix.size(); j++){
      sum += qp_matrix[i][j];
    }

    if (sum > 0){
      max_flow_matrix[0][i+1] = sum;
    }
    sum = 0;
  }

  // copy edge capacities
  for (unsigned int i = 0; i < qp_matrix.size() ; i++){
    for (unsigned int j = 0; j < qp_matrix.size(); j++){
      if (i != j){
        max_flow_matrix[i+1][j+1] = qp_matrix[i][j];
      }
    }
  }


  // set edge capacities for edges entering the target_vertex
  sum = 0;
  for (unsigned int i = 0; i < qp_matrix.size() ; i++){
    for (unsigned int j = 0; j < qp_matrix.size(); j++){
      sum += qp_matrix[i][j];
    }

    if (sum < 0){
      max_flow_matrix[i+1][target_vertex] = -1*sum;
    }
    sum = 0;
  }

}

/* ======================================================================
                                QP 
   ====================================================================== */
//Fill out the rest of this function. It should solve exercise 6 from P2 together with convert 
double QP(vector<vector<double> > qp_matrix, vector<int>& S) {
  //Convert the problem 
  size_t dimension = qp_matrix.size();
  vector< vector<double> > max_flow_matrix;
  vector<double> new_row(dimension+2);
  max_flow_matrix.resize(dimension+2,new_row); // Any row with size dimension+2 will do
  int source_vertex; 
  int target_vertex;

  convert(qp_matrix, max_flow_matrix, source_vertex, target_vertex);
  
  //Calculate minimum cut 
  //vector<int> S;
  vector<int> T;
  double min_cut_val = 0;
  
  MinimumCut(max_flow_matrix, max_flow_matrix.size(),
	     source_vertex, target_vertex, S, T, min_cut_val);

  //Calculate the QP solution, fill out the rest before we return 
  double c_si = 0;
  double qp_solution;

  
  // Calculate objective function value for QP
  for (unsigned int i =1; i<=qp_matrix.size(); i++){
    c_si += max_flow_matrix[0][i];
  }
  
  qp_solution = c_si - min_cut_val;

  
  return qp_solution;
}

/* ======================================================================
                        Ex11 Begins here 
   ====================================================================== */

// restore diagonal after lagrange
void lagrange_restore(vector<vector<double> > &matrix,
                      vector<int> &weights, 
                      double lambda){
  for (size_t i=0; i < matrix.size(); i++){
    matrix[i][i] = matrix[i][i] + weights[i] * lambda;
  }
}

/* ======================================================================
                             upper_bound
   ====================================================================== */
//fill out this function, it should solve the lagrange dual problem. 
double upper_bound(vector<vector<double> > &qp_matrix,
		   int c){

  double UB=10e30;
  double k;
 
  //*********************************************************************
  // Golden Section Search from M. Heath's "Scientific Computing 2nd Ed."
  //*********************************************************************

  // Tolerance indicating how close to an optimal solution we terminate
  double tol = 0.0001;
  
  // "golden" ratio, which lets you reuse samplings between iterations 
  double tau = (sqrt(5)-1)/2;
  
  // smallest possible Lagrange Multiplier
  double a = 0;

  // largest possible Lagrange Multiplier
  double b = 100;

  // Sample 1
  double x1 = a +(1-tau)*(b-a);
  lagrange_relax(qp_matrix, weights,x1,c,k);
  vector<int> S1;
  double f1 = QP(qp_matrix,S1) + k;
  lagrange_restore(qp_matrix, weights,x1);

  // Sample 2
  double x2 = a + tau*(b-a);
  lagrange_relax(qp_matrix, weights,x2,c,k);
  vector<int> S2;
  double f2 = QP(qp_matrix,S2) + k;
  lagrange_restore(qp_matrix, weights,x2);

  while((b-a) >tol){
    if (f1 >f2){
      a = x1;
      x1 = x2;
      f1 = f2;
      x2 = a + tau*(b-a);
      lagrange_relax(qp_matrix, weights,x2,c,k);
      vector<int> S2;
      f2 = QP(qp_matrix,S2) + k;
      lagrange_restore(qp_matrix, weights,x2);
      if (f2 < UB) {
        UB = f2;
      }
    }else{
      b = x2;
      x2 = x1;
      f2 = f1;
      x1 = a+(1-tau)*(b-a);
      lagrange_relax(qp_matrix, weights,x1,c,k);
      vector<int> S1;
      f1 = QP(qp_matrix,S1) + k;
      lagrange_restore(qp_matrix, weights,x1);
      if (f1 < UB) {
        UB = f1;
      }
    }

  }
  
  return UB;
}

/* ======================================================================
                        Ex12 Begins here 
   ====================================================================== */

/* ======================================================================
                              quadknap
   ====================================================================== */
//Fill in the pseudocode described in P2 pages 5-6 
//You can delete the statistic part if you like.

void quadknap(double P, int c, int n){

  proc++;
  if(c < 0){infeasible++;print_stat_line(0); return;}
  if(P > zstar) copy_solution(P,x);
  if(n==0){belowLB++;  print_stat_line(0); return;}

  double UB = upper_bound(qp_matrix,c);

  //Print statistic 
  if(P + UB <= zstar) {
    belowLB++;
  }
  else {
    aboveLB++;

    x[n-1] = 1;

    for (int i = 0; i<n-1 ; i++){
      qp_matrix[i][i] = qp_matrix[i][i] + qp_matrix[i][n-1] + qp_matrix[n-1][i]; 
    }

    quadknap(P+qp_matrix[n-1][n-1],c-weights[n-1],n-1);
    
    x[n-1] = 0;
    for (int i = 0; i<n-1 ; i++){
      qp_matrix[i][i] = qp_matrix[i][i] - qp_matrix[i][n-1] - qp_matrix[n-1][i]; 
    }

    quadknap(P,c,n-1);
    
  }
  
  

  print_stat_line(UB);

  //Psudocode from P2
}

/* ======================================================================
		              Ex6Main
   ====================================================================== */
void Ex6main(int c){
  //We use a Lagrangian relaxation to obtain a instance of QP
  double k;

  lagrange_relax(qp_matrix, weights, 4, c, k);

  //Call QP and get a 
  vector<int> S;
  double qp_solution = QP(qp_matrix,S);
  
  //Write solution to screen
  std::cout << "QP solution: " << qp_solution << std::endl;
}

/* ======================================================================
                             Ex11Main
   ====================================================================== */

void Ex11Main(int c){
  //This section is used for Exercise11 just remove the comment in the Main function when you are ready 

  //Solve the problem "min QKP(lambda)" 
  double UB = upper_bound(qp_matrix,c);
  
  //Write solution to screen
  std::cout << "min QKP(Lambda): " << UB << std::endl;
  
}

/* ======================================================================
                             Ex12Main
   ====================================================================== */

void Ex12main(int dimension,int c){
  //This section is used for Exercise12 just remove the comment in the Main function when you are ready 
  //Setup the solution vectors and initialize zstar, maybe we can add a heuristic 
  xstar = vector<int>(dimension);
  x = vector<int>(dimension);
  zstar = 0;
  
  //print the statistic head 
  print_stat_head();

  //call the quadknap procedure
  quadknap(0,c,qp_matrix.size());

  //Print out the final information 
  line_cnt=print_int-1;
  print_stat_line(0);
  cout << "Opt " << zstar << endl
       << "Processed nodes " << proc << endl 
       << "Time used " << clock()/CLOCKS_PER_SEC << " sec " << endl;

}

/* ======================================================================
		         Main
   ====================================================================== */

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

  // Check argument
  if(argc != 2) {
    std::cerr << "Use " << argv[0] << " filename " << std::endl;
    exit(1);
  }

  //Read the problem this part should should not be deleted 
  int dimension;
  int c;
  ReadInstance(argv[1], dimension, c, weights, qp_matrix);

  //Ex6main(c);
  //Ex11Main(c);
  Ex12main(dimension,c);

  

  
}