/*
 * file:   curvature_proc.c
 * author: Hinke Osinga and Reza Rokni --- University of Exeter
 * date:   April 2003
 */

/* curvature_proc(struct Fixpt_DataS*, int, double)
 * triangle(double*, double*, double*, int, 
            double, double, double, double, double)
 */


#include <stdio.h>
#include "constants.h"
#include "defaults.h"
#include "pm.h"
#include "math_utils.h"
#include "flow.h"
#include "fixed.h"
#include <math.h>
#include "ssman1d.h"



int curvature_proc(struct Fixpt_DataS* fp, int tot_len, 
		   double arclength_before)
{

  int        I = 0, J = 2;
  int        tot_dim = fp->prop_cntl.ph_space_dim;
  int        var_dim = tot_dim - 1;
  int        status = 0;
  int        p = 0, q = 1, r = 2, k=0;
  int        jj = 0;
  int        flag = 1;

  double     X[tot_dim], Y[tot_dim], Z[tot_dim];
  double     alpha_min = *((double *) pm(GET, "SSMan1D.alpha_min", NULL));
  double     alpha_max = *((double *) pm(GET, "SSMan1D.alpha_max", NULL));
  double     Delta_alpha_min = *((double *) pm(GET, "SSMan1D.Delta_alpha_min", NULL));
  double     Delta_alpha_max = *((double *) pm(GET, "SSMan1D.Delta_alpha_max", NULL));
  double     arc = 0, arclength_after = 0;
  
  /* 
  fprintf(stdout,"\n******curvature is successfully started**********\n");
  */

  /* J = 2; */
  while (r <= tot_len && flag != -1){
    for(k = 0; k < tot_dim; k++){
      X[k] = fp->prop_cntl.traj_segment[p][k];
      Y[k] = fp->prop_cntl.traj_segment[q][k];
      Z[k] = fp->prop_cntl.traj_segment[r][k];
    }
    status = triangle(X,Y,Z,var_dim,alpha_min,alpha_max,
		      Delta_alpha_min,Delta_alpha_max);
    /* fprintf(stdout, "\nCheck %d %d %d -- status = %d", p, q, r, status); */
    
    if(status == 0){
      /* this point can safely be skipped */ 
      if(r == tot_len){/* except when it is the last point */
	for (jj = 0; jj < tot_dim; jj++)
	  fp->prop_cntl.traj_segment[J][jj] = 
	    fp->prop_cntl.traj_segment[r][jj]; 
	J++;
      }
      r += 1;
    }
    if(status == 1){ /* save this point and continue */
      for (jj = 0; jj < tot_dim; jj++)
	fp->prop_cntl.traj_segment[J][jj] = 
	  fp->prop_cntl.traj_segment[r][jj]; 
      J++;
      p = q, q = r, r += 1;
    }
    if(status == 2){
      if(r != q + 1) {/* include again previously erased point */
	p = q, q = r - 1;
	for (jj = 0; jj < tot_dim; jj++)
	  fp->prop_cntl.traj_segment[J][jj] = 
	    fp->prop_cntl.traj_segment[q][jj]; 
	J++;
      }
      else{/* this is an accuracy violation */
	flag = -1;
	if (r > q+1){ /* Save last point that satisfied accuracy */
	  r += -1;
	  for (jj = 0; jj < tot_dim; jj++)
	    fp->prop_cntl.traj_segment[J][jj] = 
	      fp->prop_cntl.traj_segment[r][jj]; 
	  J++;
	}
      }
    }
  }

  arc = 0;
  for (jj = 0; jj < var_dim; jj++) 
    arc += pow((fp->prop_cntl.traj_segment[0][jj]-fp->x1[jj]),2);
  arc = sqrt(arc);
  arclength_after = arc;

  for(I = 1; I < J; I++){
    arc = 0;
    for (jj = 0; jj < var_dim; jj++)
      arc += pow(fp->prop_cntl.traj_segment[I][jj] -
		 fp->prop_cntl.traj_segment[I-1][jj],2);
    arclength_after += sqrt(arc);
  }

  if (flag == -1){
    fprintf(stdout,"\n\tCurvature accuracy violation at arclength %.10f",
	    arclength_after);
    fprintf(stdout,
	    "\n\tDecrease integration `Step size' in `Orbits' window\n");
  }

  fprintf(stdout,"\nNumber of mesh points on the manifold: %d",J);
  fprintf(stdout,"\nPoint reduction by curvature: removed %d of %d points",
	  tot_len-J, tot_len);
  /* Typically, arclength after point reduction is slightly less */
  /* It may be substantially less if a curvature violation occurred */
  fprintf(stdout,"\nArclength before point reduction = %.10f",
	  arclength_before);
  fprintf(stdout,"\nArclength after  point reduction = %.10f",
	  arclength_after);
  
  return(J);
}

int triangle(double *x, double *y, double *z, int var_dim,
	     double alpha_min, double alpha_max,
	     double Delta_alpha_min, double Delta_alpha_max)
{

  int    k;
  int    status = 1;
  double alpha = 0;
  double del_xy = 0;
  double del_yz = 0;
  double del_xW = 0;
  double W;

  for(k = 0; k < var_dim; k++){
    del_yz += (y[k] - z[k])*(y[k] - z[k]);
    del_xy += (x[k] - y[k])*(x[k] - y[k]);
  }
  del_yz = sqrt(del_yz);
  del_xy = sqrt(del_xy);

  /* Approximate angle using sin(phi) = phi for phi close to 0 */
  for(k = 0; k < var_dim; k++){
    W = y[k] + (del_xy/del_yz)*(y[k] - z[k]);
    del_xW += (x[k] - W)*(x[k] - W);
  }
  del_xW = sqrt(del_xW);
  
  alpha = del_xW/del_xy;
  
  /* Check curvature conditions */
  if(del_yz*alpha > Delta_alpha_max || alpha > alpha_max){
    /* points are too far apart */
    fprintf(stdout, "\n\n\talpha = %.10f, (Delta alpha) = %.10f", 
	    alpha, del_yz*alpha);
    status = 2;
    return(status);
  }
  if(del_yz*alpha < Delta_alpha_min && alpha < alpha_min){
    /* more than accurate enough */
    /* status = (del_yz < Delta_min) ? 0 : 1; */
    status = 0;
    return(status);
  }
  return(status);
}












