#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#define R (sqrt(5)-1.)*0.5
#define C (1.0-R)
#define SHFT2(a,b,c) (a)=(b);(b)=(c);
#define SHFT3(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);


//.........................COMPILE TO SHARED LIBRARY FOR USE WITH PYTHON.........................................

// gcc -shared overlap_algorithm.c -o overlap_algorithm.so -lm -fPIC -O3


//..................................LINE MINIMIZATION ALGORITHMS.................................................


double gss_eigenvalue_minim(double a, double b, double eps, double t, double *matA, double *matB, int early_stop,
                  double f ( double x, double *mat1, double *mat2 ), double *xmin, int *feval) {

  double f1,f2,x0,x1,x2,x3,m,tol,t2;
  int num_eval;

  x0=a;
  x3=b;

  x1 = a + C * (b - a);
  x2 = a + R * (b - a);

  f1=f(x1, matA, matB);
  f2=f(x2, matA, matB);
  num_eval = 2;

  for ( ; ; )
  {
    m = 0.5 * ( x1 + x2 ) ;
    tol = eps * fabs ( m ) + t;
    t2 = 2.0 * tol;
/*
  Check the stopping criterion.
*/
    if (fabs(x3-x0) < t2)
    {
      break;
    }

    if ( early_stop == 1 && (f1 < -1. || f2 < -1.)) {
      break;
    }

    if (f2 < f1) {
    SHFT3(x0,x1,x2,R*x1+C*x3);
    SHFT2(f1,f2,f(x2, matA, matB));
    num_eval++;
    }
    else {
      SHFT3(x3,x2,x1,R*x2+C*x0);
      SHFT2(f2,f1,f(x1, matA, matB));
      num_eval++;
    }
  }

  if (f1 < f2) {
    *xmin=x1;
    *feval = num_eval;
    return f1;
  }
  else {
    *xmin=x2;
    *feval = num_eval;
    return f2;
  }
}



double brent_eigenvalue_minim ( double a, double b, double eps, double t,
  double *matA, double *matB, int early_stop,
  double f ( double x, double *mat1, double *mat2 ), double *x, int *feval)

/*

  Adapted version of LOCAL_MIN function for finding eigenvalue extrema.

  Authors of original minimization code:

    Orignal FORTRAN77 version by Richard Brent.
    C version by John Burkardt.

  Licensing:

    This code is distributed under the GNU LGPL license.

  Parameters:

    Input, double A, B, the endpoints of the interval.

    Input, double EPS, a positive relative error tolerance.
    EPS should be no smaller than twice the relative machine precision,
    and preferably not much less than the square root of the relative
    machine precision.

    Input, double T, a positive absolute error tolerance.

    Input, double F ( double x ,double *mat1, double *mat2 ), a user-supplied
    function whose local minimum along x is being sought, with matrices mat1
    and mat2 as parameters.

    Output, double *X, the estimated value of an abscissa
    for which F attains a local minimum value in [A,B].

    Output, double *feval, the number of function evaluations.

    Output, double LOCAL_MIN, the value F(X).
*/
{
  double c;
  double d;
  double e;
  double fu;
  double fv;
  double fw;
  double fx;
  double m;
  double p;
  double q;
  double r;
  double sa;
  double sb;
  double t2;
  double tol;
  double u;
  double v;
  double w;
  int num_eval;
/*
  C is the square of the inverse of the golden ratio.
*/
  c = 0.5 * ( 3.0 - sqrt ( 5.0 ) );

  sa = a;
  sb = b;
  *x = sa + c * ( b - a );
  w = *x;
  v = w;
  e = 0.0;
  fx = f ( *x, matA, matB );
  fw = fx;
  fv = fw;
  num_eval = 1;

  for ( ; ; )
  {
    m = 0.5 * ( sa + sb ) ;
    tol = eps * fabs ( *x ) + t;
    t2 = 2.0 * tol;
/*
  Check the stopping criterion.
*/
    if ( fabs ( *x - m ) <= t2 - 0.5 * ( sb - sa ) )
    {
      break;
    }

/*
  If we are looking for the maximum of smallest eigenvalue and the
  calculation goes over the value of 1 (under -1 because algorithm finds min),
  we can stop as that means the ellipses certainly do not overlap
*/
    if ( early_stop == 1 && fx < -1.) {
      break;
    }
/*
  Fit a parabola.
*/
    r = 0.0;
    q = r;
    p = q;

    if ( tol < fabs ( e ) )
    {
      r = ( *x - w ) * ( fx - fv );
      q = ( *x - v ) * ( fx - fw );
      p = ( *x - v ) * q - ( *x - w ) * r;
      q = 2.0 * ( q - r );
      if ( 0.0 < q )
      {
        p = - p;
      }
      q = fabs ( q );
      r = e;
      e = d;
    }

    if ( fabs ( p ) < fabs ( 0.5 * q * r ) &&
         q * ( sa - *x ) < p &&
         p < q * ( sb - *x ) )
    {
/*
  Take the parabolic interpolation step.
*/
      d = p / q;
      u = *x + d;
/*
  F must not be evaluated too close to A or B.
*/
      if ( ( u - sa ) < t2 || ( sb - u ) < t2 )
      {
        if ( *x < m )
        {
          d = tol;
        }
        else
        {
          d = - tol;
        }
      }
    }
/*
  A golden-section step.
*/
    else
    {
      if ( *x < m )
      {
        e = sb - *x;
      }
      else
      {
        e = sa - *x;
      }
      d = c * e;
    }
/*
  F must not be evaluated too close to X.
*/
    if ( tol <= fabs ( d ) )
    {
      u = *x + d;
    }
    else if ( 0.0 < d )
    {
      u = *x + tol;
    }
    else
    {
      u = *x - tol;
    }

    fu = f ( u, matA, matB );
    num_eval++;
/*
  Update A, B, V, W, and X.
*/
    if ( fu <= fx )
    {
      if ( u < *x )
      {
        sb = *x;
      }
      else
      {
        sa = *x;
      }
      v = w;
      fv = fw;
      w = *x;
      fw = fx;
      *x = u;
      fx = fu;
    }
    else
    {
      if ( u < *x )
      {
        sa = u;
      }
      else
      {
        sb = u;
      }

      if ( fu <= fw || w == *x )
      {
        v = w;
        fv = fw;
        w = u;
        fw = fu;
      }
      else if ( fu <= fv || v == *x || v == w )
      {
        v = u;
        fv = fu;
      }
    }
  }
  *feval = num_eval;
  return fx;
}


double r8_epsilon ( )
/*
  Purpose:

    R8_EPSILON returns the R8 round off unit.

  Discussion:

    R8_EPSILON is a number R which is a power of 2 with the property that,
    to the precision of the computer's arithmetic,
      1 < 1 + R
    but
      1 = ( 1 + R / 2 )

  Parameters:

    Output, double R8_EPSILON, the R8 round-off unit.
*/
{
  const double value = 2.220446049250313E-016;

  return value;
}


//.................................EIGENVALUE AND EIGENVECTOR CALCULATION.............................................


double determinant(double* a){
  double det = a[0] * (a[4]*a[8] - a[7]*a[5]) - a[3] * (a[1]*a[8] - a[7]*a[2]) + a[6] * (a[1]*a[5] - a[4]*a[2]);
  return det;
}


void eigenvalues(double* matrix, double* eigs){

  double p1 = matrix[1]*matrix[1] + matrix[2]*matrix[2] + matrix[5]*matrix[5];
  double q, p2, p;
  double aux[9];
  double r, phi;
  double pi = M_PI;

  if (p1 == 0) {
    eigs[0] = matrix[0];
    eigs[1] = matrix[4];
    eigs[2] = matrix[8];
  }
  else {
    q = (matrix[0]+matrix[4]+matrix[8])/3;
    p2 = (matrix[0]-q)*(matrix[0]-q) + (matrix[4]-q)*(matrix[4]-q) + (matrix[8]-q)*(matrix[8]-q) + 2*p1;
    p = sqrt(p2 / 6);

    for (int i=0; i<9; i++) {
      aux[i] = matrix[i];
      if (i % 4 == 0) {
        aux[i] -= q;
      }
      aux[i] = (1 / p) * aux[i];
    }

    r = determinant(aux) / 2;

    if (r <= -1){
      phi = pi / 3;
    }
    else if (r >= 1){
      phi = 0;
    }
    else {
      phi = acos(r) / 3;
    }

    eigs[2] = q + 2 * p * cos(phi);
    eigs[0] = q + 2 * p * cos(phi + (2*pi/3));
    eigs[1] = 3 * q - eigs[0] - eigs[2];
  }

}


void eigenvector(double* matrix, int which, double* vector){

  double eigs[3];
  double mat_lbd1[9];
  double mat_lbd2[9];
  double product[9];
  double norm1, norm2, norm3, norm;
  int idx;

  eigenvalues(matrix, eigs);

  for (int i=0; i<9; i++) {
    mat_lbd1[i] = matrix[i];
    mat_lbd2[i] = matrix[i];
    if (i % 4 == 0) {
      if (which == 1) {
        mat_lbd1[i] -= eigs[1];
        mat_lbd2[i] -= eigs[2];
      }
      else if (which == 2) {
        mat_lbd1[i] -= eigs[0];
        mat_lbd2[i] -= eigs[2];
      }
      else {
        mat_lbd1[i] -= eigs[0];
        mat_lbd2[i] -= eigs[1];
      }
    }
  }

  for (int i=0; i<3; i++) {
    for (int k=0; k<3; k++) {
      product[3*i+k] = mat_lbd1[3*i] * mat_lbd2[k] + mat_lbd1[3*i+1] * mat_lbd2[3+k] + mat_lbd1[3*i+2] * mat_lbd2[6+k];
    }
  }

  norm1 = product[0]*product[0] + product[3]*product[3] + product[6]*product[6];
  norm2 = product[1]*product[1] + product[4]*product[4] + product[7]*product[7];
  norm3 = product[2]*product[2] + product[5]*product[5] + product[8]*product[8];

  if (norm1 > norm2 && norm1 > norm3) {
    idx = 0;
    norm = norm1;
  }
  else if (norm2 > norm1 && norm2 > norm3) {
    idx = 1;
    norm = norm2;
  }
  else {
    idx = 2;
    norm = norm3;
  }

  norm = 1/sqrt(norm);

  vector[0] = product[idx]*norm;
  vector[1] = product[3+idx]*norm;
  vector[2] = product[6+idx]*norm;
}


//.....................................EIGENVALUE EXTREMIZATION.................................................


double neg_min_eig(double lbd, double* matA, double* matB){

  double lcb[9];
  double eigs[3];
  double result;

  for (int i=0; i<9; i++) {
    lcb[i] = lbd * matA[i] + (1 - lbd) * matB[i];
  }

  eigenvalues(lcb, eigs);

  result = -eigs[0];

  return result;
}


double med_eig(double lbd, double* matA, double* matB){

  double lcb[9];
  double eigs[3];
  double result;

  for (int i=0; i<9; i++) {
    lcb[i] = lbd * matA[i] + (1 - lbd) * matB[i];
  }

  eigenvalues(lcb, eigs);

  result = eigs[1];


  return result;
}


double max_min_eig_brent(double* matA, double* matB, int early_stop, double* t_point, int* feval){

  double a = r8_epsilon();
  double b = 1. - r8_epsilon();
  double e = r8_epsilon(); // relative error
  double t = 1.0E-7; // absolute error
  double solution;
  double initial = 0.;
  double *y = &initial;


  solution = -brent_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, neg_min_eig, t_point, feval);


  return solution;

}


double min_med_eig_brent(double* matA, double* matB, double* t_point, int* feval){

  double a = r8_epsilon();
  double b = 1. - r8_epsilon();
  double e = r8_epsilon(); // relative error
  double t = 1.0E-7; // absolute error
  int early_stop = 0;
  double solution;
  double initial = 0.;
  double *y = &initial;

  solution = brent_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, med_eig, t_point, feval);

  return solution;

}


double max_min_eig_gss(double* matA, double* matB, int early_stop, double* t_point, int* feval){

  double a = r8_epsilon();
  double b = 1. - r8_epsilon();
  double c = 0.5;
  double m = 100.;
  double mechep = r8_epsilon();
  double e = r8_epsilon(); // relative error
  double t = 1.0E-7; // absolute error
  double solution;

  solution = -gss_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, neg_min_eig, t_point, feval);

  return solution;

}

double min_med_eig_gss(double* matA, double* matB, double* t_point, int* feval){

  double a = r8_epsilon();
  double b = 1. - r8_epsilon();
  double c = 0.5;
  double m = 100.;
  double mechep = r8_epsilon();
  double e = r8_epsilon(); // relative error
  double t = 1.0E-7; // absolute error
  int early_stop = 0;
  double solution;

  solution = gss_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, med_eig, t_point, feval);

  return solution;

}


//.......................................CONTACT FUNCTION...................................................


void cross_product(double* coord, double* orient, double* cross) {

  cross[0] = coord[1]*orient[2] - coord[2]*orient[1];
  cross[1] = coord[2]*orient[0] - coord[0]*orient[2];
  cross[2] = coord[0]*orient[1] - coord[1]*orient[0];

}


double contact_function(double a0, double a1, double b0, double b1,
  double* coord0, double* orient0, double* coord1, double* orient1, int minimizer,
  double* other_results, int* fevals, int* branch) {

  /*
    Other results array: [min_eig_T, med_eig_T, min_vec_scalar, med_vec_scalar]
    fevals array: [feval_min, feval_med]
  */

  double a_inter;
  if (a0 < a1) {
    a_inter = a0;
    a0 = a1;
    a1 = a_inter;
  }

  double cross0[3];
  double cross1[3];
  double lin_interpolation[9];
  double qf0[9];
  double qf1[9];
  double min_eig;
  double med_eig;
  double eigvec_mineig[3];
  double mineig_coord0=0.;
  double mineig_coord1=0.;
  double inv2a0=1/(a0 * a0);
  double inv2a1=1/(a1 * a1);
  double inv2b0=1/(b0 * b0);
  double inv2b1=1/(b1 * b1);
  double limit=inv2a0;

  cross_product(coord0, orient0, cross0);
  cross_product(coord1, orient1, cross1);

  /* Trasformation of quadratic forms */
  for (int i=0; i<3; i++){
    for (int l=0; l<3; l++){
      qf0[3*i+l] = orient0[i]*inv2a0*orient0[l] + cross0[i]*inv2b0*cross0[l];
      qf1[3*i+l] = orient1[i]*inv2a1*orient1[l] + cross1[i]*inv2b1*cross1[l];
    }
  }

  if (minimizer == 0) {
    min_eig = max_min_eig_brent(qf0, qf1, 0, &other_results[0], &fevals[0]);
  }
  else if (minimizer == 1) {
    min_eig = max_min_eig_brent(qf0, qf1, 1, &other_results[0], &fevals[0]);
  }
  else if (minimizer == 2) {
    min_eig = max_min_eig_gss(qf0, qf1, 0, &other_results[0], &fevals[0]);
  }
  else if (minimizer == 3) {
    min_eig = max_min_eig_gss(qf0, qf1, 1, &other_results[0], &fevals[0]);
  }
  else {
    return -1.;
  }


  /* eigenvector corresponding to maximal 1st eigenvalue (1st intersection coordinate)*/
  for (int i=0; i<9; i++) {
    lin_interpolation[i] = other_results[0] * qf0[i] + (1 -   other_results[0]) * qf1[i];
  }
  eigenvector(lin_interpolation, 1, eigvec_mineig);

  /* scalar product between eigenvectors and position vectors for both ellipses in the pair */
  for (int i=0; i<3; i++) {
    mineig_coord0 += eigvec_mineig[i]*coord0[i];
    mineig_coord1 += eigvec_mineig[i]*coord1[i];
  }

  other_results[2] = mineig_coord0 * mineig_coord1;
  other_results[3] = 0;


  if (mineig_coord0 * mineig_coord1 > 0) {
    other_results[1] = -1; // meaningless result for med_eig_T
    fevals[1] = 0;
    if (min_eig < limit) {
      *branch = 0;
      return min_eig;
    }
    else {
      *branch = 2;
      return limit;
    }
  }

  else {

    if (minimizer == 0 || minimizer == 1) {
      med_eig = min_med_eig_brent(qf0, qf1, &other_results[1], &fevals[1]);
    }
    else if (minimizer == 2 || minimizer == 3) {
      med_eig = min_med_eig_gss(qf0, qf1, &other_results[1], &fevals[1]);
    }
    else {
      return -1.;
    }

    if (1.0E-4 < other_results[1] && other_results[1] < 1 - 1.0E-4 && med_eig < limit) {
      *branch = 1;
      return med_eig;
    }
    else {
      *branch = 2;
      other_results[1] = 1;
      return limit;
    }

  }

}


void contact_function_multi(int n, double a0, double a1, double b0, double b1,
  double* coords0, double* orients0, double* coords1, double* orients1, int minimizer,
  int* all_evals, double* results) {

  double other_results[4];
  int branch=0;
  int* branch_point = &branch;
  int fevals[2];
  fevals[0] = 0;
  fevals[1] = 0;

  for (int i=0; i<n; i++){
    results[i] = contact_function(a0, a1, b0, b1, &coords0[3*i], &orients0[3*i], &coords1[3*i], &orients1[3*i],
      minimizer, &other_results[0], fevals, &branch);
    all_evals[0] += fevals[0];
    all_evals[1] += fevals[1];
    fevals[0] = 0;
    fevals[1] = 0;
  }

}



double contact_mono_packing(double inv2a, double inv2b,
                            double* coord0, double* orient0,
                            double* coord1, double* orient1,
                            int try_second, int early_stop) {

  /*
    Simplified contact function calculation for monodispersed ellipse packing.
  */

  double cross0[3];
  double cross1[3];
  double lin_interpolation[9];
  double qf0[9];
  double qf1[9];
  double min_eig;
  double min_eig_T = 0.;
  double* meT_point = &min_eig_T;
  int fevals = 0;
  int* fevals_point = &fevals;

  cross_product(coord0, orient0, cross0);
  cross_product(coord1, orient1, cross1);

  /* Trasformation of quadratic forms */
  for (int i=0; i<3; i++){
    for (int l=0; l<3; l++){
      qf0[3*i+l] = orient0[i]*inv2a*orient0[l] + cross0[i]*inv2b*cross0[l];
      qf1[3*i+l] = orient1[i]*inv2a*orient1[l] + cross1[i]*inv2b*cross1[l];
    }
  }

  min_eig = max_min_eig_brent(qf0, qf1, early_stop, meT_point, fevals_point);

  if (try_second == 0 ){
    return min_eig;
  }

  else {  // The case where the 2nd eigenvalue must also be calculated

    double lin_interpolation[9];
    double eigvec_mineig[3];
    double mineig_coord0 = 0;
    double mineig_coord1 = 0;

    /* eigenvector corresponding to maximal 1st eigenvalue (1st intersection coordinate)*/
    for (int i=0; i<9; i++) {
      lin_interpolation[i] = min_eig_T * qf0[i] + (1 - min_eig_T) * qf1[i];
    }
    eigenvector(lin_interpolation, 1, eigvec_mineig);

    /* scalar product between eigenvectors and position vectors for both ellipses in the pair */
    for (int i=0; i<3; i++) {
      mineig_coord0 += eigvec_mineig[i]*coord0[i];
      mineig_coord1 += eigvec_mineig[i]*coord1[i];
    }

    if (mineig_coord0 * mineig_coord1 > 0) {
      return min_eig;
    }

    else {
      min_eig_T = 0.;
      fevals = 0;
      return min_med_eig_brent(qf0, qf1, meT_point, fevals_point);
    }

  }

}


double contact_mono_pack_multi(int n, double inv2a, double inv2b, double* coords, double* orients, double dist_lim) {

  double energy=0.;
  double cos_dist;
  double contact_f;
  int try_second=0;

  if (inv2a < 2) {
    try_second = 1;  // for large ellipses, 2nd eigenvalue might be the correct one
  }

  for (int i=0; i<n; i++) {
    for (int j=i+1; j<n; j++) {
      cos_dist = coords[3*i]*coords[3*j] + coords[3*i+1]*coords[3*j+1] + coords[3*i+2]*coords[3*j+2];
      if (cos_dist > dist_lim) {
        contact_f = contact_mono_packing(inv2a, inv2b, &coords[3*i], &orients[3*i], &coords[3*j], &orients[3*j],
                                          try_second, 1);
        if (contact_f < 1) {
          energy += contact_f + 1 / contact_f - 2;
        }
      }
    }
  }

  return energy;
}


double contact_bidisp_packing(double inv2a_0, double inv2b_0,
                              double inv2a_1, double inv2b_1,
                              double* coord0, double* orient0,
                              double* coord1, double* orient1,
                              int try_second, double omega) {

  /*
    Simplified contact function calculation for bidispersed ellipse packing.
  */

  double cross0[3];
  double cross1[3];
  double lin_interpolation[9];
  double qf0[9];
  double qf1[9];
  double min_eig;
  double med_eig;
  double min_eig_T = 0.;
  double* meT_point = &min_eig_T;
  int fevals = 0;
  int* fevals_point = &fevals;

  cross_product(coord0, orient0, cross0);
  cross_product(coord1, orient1, cross1);

  /* Trasformation of quadratic forms */
  for (int i=0; i<3; i++){
    for (int l=0; l<3; l++){
      qf0[3*i+l] = orient0[i]*inv2a_0*orient0[l] + cross0[i]*inv2b_0*cross0[l];
      qf1[3*i+l] = orient1[i]*inv2a_1*orient1[l] + cross1[i]*inv2b_1*cross1[l];
    }
  }

  min_eig = max_min_eig_brent(qf0, qf1, 1, meT_point, fevals_point);

  if (try_second == 0 ){
    if (min_eig < omega) {
      return min_eig;
      }
    else {
      return omega;
      }
  }

  else {  // The case where the 2nd eigenvalue must also be calculated

    double lin_interpolation[9];
    double eigvec_mineig[3];
    double mineig_coord0 = 0;
    double mineig_coord1 = 0;

    /* eigenvector corresponding to maximal 1st eigenvalue (1st intersection coordinate)*/
    for (int i=0; i<9; i++) {
      lin_interpolation[i] = min_eig_T * qf0[i] + (1 - min_eig_T) * qf1[i];
    }
    eigenvector(lin_interpolation, 1, eigvec_mineig);

    /* scalar product between eigenvectors and position vectors for both ellipses in the pair */
    for (int i=0; i<3; i++) {
      mineig_coord0 += eigvec_mineig[i]*coord0[i];
      mineig_coord1 += eigvec_mineig[i]*coord1[i];
    }

    if (mineig_coord0 * mineig_coord1 > 0) {
      if (min_eig < omega) {
        return min_eig;
        }
      else {
        return omega;
        }
    }

    else {
      min_eig_T = 0.;
      fevals = 0;
      med_eig = min_med_eig_brent(qf0, qf1, meT_point, fevals_point);

      if (med_eig < omega) {
        return med_eig;
        }
      else {
        return omega;
        }

    }
  }
}


double contact_bidisp_pack_multi(int n, double* inv2a, double* inv2b,
                                double* coords, double* orients, double dist_lim, double omega) {

  double energy=0.;
  double cos_dist;
  double contact_f;
  int try_second;

  for (int i=0; i<n; i++) {
    for (int j=i+1; j<n; j++) {

      cos_dist = coords[3*i]*coords[3*j] + coords[3*i+1]*coords[3*j+1] + coords[3*i+2]*coords[3*j+2];

      if (cos_dist > dist_lim) {

        if (inv2a[i] * inv2a[j] < inv2a[i] + inv2a[j]) {  // derived from the condition alpha0 + alpha1 > pi / 2
          try_second = 1;  // for large ellipses, 2nd eigenvalue might be the correct one
        }
        else {
          try_second = 0;
        }

        contact_f = contact_bidisp_packing(inv2a[i], inv2b[i], inv2a[j], inv2b[j],
                      &coords[3*i], &orients[3*i], &coords[3*j], &orients[3*j], try_second, omega);

        if (contact_f < 1) {
          energy += contact_f + 1 / contact_f - 2;
        }

      }
    }
  }

  return energy;
}