#include "problem.hpp"

class nanException : public std::exception {
  virtual const char* what() const throw() { return "The linear problem returned NaN."; }
} nanex;

problem::problem(const graph& G, unsigned axis, cholmod_sparse* vcmat, cholmod_common* c)
    : G(G), axis(axis), voltcurmat(vcmat), c(c) {
  b = CHOL_F(zeros)(G.vertices.size(), 1, CHOLMOD_REAL, c);
  for (unsigned i = 0; i < G.edges.size(); i++) {
    graph::coordinate v0 = G.vertices[G.edges[i].v[0]].r;
    graph::coordinate v1 = G.vertices[G.edges[i].v[1]].r;

    if (G.edges[i].crossings[axis]) {
      bool ind;
      if (axis == 1) {
        ind = v0.y < v1.y ? 0 : 1;
      } else {
        ind = v0.x < v1.x ? 0 : 1;
      }

      ((double*)b->x)[G.edges[i].v[ind]] += 1.0;
      ((double*)b->x)[G.edges[i].v[!ind]] -= 1.0;
    }
  }

  unsigned nnz = G.vertices.size() + G.edges.size();

  cholmod_triplet* t =
      CHOL_F(allocate_triplet)(G.vertices.size(), G.vertices.size(), nnz, 1, CHOLMOD_REAL, c);

  for (unsigned i = 0; i < G.vertices.size(); i++) {
    ((CHOL_INT*)t->i)[i] = i;
    ((CHOL_INT*)t->j)[i] = i;
    ((double*)t->x)[i] = 0.0;
  }

  unsigned terms = G.vertices.size();

  std::unordered_map<std::array<unsigned, 2>, unsigned> known_edges;

  for (unsigned i = 0; i < G.edges.size(); i++) {
    unsigned v0 = G.edges[i].v[0];
    unsigned v1 = G.edges[i].v[1];

    ((double*)t->x)[v0]++;
    ((double*)t->x)[v1]++;

    unsigned s0 = v0 < v1 ? v0 : v1;
    unsigned s1 = v0 < v1 ? v1 : v0;

    auto it = known_edges.find({s0, s1});

    if (it == known_edges.end()) {
      ((CHOL_INT*)t->i)[terms] = s1;
      ((CHOL_INT*)t->j)[terms] = s0;
      ((double*)t->x)[terms] = -1.0;

      known_edges[{s0, s1}] = terms;
      terms++;
    } else {
      ((double*)t->x)[it->second] -= 1.0;
    }
  }

  ((double*)t->x)[0]++;

  t->nnz = terms;

  cholmod_sparse* laplacian = CHOL_F(triplet_to_sparse)(t, terms, c);
  CHOL_F(free_triplet)(&t, c);
  factor = CHOL_F(analyze)(laplacian, c);
  CHOL_F(factorize)(laplacian, factor, c);
  CHOL_F(free_sparse)(&laplacian, c);

  sol.currents.resize(G.edges.size());
}

problem::problem(const graph& G, unsigned axis, cholmod_common* c) : problem(G, axis, NULL, c) {
  cholmod_triplet* t = CHOL_F(allocate_triplet)(G.edges.size(), G.vertices.size(),
                                                2 * G.edges.size(), 0, CHOLMOD_REAL, c);

  t->nnz = 2 * G.edges.size();

  for (unsigned i = 0; i < G.edges.size(); i++) {
    ((CHOL_INT*)t->i)[2 * i] = i;
    ((CHOL_INT*)t->j)[2 * i] = G.edges[i].v[0];
    ((double*)t->x)[2 * i] = 1.0;

    ((CHOL_INT*)t->i)[2 * i + 1] = i;
    ((CHOL_INT*)t->j)[2 * i + 1] = G.edges[i].v[1];
    ((double*)t->x)[2 * i + 1] = -1.0;
  }

  voltcurmat = CHOL_F(triplet_to_sparse)(t, 2 * G.edges.size(), c);

  CHOL_F(free_triplet)(&t, c);
}

problem::problem(const problem& other) : G(other.G), axis(other.axis), c(other.c), sol(other.sol) {
  b = CHOL_F(copy_dense)(other.b, c);
  factor = CHOL_F(copy_factor)(other.factor, c);
  voltcurmat = CHOL_F(copy_sparse)(other.voltcurmat, c);
}

problem::~problem() {
  CHOL_F(free_dense)(&b, c);
  CHOL_F(free_factor)(&factor, c);
  CHOL_F(free_sparse)(&voltcurmat, c);
}

void problem::solve(std::vector<bool>& fuses) {
  cholmod_dense* x = CHOL_F(solve)(CHOLMOD_A, factor, b, c);

  if (((double*)x->x)[0] != ((double*)x->x)[0]) {
    throw nanex;
  }

  cholmod_dense* y = CHOL_F(allocate_dense)(G.edges.size(), 1, G.edges.size(), CHOLMOD_REAL, c);

  double alpha[2] = {1, 0};
  double beta[2] = {0, 0};
  CHOL_F(sdmult)(voltcurmat, 0, alpha, beta, x, y, c);

  sol.conductivity = {0, 0};

  for (int i = 0; i < G.edges.size(); i++) {
    if (fuses[i]) {
      sol.currents[i] = 0;
    } else {
      sol.currents[i] = ((double*)y->x)[i];

      graph::coordinate v0 = G.vertices[G.edges[i].v[0]].r;
      graph::coordinate v1 = G.vertices[G.edges[i].v[1]].r;

      if (G.edges[i].crossings[axis]) {
        bool comp;
        if (axis == 1) {
          comp = v0.y > v1.y;
        } else {
          comp = v0.x > v1.x;
        }

        if (comp) {
          sol.currents[i] += 1.0;
          sol.conductivity[axis] += sol.currents[i];
        } else {
          sol.currents[i] -= 1.0;
          sol.conductivity[axis] -= sol.currents[i];
        }
      }
    }
  }

  sol.conductivity[!axis] = 0.0;

  CHOL_F(free_dense)(&x, c);
  CHOL_F(free_dense)(&y, c);
}

void problem::break_edge(unsigned e, bool unbreak) {
  unsigned v0 = G.edges[e].v[0];
  unsigned v1 = G.edges[e].v[1];

  unsigned n = factor->n;

  cholmod_sparse* update_mat = CHOL_F(allocate_sparse)(n, n, 2, true, true, 0, CHOLMOD_REAL, c);

  unsigned s1, s2;
  s1 = v0 < v1 ? v0 : v1;
  s2 = v0 < v1 ? v1 : v0;

  CHOL_INT* pp = (CHOL_INT*)update_mat->p;
  CHOL_INT* ii = (CHOL_INT*)update_mat->i;
  double* xx = (double*)update_mat->x;

  for (unsigned i = 0; i <= s1; i++) {
    pp[i] = 0;
  }

  for (unsigned i = s1 + 1; i <= n; i++) {
    pp[i] = 2;
  }

  ii[0] = s1;
  ii[1] = s2;
  xx[0] = 1;
  xx[1] = -1;

  cholmod_sparse* perm_update_mat =
      CHOL_F(submatrix)(update_mat, (CHOL_INT*)factor->Perm, factor->n, NULL, -1, true, true, c);

  CHOL_F(updown)(unbreak, perm_update_mat, factor, c);

  CHOL_F(free_sparse)(&perm_update_mat, c);
  CHOL_F(free_sparse)(&update_mat, c);

  graph::coordinate r0 = G.vertices[v0].r;
  graph::coordinate r1 = G.vertices[v1].r;

  if (G.edges[e].crossings[axis]) {
    bool ind;
    if (axis == 1) {
      ind = r0.y < r1.y ? unbreak : !unbreak;
    } else {
      ind = r0.x < r1.x ? unbreak : !unbreak;
    }

    ((double*)b->x)[G.edges[e].v[ind]] -= 1.0;
    ((double*)b->x)[G.edges[e].v[!ind]] += 1.0;
  }
}