#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>



typedef struct {
    int x;
    int y;
} Tuple;

void setBlackRed(int L, Tuple* blackCells, Tuple* redCells){
  int blackCount=0;
  int redCount=0;
  for(int i=1; i<L-1; i++){
    for(int j=1; j<L-1; j++){
      Tuple cell;
      cell.x = i;
      cell.y = j;
      if((i+j)%2 == 0){
        blackCells[blackCount] = cell;
        blackCount++;
      } else {
        redCells[redCount] = cell;
        redCount++;
      }
    }
  }
}

void setPeriodicBoundary(int L, double m[L][L]){
  for(int i=1; i<L-1; i++){
    m[i][0] = m[i][L-2];
    m[i][L-1] = m[i][1];
    m[0][i] = m[L-2][i];
    m[L-1][i] = m[1][i];
  }
}

double testFunction(int x, int y, int L){
  double xval = ((double) x);
  double yval = ((double) y);
  double dev = ((double)L-2)/10;
  //printf("%f\n", 4*exp(-(xval*xval+yval*yval)/(dev*dev)) * (-dev*dev+xval*xval+yval*yval)
  //                / (dev*dev*dev*dev));
  //return 0;
  return 4*exp(-(xval*xval+yval*yval)/(dev*dev)) * (-dev*dev+xval*xval+yval*yval)
                  / (dev*dev*dev*dev);
}

double gausian(int x, int y, int L){
  double xval = ((double) x);
  double yval = ((double) y);
  double dev = ((double)L-2)/10;
  return exp(-(xval*xval + yval*yval)/(dev*dev));
}


double error(int L, double m[L][L], double m1[L][L]){
  double error = 0;
  for(int i=1; i<L-1; i++){
    for(int j=1; j<L-1; j++){
      //printf("%.12f\t%.12f\n", m[i][j], m1[i][j]);
      error += fabs(m[i][j]-m1[i][j]);
    }
  }
  return error/((L-2)*(L-2));
}




void SOR(int n, int L, double m[L][L], float w, double F[L][L]){
  for(int i=0; i<n; i++){
    setPeriodicBoundary(L, m);
    Tuple blackCells[(L-2)*(L-2)/2];
    Tuple redCells[(L-2)*(L-2)/2];
    setBlackRed(L, blackCells, redCells);
    for(int j=0; j<(L-2)*(L-2)/2; j++){
      int x = blackCells[j].x;
      int y = blackCells[j].y;
      m[x][y] = (1-w)*m[x][y] + w*.25*(m[x-1][y] + m[x+1][y] + m[x][y-1] + m[x][y+1]
               - F[x][y]/((L-2)*(L-2)));
    }
    setPeriodicBoundary(L, m);
    for(int j=0; j<(L-2)*(L-2)/2; j++){
      int x = redCells[j].x;
      int y = redCells[j].y;
      m[x][y] = (1-w)*m[x][y] + w*.25*(m[x-1][y] + m[x+1][y] + m[x][y-1] + m[x][y+1]
               - F[x][y]/((L-2)*(L-2)));
    }
  }
  setPeriodicBoundary(L, m);
}

double calculateConvergence(int L, int startingIterations, int iterationInterval, double tolerance, double w, double (*f)(int, int, int)){
  double m[L][L];
  double m1[L][L]; // Used for testing error
  double F[L][L];
  for(int i=0; i<L; i++){
    for(int j=0; j<L; j++){
      m[i][j] = (double)rand()/(double)RAND_MAX;
      F[i][j] = (*f)(i, j, L);
    }
  }

  SOR(startingIterations-1, L, m, w, F);
  for(int i=0; i<L; i++){for(int j=0; j<L; j++){m1[i][j] = m[i][j];}}
  SOR(1, L, m, w, F);
  double error1 = error(L, m, m1);
  double error2 = error1;

  while(error2 > tolerance){
    //printf("%.12f\t%.12f\n", error1, error2);
    error1 = error2;
    SOR(iterationInterval-1, L, m, w, F);
    for(int i=0; i<L; i++){for(int j=0; j<L; j++){m1[i][j] = m[i][j];}}
    SOR(1, L, m, w, F);
    error2 = error(L, m, m1);
  }

  return pow(error2/error1, 1./iterationInterval);
}

void mgm(int level, int L, double phi[L][L], double F[L][L], int m1, int m2, int gamma){
  SOR(m1, L, phi, 1, F);
  if(level > 0){
    double residual[L][L];
    for(int i=1; i<L-1; i++){for(int j=1; j<L-1; j++){
      residual[i][j] = (phi[i-1][j] + phi[i+1][j] + phi[i][j-1] + phi[i][j+1] - 4*phi[i][j])
                        - F[i][j]/((L-2)*(L-2));
      //residual[i][j] = -residual[i][j];
    }}
    setPeriodicBoundary(L, residual);

    int Lnew = (L-2)/2 + 2;
    double psi[Lnew][Lnew];
    double d[Lnew][Lnew];
    for(int i=1; i<Lnew-1; i++){for(int j=1; j<Lnew-1; j++){
      psi[i][j] = 0;
      int x = 2*i-1;
      int y = 2*j-1;
      d[i][j] = .25 * (residual[x][y] + residual[x+1][y]
                    + residual[x][y+1]+ residual[x+1][y+1]);
    }}


    for(int i=0; i<gamma; i++){
      setPeriodicBoundary(Lnew, d);
      setPeriodicBoundary(Lnew, psi);
      mgm(level-1, Lnew, psi, d, m1, m2, gamma);
    }

    for(int i=1; i<Lnew-1; i++){for(int j=1; j<Lnew-1; j++){
      int x = 2*i-1;
      int y = 2*j-1;
      phi[x][y] = phi[x][y] + psi[i][j];
      phi[x+1][y] = phi[x+1][y] + psi[i][j];
      phi[x][y+1] = phi[x][y+1] + psi[i][j];
      phi[x+1][y+1] = phi[x+1][y+1] + psi[i][j];
    }}

  }
  SOR(m2, L, phi, 1, F);
}

double calculateMgmConvergence(int level, int m1, int m2, int gamma, int startingIterations, int iterationInterval, double tolerance){
  int L = pow(2, level) + 2;

  double phi[L][L];
  double F[L][L];
  double testPhi[L][L];
  for(int i=1; i<L-1; i++){for(int j=1; j<L-1; j++){
    phi[i][j] = (double)rand()/(double)RAND_MAX;
    //phi[i][j] = i + j;
    F[i][j] = testFunction(i, j, L);
  }}

  // Starting iterations
  for(int i=0; i<startingIterations-1; i++){ mgm(level, L, phi, F, m1, m2, gamma); }
  // Update test
  for(int x=1; x<L-1; x++){for(int y=1; y<L-1; y++){testPhi[x][y] = phi[x][y];}}
  // Extra iteration
  mgm(level, L, phi, F, m1, m2, gamma);
  double error1 = error(L, phi, testPhi);
  double error2 = error1;

  while(error2 > tolerance){
    printf("%.12f\t%.12f\n", error1, error2);
    error1 = error2;

    for(int i=0; i<iterationInterval-1; i++){ mgm(level, L, phi, F, m1, m2, gamma); }
    // Update test
    for(int x=1; x<L-1; x++){for(int y=1; y<L-1; y++){testPhi[x][y] = phi[x][y];}}
    // Extra iteration
    mgm(level, L, phi, F, m1, m2, gamma);
    error2 = error(L, phi, testPhi);
  }

  return pow(error2/error1, 1./iterationInterval);
}


int main(){

  // int m1 = 2;
  // int m2 = 2;
  // int startingIterations = 1;
  // int iterationInterval = 1;
  // double tolerance = .0000001;

  // int start = 4;
  // int range = 4;
  // int Lrange[range];
  // double convergenceArr1[range];
  // double convergenceArr2[range];
  // for(int i=start; i<start+range; i++){
  //   printf("levels: %i\n", i);
  //   Lrange[i-start] = i;
  //   convergenceArr1[i-start] = calculateMgmConvergence(i, m1, m2, 1, startingIterations, iterationInterval, tolerance);
  //   convergenceArr2[i-start] = calculateMgmConvergence(i, m1, m2, 2, startingIterations, iterationInterval, tolerance);
  // }




  int N = 64;
  int L = N+2;
  int startingIterations = 100;
  int iterationInterval = 20;
  double tolerance = .0000001;


  int omegaLength = 100;
  double omegaArr[omegaLength];
  double convergenceArr[omegaLength];
  for(int i=0; i<omegaLength; i++){
    omegaArr[i] = 1 + ((double)i)/omegaLength;
    convergenceArr[i] = calculateConvergence(L, startingIterations, iterationInterval,
                                              tolerance, omegaArr[i], testFunction);
    printf("%.12f\t%.12f\n", omegaArr[i], convergenceArr[i]);
  }


  printf("Writing to file\n");
  FILE *f = fopen("../out/temp.txt", "w");
  if (f == NULL){
      printf("Error opening file!\n");
      exit(1);
  }

  /* print some text */
  const char *text = "Plot1";
  fprintf(f, "%s\n", text);

  /* print integers and floats */
  //for(int i=0; i<range; i++){
  for(int i=0; i<omegaLength; i++){
    //fprintf(f, "%i\t%.12f\t%.12f\n", Lrange[i], convergenceArr1[i], convergenceArr2[i]);
    fprintf(f, "%.12f\t%.12f\n", omegaArr[i], convergenceArr[i]);
  }

  fclose(f);
}