/* daixyz.c  - Convert from Distance Azimuth Inclination to X Y Z
 * (C) Copyright 1999 by John Halleck
 *     All Rights Reserved
 */

/* Version of August 13th, 1999 */

#include <math.h>
  /* We need the trig functions. */

#include "errors.h"
  /* We use our package wide error codes. */
#include "mat.h"
  /* We will use some matrix operations. */

#include "matdebug.h"
 /* we use the debugging routines to print and test matrices */

#include "survey3d.h"
/* Three dimensional covariances and weights. */

#include "daixyz.h"
 /* We need the definition of our own routines */


double workcvr[3][3];
double workwht[3][3];

/* covariance matrices have a number of statistical properties that
 * are necessary.  No physical system can produce a matrix that
 * violates these.
 */
error covarok (covariance given) {

  if (!given) return ERRnil; /* The matrix must exist. */

  /* It is positive definite, therefore: */

     /* The diagonal elements must be POSITIVE */
     /* Proof is in Golub and Van Loan's Matrix computations, page 141 */
     if (X(given) <= 0.0 ||
         Y(given) <= 0.0 ||
         Z(given) <= 0.0) return ERRfalse;

     /* No off diagonal element Gij can be larger than (Gii+Gjj)/2.0 */
     /* Proof is in Golub and Van Loan's Matrix computations, page 141 */
     if ( (2.0 * XY(given)) > (X(given) + Y(given)) ) return ERRfalse;
     if ( (2.0 * XZ(given)) > (X(given) + Z(given)) ) return ERRfalse;
     if ( (2.0 * YZ(given)) > (Y(given) + Z(given)) ) return ERRfalse;

     /* Olly Betts <olly@muscat.co.uk> points out that a tighter bound exists,
      * in that Mij^2 < Mii*Mjj
      * This code needs to be fixed to use the tighter bound.
      * (And to document the proof)
      */

  return NoError;
}

error daixyz (xyz      result,     /* The final X, Y, Z */
            covariance shotcovariance, /* covariance matrix of the result */
            dai        data,           /* Initial data */
            covariance datacovariance  /* Covariance of data */
           ) {

   double jacobian[3][3]; /* The jacobian matrix we will compute */
   double sina, cosa, sini, cosi; /* Intermediate results */
   double temp[3][3]; /* Another intermediate result */
   error status; /* status out of matrix routines. */

   if (!shotcovariance || !result || !data || !datacovariance) return ERRnil;
      /* All matrices must be given */

#define x result[0]
#define y result[1]
#define z result[2]

#define D data[0]
#define A data[1]
#define I data[2]

   if (D <= 0.0) return ERRnumeric; /* produces singular matrices. */

/*  The standard conversion is:
 * x =   sine(A) * cosine(I) * D
 * y = cosine(A) * cosine(I) * D
 * z =               sine(I) * D
 */

   sina = sin(A); cosa = cos(A);
   sini = sin(I); cosi = cos(I);

   x =   sina * cosi * D;
   y =   cosa * cosi * D;
   z =          sini * D;

/* Using P(a,b) to mean the partial derivative of a with respect to b,
 * the jacobian of the standard transformation is:
 *
 * [ P(sin(A)*cos(I)*D,D)  P(sin(A)*cos(I)*D,A)  P(sin(A)*cos(I)*D,I) ]
 * [ P(cos(A)*cos(I)*D,D)  P(cos(A)*cos(I)*D,A)  P(cos(A)*cos(I)*D,I) ]
 * [        P(sin(I)*D,D)         P(sin(I)*D,A)         P(sin(I)*D,I) ]
 *
 * Which a little calculus reduces to:
 *
 * [  sin(A)*cos(I)         cos(A)*cos(I)*D     -sin(A)*sin(I)*D ]
 * [  cos(A)*cos(I)        -sin(A)*cos(I)*D     -cos(A)*sin(I)*D ]
 * [         sin(I)        0                            cos(I)*D ]
 */

  jacobian[0][0] =    sina*cosi;
  jacobian[0][1] =                 cosa*cosi*D;
  jacobian[0][2] =                                -sina*sini*D;
  jacobian[1][0] =    cosa*cosi; 
  jacobian[1][1] =                -sina*cosi*D;
  jacobian[1][2] =                                -cosa*sini*D;
  jacobian[2][0] =         sini;
  jacobian[2][1] =                       0;
  jacobian[2][2] =                                      cosi*D;

/* The covariance of the final shots is just:
 *   Jacobian*datacovariance*Jacobian'
 * (Where A' means A transpose)
 */
  matprint ("Jacobian was:", 3, 3, jacobian);

  if ( (status = matfmcvr (workcvr, datacovariance)) ) return status;

  if ((status = matmul (3, 3, temp, 3, jacobian, workcvr) ))
     return status;
  matprint ("Temp was: ", 3, 3, temp);
  if ((status = matmult (3, 3, workcvr, 3, temp, jacobian) ))
     return status;

/* ***************  UNFORTUNATELY ******************* */
/* The derivation above produces a jacobian that may not be full rank.
 * (For example with a vertical shot) and may be rank deficient
 * in general.
 * Note that this doesn't make any real difference, since the 
 * Uncertainties of the end points can't be zero, so adding
 * them will make it non-singular.
 *
 * Note that the underlying reason for this is that in the virtical
 * case we are claiming three independent measurements, but only
 * using two.  Therefore that one makes no difference, and the
 * partials it adds to the jacobian make it singular.
 * Another approach is to handle that case as a case of only
 * one significant measurement...
 */

 shotcovariance[0] = workcvr[0][0];
 shotcovariance[1] = workcvr[0][1];
 shotcovariance[2] = workcvr[0][2];
 shotcovariance[3] = workcvr[1][1];
 shotcovariance[4] = workcvr[1][2];
 shotcovariance[5] = workcvr[2][2];

  return NoError;
}