#ifdef OS_LINUX
#define f2cFortran
#endif

#include <stdio.h>
#include <math.h>
#include "cfortran.h"
#include <hbook.h>

#define sqr(x) ((x)*(x))
#define PI 3.14159

float * quad(float *x, float a, float b, float c) {

  float delta;
  float tmp;

  delta = b*b-4*a*c;
  if (delta<0.0) return NULL;
  tmp=sqrt(delta);
  x[0] = (0.5/a)*(-b+tmp);
  x[1] = (0.5/a)*(-b-tmp);

  return x;
}

float * intersect(float * x, float x0, float y0, float z0,
                  float theta, float phi, float r) {
  float px, py, pz, a, b, c;
  float x_tmp[2];

  theta = theta*PI/180.;
  phi = phi*PI/180.;

  px = sin(theta)*cos(phi);
  py = sin(theta)*sin(phi);
  pz = cos(theta);

  if (fabs(px)<0.0001) {
    if (fabs(py)<0.0001) return NULL;
    else {
      x[0] = 0.0;
      x[1] = (py>0.0)? r: -r;
      x[2] = pz*(x[1]-y0)/py + z0;
    }
  }
  else {
    a = 1+sqr(py/px);
    b = 2*py*y0/px-2*sqr(py/px)*x0;
    c = sqr(y0)+sqr(py*x0/px)-2*py/px*x0*y0-sqr(r);
    
    if (!quad(x_tmp, a, b, c)) return NULL;
    if (cos(phi) > 0.0) 
      x[0] = (x_tmp[0]>x0)? x_tmp[0]: x_tmp[1];
     else 
       x[0] = (x_tmp[0]<x0)? x_tmp[0]: x_tmp[1];
    x[1] = py*(x[0]-x0)/px + y0;
    x[2] = pz*(x[0]-x0)/px + z0;

    return x;
    }
}

/********************************************************************* 
* point1 and point2 define a line, p1 and p2 are intersecting points * 
*   b/w this line and a cylinder with radium r, along z              *   
*                                                                    *
*********************************************************************/

float * intersect2(float *p1, float *p2, float *point1, 
		   float *point2, float r) {

  float x0, y0, z0, x1, y1, z1;
  float px, py, pz, a, b, c;
  float x_tmp[2];

  x0 = point1[0]; y0 = point1[1], z0= point1[2];
  x1 = point2[0]; y1 = point2[1], z1= point2[2];

  px = x1-x0; py = y1-y0; pz = z1-z0;
  if (fabs(px)<0.00001) {
    if (fabs(py)<0.00001) return NULL;
    p1[0]=p2[0]=x0;
    p1[1] = sqrt(sqr(r)-sqr(x0));
    p2[1] = -sqrt(sqr(r)-sqr(x0));
  }
  else {
    a = 1+sqr(py)/sqr(px);
    b = 2*y0*py/px - 2*x0*sqr(py)/sqr(px);
    c = sqr(y0) - 2*y0*x0*py/px + sqr(py*x0/px) - sqr(r);
    if (quad(x_tmp, a, b, c) == NULL) return NULL; 
    p1[0] = x_tmp[0];
    p2[0] = x_tmp[1];
    p1[1] = y0+(p1[0]-x0)*py/px;
    p2[1] = y0+(p2[0]-x0)*py/px;
  }
    p1[2] = z0+(p1[0]-x0)*pz/px;
    p2[2] = z0+(p2[0]-x0)*pz/px;
  
    return p1;
}
  

float dif(float * x1, float * x2)
{
  float tmp;
 
  tmp = sqr((x1[0]-x2[0]))+sqr((x1[1]-x2[1]))+sqr((x1[2]-x2[2]));
  return tmp;
}

/******************************************************************/

float path_length_tgt(float *p1, float *p2, float *tgt_offset)
{
  double x0,y0,z0,x1,y1,z1,x2,y2,z2,pyx,r;
  double a, b, c;
  float x[2],y[2],z[2];
  float point1[3], point2[3];
  float l_edge, r_edge, l_z, r_z, path_length;
  int i;

  x0 = tgt_offset[0]; y0 = tgt_offset[1]; z0 = tgt_offset[2];
  x1 = p1[0]; y1 = p1[1]; z1 = p1[2];
  x2 = p2[0]; y2 = p2[1]; z2 = p2[2];
  r = 20;
  l_edge = z0 - 25;
  r_edge = z0 + 25;

  if (x2 != x1) {
    pyx = (y2-y1)/(x2-x1);
    a = 1+sqr(pyx);
    b = 2*(-x0-x1*sqr(pyx)+pyx*(y1-y0));
    c = x0*x0+sqr(pyx*x1)-2*pyx*(y1-y0)*x1+sqr(y1-y0)-r*r;
    if (!quad(x, a, b, c)) return 0;
    for (i=0;i<2;i++) {
      y[i] = y1+pyx*(x[i]-x1);
      z[i] = z1+(z2-z1)/(x2-x1)*(x[i]-x1);
    }
  }
  else {
    x[0]=x[1]=x1;
    if (y1 != y2) {
      y[0] = y0+sqrt(sqr(r)-sqr(x1-x0));
      y[1] = y0-sqrt(sqr(r)-sqr(x1-x0));
      z[0] = z1+(y[0]-y1)*(z2-z1)/(y2-y1);
      z[1] = z1+(y[1]-y1)*(z2-z1)/(y2-y1);
    }
    else return 0;
  }
  
  if (z[0]<z[1]) {
    point1[0]=x[0]; point1[1]=y[0]; point1[2]=z[0];
    point2[0]=x[1]; point2[1]=y[1]; point2[2]=z[1];
  }
  else {
    point1[0]=x[1]; point1[1]=y[1]; point1[2]=z[1];
    point2[0]=x[0]; point2[1]=y[0]; point2[2]=z[0];
  }

  if (point1[2]>r_edge || point2[2]<l_edge) return 0;
  if (point1[2]<l_edge)
    l_z = l_edge;
  else
    l_z = point1[2];
  if (point2[2]>r_edge)
    r_z = r_edge;
  else
    r_z = point2[2];
  path_length = sqrt(dif(point1,point2));
  if (point1[2] == point2[2])
    return path_length;
  path_length = path_length*(r_z-l_z)/(point2[2]-point1[2]);
  
  return path_length;
}

#define Z_MIN 4.0
#define Z_MAX 10.0
#define X_MIN -4.0
#define X_MAX 3.0
#define Y_MIN -8.0
#define Y_MAX 8.0

int pawc_[300000];
main() {
  
  FILE * fin, * fout;
  int ISTAT, ICYCLE;
  long int n_total, n_correct;
  float p1[3], p2[3], offset[3], pathlength, tdc, ratio, error;
  float x, y, z;
  int LREC=1024;
  int X_BIN, Y_BIN, Z_BIN; 
  HLIMIT(200000);

  Z_BIN = (int)(2*(Z_MAX-Z_MIN));
  X_BIN = (int)(2*(X_MAX-X_MIN));
  Y_BIN = (int)(2*(Y_MAX-Y_MIN));
  
  HBOOK2(1,"ratio in x-z", X_BIN, X_MIN,X_MAX,Z_BIN,Z_MIN,Z_MAX,(float)0);
  HBOOK2(2,"ratio in x-y", X_BIN, X_MIN,X_MAX,Y_BIN,Y_MIN,Y_MAX,(float)0);
  HBOOK2(3,"ratio in y-z", Y_BIN, Y_MIN,Y_MAX,Z_BIN,Z_MIN,Z_MAX,(float)0);

  if(!(fout = fopen("xy.dat", "w"))) {
    printf("can't open xy.dat and exit\n");
    exit(1);
  }
 
  for (z=Z_MIN; z<=Z_MAX; z=z+0.5)
    for (y=Y_MIN; y<=Y_MAX; y=y+0.5) {
      x = -0.5;
      offset[0] = (float)x;
      offset[1] = (float)y;
      offset[2] = (float)z;
      if(!(fin = fopen("ch1_p_tdc.dat", "r"))) {
	printf("can't open ch1_p_tdc.dat and exit\n");
	exit(1);
      }
     
      n_total=n_correct=0;
      while(fscanf(fin,"%f%f%f%f%f%f%f\n",&(p1[0]),&(p1[1]),&(p1[2]),&(p2[0]),&(p2[1]),&(p2[2]),&tdc) != EOF) {
	
	
	pathlength = path_length_tgt(p1,p2,offset);
	if (tdc>-10) {
	  n_total++;
	  if (pathlength>0)
	    n_correct++;
	}
      }
      close(fin);
      ratio = (float)n_correct/(float)n_total;
      error = sqrt((ratio+ratio*ratio)/n_total);
      error = error/ratio*100.;
      printf("x=%f,y=%f,z=%f, %f, %6.3f%%\n",offset[0],offset[1],offset[2],ratio, error);
      fprintf(fout,"x=%f,y=%f,z=%f, %f, %f\n",offset[0],offset[1],offset[2],ratio, error);
      HF2(1,offset[0],offset[2],ratio*1000.);
      HF2(2,offset[0],offset[1],ratio*1000.);
      HF2(3,offset[1],offset[2],ratio*1000.);
    }
  HROPEN(1,"xy","xy.rz","N",LREC,ISTAT);
  HROUT(0,ICYCLE,"N");
  HREND("xy");
  close (fout);
}