#include "LinearSolver/cghs.h"

struct Operator {
  int n;
  double lambda, dt;
  std::vector<SlTri> tris;
};

std::vector<SlVector3> laplacian(const std::vector<SlVector3> &pts, const std::vector<SlTri> &tris);

void mult (const Operator &op, double *v, double *w) {
  std::vector<SlVector3> pts;
  pts.resize(op.n);
  for (unsigned int i=0; i<pts.size(); i++) pts[i].set(v[3*i+0], v[3*i+1], v[3*i+2]);
  std::vector<SlVector3> l = laplacian(pts, op.tris);
  for (unsigned int i=0; i<pts.size(); i++) {
    l[i] *= op.lambda*op.dt;
    w[3*i+0] = v[3*i+0] - l[i][0];
    w[3*i+1] = v[3*i+1] - l[i][1];
    w[3*i+2] = v[3*i+2] - l[i][2];
  }
}

int main(int argc, char *argv[]) {
  Operator op;
  op.n = pts.size(); op.lambda = lambda; op.dt = dt; op.tris = tris;

  double b[3*pts.size()];
  double x[3*pts.size()];

  for (unsigned int i=0; i<pts.size(); i++) {
    b[3*i+0] = pts[i][0]; b[3*i+1] = pts[i][1]; b[3*i+2] = pts[i][2];
    x[3*i+0] = pts[i][0]; x[3*i+1] = pts[i][1]; x[3*i+2] = pts[i][2];
  }

  cghs<Operator>(3*pts.size(), op, b, x, eps, true);

  for (unsigned int i=0; i<pts.size(); i++) pts[i].set(x[3*i+0], x[3*i+1], x[3*i+2]);
}