#include <functional>
#include <fstream>
#include <stack>

#include "rbmp.hpp"

using Real = long double;

class AztecDiamond {
public:
  typedef struct Edge {
    std::stack<Real> weights;
    Real probability = 0;
  } Edge;

  using Face = std::array<std::reference_wrapper<Edge>, 4>;

  std::vector<Edge> edges;
  std::vector<std::vector<Face>> lattices;

  AztecDiamond(unsigned n) : edges(pow(2 * n, 2)), lattices(n) {
    for (unsigned i = 1; i <= n; i++) {
      lattices[n - i].reserve(pow(i, 2));
      unsigned x0 = n - i;
      for (unsigned j = 0; j < pow(i, 2); j++) {
        unsigned x = 2 * (j % i);
        unsigned y = 2 * (j / i);
        lattices[n - i].push_back({
              edges[2 * n * (x0 + y) + x0 + x],
              edges[2 * n * (x0 + y) + x0 + x + 1],
              edges[2 * n * (x0 + y + 1) + x0 + x],
              edges[2 * n * (x0 + y + 1) + x0 + x + 1]
              });
      }
    }
  }

  void computeWeights() {
    for (std::vector<Face>& faces : lattices) {
#pragma omp parallel for
      for (Face& f : faces) {
        Real w = f[0].get().weights.top();
        Real x = f[1].get().weights.top();
        Real y = f[2].get().weights.top();
        Real z = f[3].get().weights.top();

        Real cellFactor = w * z + x * y;

        f[0].get().weights.push(z / cellFactor);
        f[1].get().weights.push(y / cellFactor);
        f[2].get().weights.push(x / cellFactor);
        f[3].get().weights.push(w / cellFactor);
      }
    }

    // This process computes one extra weight per edge.
    for (Edge& e : edges) {
      e.weights.pop();
    }
  }

  void computeProbabilities() { // destroys *all* weights
    for (auto it = lattices.rbegin(); it != lattices.rend(); it++) {
#pragma omp parallel for
      for (Face& f : *it) {
        Real p = f[0].get().probability;
        Real q = f[1].get().probability;
        Real r = f[2].get().probability;
        Real s = f[3].get().probability;

        Real w = f[0].get().weights.top();
        Real x = f[1].get().weights.top();
        Real y = f[2].get().weights.top();
        Real z = f[3].get().weights.top();

        Real cellFactor = w * z + x * y;
        Real deficit = 1 - p - q - r - s;

        f[0].get().probability = s + deficit * w * z / cellFactor;
        f[1].get().probability = r + deficit * x * y / cellFactor;
        f[2].get().probability = q + deficit * x * y / cellFactor;
        f[3].get().probability = p + deficit * w * z / cellFactor;

        for (Edge& e : f) {
          e.weights.pop();
        }
      }
    }
  }
};

int main(int argc, char* argv[]) {
  unsigned n = 100;
  unsigned m = 100;
  Real T = 1;

  int opt;

  while ((opt = getopt(argc, argv, "n:m:T:")) != -1) {
    switch (opt) {
    case 'n':
      n = atoi(optarg);
      break;
    case 'm':
      m = (unsigned)atof(optarg);
      break;
    case 'T':
      T = atof(optarg);
      break;
    default:
      exit(1);
    }
  }

  std::string filename = "order_" + std::to_string(n) + "_" + std::to_string(T) + ".dat";
  std::ifstream input(filename);

  Rng r;
  AztecDiamond a(n);

  std::vector<Real> avgProbabilities(a.edges.size());

  for (unsigned i = 0; i < m; i++) {
    for (AztecDiamond::Edge& e : a.edges) {
      e.weights.push(exp(- r.variate<Real, std::exponential_distribution>(1) / T));
      e.probability = 0;
    }
    a.computeWeights();
    a.computeProbabilities();

    for (unsigned j = 0; j < a.edges.size(); j++) {
      avgProbabilities[j] += a.edges[j].probability;
    }
  }

  Graph G(n, r);

  std::vector<Real> data_x(G.vertices.size());
  std::vector<Real> data_y(G.vertices.size());

  for (unsigned i = 0; i < G.edges.size(); i++) {
    const Edge& e = G.edges[i];
    const Vertex& vt = e.halfedges[0].getTail();
    const Vertex& vh =  e.halfedges[0].getHead();
    data_x[vt.index] += avgProbabilities[i] * (vt.coordinate[0] - vh.coordinate[0]);
    data_y[vt.index] += avgProbabilities[i] * (vt.coordinate[1] - vh.coordinate[1]);
    data_x[vh.index] += avgProbabilities[i] * (vt.coordinate[0] - vh.coordinate[0]);
    data_y[vh.index] += avgProbabilities[i] * (vt.coordinate[1] - vh.coordinate[1]);
  }

  std::ofstream output(filename);

  for (unsigned i = 0; i < G.vertices.size(); i++) {
    output << data_x[i] << " " << data_y[i] << std::endl;
  }

  output.close();

  return 0;
}