/*

  Rob Kutschke, CD/EXP, July 7, 2005.

  Third order estimate of the beam position, to be used with data
  from the upgraded Tevatron BPM system.

  Inputs:
    A, B - Magnitudes of signals on the A and B electrodes.
    y    - Estimate of the unmeasured orthogonal coordinate.
           Set to 0 if no information is available.

   Outputs:
    Function value: the value of the beam position.
    error -  1   = OK
            else = cubic polynomial has too many roots.

   Method:
     See Beams-doc-1893.

*/

#include "bposO3.h"
#include <math.h>

double bposO3( double A, 
	       double B, 
	       double y,
	       int* error
	       ){
  
  /* Control initialization of other static variables. */
  static int init = 1;

  /* Properties of the pickups:
     b   - physical radius
     phi - arc subtended by the pickup.
     cc  - coupling coefficient
  */
  static double b   = 35.;
  static double phi = 110./180*M_PI;
  static double cc  = 0.118098248052973;
  
  /* Expansion coefficients, without coupling. */
  static double a1, a2, a3;

  /* Coupling coefficient. */
  static double ccf;

  /* Expansion coefficients, with coupling. */
  static double b1, b2, b3;
  static double r31;

  /* Miscellaneous statics. */
  static double half        = 0.5;
  static double threehalves = 1.5;

  /* First order position estimate. */
  double x1;

  /* Coefficients of cubic in standard form, for DRTEQ3. */
  double r, s, t;

  /* Roots of the cubic, returned by DRTEQ3. */
  double c[3];

  /* Discriminant of the cubic, returned by DRTEQ3. */
  double disc;

  /* On first call, initialize static variables. */
  if ( init == 1  ) {
    init = 0;
    a1  = 4./phi/b*sin(half*phi);
    a2  = 2./phi/b/b*sin(phi);
    a3  = 4./3./phi/b/b/b*sin(threehalves*phi);

    ccf = (1.-cc)/(1.+cc);

    b1 = a1*ccf;
    b2 = a2;
    b3 = a3*ccf;
    
    r31 = b3/b1;
  }

  /* First order position estimate. */
  x1 = (A-B)/(A+B)/b1;

  /* Coefficients of cubic polynomial. */
  r = - b2*x1/r31;
  s = (1.-3.*r31*y*y)/r31;
  t = -x1*(1.-b2*y*y)/r31;

  /* Solve cubic. */
  drteq3_ ( &r, &s, &t, c, &disc);

  /* Normal return. */
  if ( disc > 0. ) {
    *error = 1;
    return c[0];
  }

  /* 
     Unexpected value for the discriminant.  
     Don't know which root to choose so make sure the value
     won't be confused with a real answer.
  */
  *error = -1;
  return 998.;

}