#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_matrix.h>
#include "common.h"

typedef struct {
  int capacity;
  int size;
  double *x;
  double *y;
} SampleData;

// ODE
// du/dphi = v
// dv/dphi = 3Mu^2 - u
static int func(double phi, const double y[], double f[], void *params){
  f[0] = y[1];
  f[1] = -y[0] + 3.0*M*y[0]*y[0];
  return GSL_SUCCESS;
}

static int jac(double phi, const double y[], double *dfdy, double dfdphi[], void *params){
  gsl_matrix_view dfdy_mat = gsl_matrix_view_array(dfdy, 2, 2);
  gsl_matrix *m = &dfdy_mat.matrix;
  gsl_matrix_set(m, 0, 0, 0.0);
  gsl_matrix_set(m, 0, 1, 1.0);
  gsl_matrix_set(m, 1, 0, -3*M*y[0]-1.0);
  gsl_matrix_set(m, 1, 1, 0.0);

  dfdphi[0] = 0.0;
  dfdphi[0] = 0.0;

  return GSL_SUCCESS;
}

double find_root(double *y, double phi){
  //printf("phi: %g, y:{%g, %g}\n", phi, y[0], y[1]);
  return phi - y[0]/y[1];
}

double find_root_4ord(double u1, double u2, double v1, double v2, double phi1, double phi2){
  double Du = u1 - u2;
  double Dphi = phi2 - phi1;
  return -(u2*v1*Du*Du*Du - v1*v2*Du*Du*Du*phi2 + u2*u2*Du*(2*v1*Du + v2*Du + 3*v1*v2*Dphi) + u2*u2*u2*(v2*Du + v1*(Du + 2*v2*Dphi)))/(v1*v2*Du*Du*Du);
}
static inline double find_root_5ord(double u1, double u2, double v1, double v2, double phi1, double phi2){
  return (-(phi1*pow(u2,5)*v1*v2) + 
     u1*pow(u2,4)*(u2 - phi1*v1)*
      v2 + pow(u1,2)*pow(u2,3)*
      (u2 + 8*phi1*v1)*v2 + 
     pow(u1,4)*u2*v1*
      (-u2 + phi2*v2) + 
     2*pow(u1,3)*pow(u2,2)*
      (u2*(v1 - v2) - 4*phi2*v1*v2)
       + pow(u1,5)*
      (-(u2*v1) + phi2*v1*v2))/
   (pow(u1 - u2,3)*
     (pow(u1,2) + 4*u1*u2 + 
       pow(u2,2))*v1*v2);
}

// \chi(b) is the totol deflection angle with parameter b
double chi(double b){
  double x;
  gsl_odeiv2_system sys = {func, jac, 2, &x};
  gsl_odeiv2_driver *d = gsl_odeiv2_driver_alloc_y_new(&sys, gsl_odeiv2_step_rk8pd, 1e-3, 1e-12, 1e-10);
  
  double u0 = 1.0/R0;
  double y[2] = {
    u0,
    sqrt(1.0/(b*b) - u0*u0 + 2.0*M*u0*u0*u0)
  };
  double phi = 0;
  double phi_max = 8*PI;
  double y_previous[2] = {0.0, 0.0};
  double phi_previous=phi;

  while (y[0] > 0 && phi < phi_max){
    double phi_next = phi + 0.1;
    phi_previous = phi;
    y_previous[0] = y[0];
    y_previous[1] = y[1];
    int status = gsl_odeiv2_driver_apply(d, &phi, phi_next, y);
    if (status != GSL_SUCCESS){
      printf("Error: GSL driver returns %d\n", status);
      break;
    }
    printf("phi: %g, y[0]: %g, y[1]: %g\n",phi, y[0], y[1]);
  }

  //printf("%g, %g \n",find_root(y_previous, phi_previous),find_root(y, phi));
  //printf("%.16g\n",find_root_4ord(y_previous[0], y[0], y_previous[1], y[1], phi_previous, phi));
  //printf("%.16g\n",find_root_5ord(y_previous[0], y[0], y_previous[1], y[1], phi_previous, phi));
  //return (find_root(y_previous, phi_previous) + find_root(y, phi))/2.0;

  gsl_odeiv2_driver_free(d);
  if (phi >= phi_max){
    return phi_max;
  }
  return find_root_5ord(y_previous[0], y[0], y_previous[1], y[1], phi_previous, phi);
}

void sample_push(SampleData *sample, double x, double y){
  if (sample->size + 1 >= sample->capacity) {
    int newcap = sample->capacity*2;
    sample->x = realloc(sample->x, sizeof(double) * newcap);
    sample->y = realloc(sample->y, sizeof(double) * newcap);
    sample->capacity = newcap;
  }
  sample->x[sample->size] = x;
  sample->y[sample->size] = y;
  sample->size += 1;
}

static void refine_interval(double xl, double xr, double yl, double yr, double rela_err_limit, SampleData *sample){
  printf("calculating interval (%g, %g)\n", xl, xr);
  double xm = (xl + xr)/2;
  double ym = chi(xm);
  int need_refine = (fabs(ym-(yl+yr)/2) >= fabs(rela_err_limit * ym));
  if (need_refine) {
    printf(" ");
    refine_interval(xl, xm, yl, ym, rela_err_limit, sample);
  }
  sample_push(sample, xm, ym);
  if (need_refine) {
    printf(" ");
    refine_interval(xm, xr, ym, yr, rela_err_limit, sample);
  }
  return ;
}

int init(double bmax, double rela_err_limit){
  SampleData sample;
  sample.capacity = 1;
  sample.size=1;
  sample.x = malloc(sizeof(double) * 1);
  sample.y = malloc(sizeof(double) * 1);
  sample.x[0] = bmin;
  sample.y[0] = chi(bmin);
  double chi_bmax = chi(bmax);

  refine_interval(bmin, bmax, sample.y[0], chi_bmax, rela_err_limit, &sample);

  sample_push(&sample, bmax, chi_bmax);

  //sort

  //output
  printf("Total sample points number: %d\n", sample.size);
  for (int i = 0; i < sample.size; i++) {
    printf("%.16g %.16g\n", sample.x[i], sample.y[i]);
  }

  free(sample.x);
  free(sample.y);
  return 0;
}