/* survey3d.c - Standard three dimensional survey information processing.
 * (C) Copyright 1999 by John Halleck
 *     All Rights Reserved.
 */
/* Version of September 6th, 1999 */

#include "errors.h"
 /* We return package standard error codes. */

#include "mat.h"
 /* Standard matrix operations. */

#include "qinv.h"
 /* Matrix inversion */

#include "survey3d.h"
 /* Our basic data types */

/* #include "survey3debug.h" */
/* #include "matdebug.h" */
 /* DEBUG */

/* ---------------------- Conversions --------------------------------------- */
/* Load matrices from values. */

static error loads33 (double result[6],  double a11, double a12, double a13,
                                                     double a22, double a23,
                                                                 double a33) {
 if (!result) return ERRnil;
   result[0] = a11; result[1] = a12; result[2] = a13;
                    result[3] = a22; result[4] = a23;
                                     result[5] = a33;
 return NoError;
}

error loadcvr  (covariance result, double a11, double a12, double a13,
                                               double a22, double a23,
                                                           double a33) {
  return loads33 (result, a11, a12, a13,  a22, a23,  a33);
}

error loadwht  (covariance result, double a11, double a12, double a13,
                                               double a22, double a23,
                                                           double a33) {
  return loads33 (result, a11, a12, a13,  a22, a23,  a33);
}

/* Make a regular matrix from a weight or covariance. */
static error matfms33 (double result[3][3], double *given) {
  if (!result || !given) return ERRnil;

  result[0][0] = given[0];   result[0][1] = given[1];  result[0][2] = given[2];
  result[1][0] = given[1];   result[1][1] = given[3];  result[1][2] = given[4];
  result[2][0] = given[2];   result[2][1] = given[4];  result[2][2] = given[5];

  return NoError;
}

error matfmwht (double result[3][3], weight given) {
  return matfms33 (result, given);
} 

error matfmcvr (double result[3][3], covariance given) {
  return matfms33 (result, given);
}

/* ---------------------- identities ---------------------------------------- */

/* Set coordinate to additive identity */
error zerocoord (coordinate result) {
    if (!result) return ERRnil;
    result[0] = 0.0;  result[1] = 0.0; result[2] = 0.0;
    return NoError;
}

/* symmetric 3x3 identity */
static error ids33 (double *final) {
   if (!final) return ERRnil;
   final [0] = 1.0;      final [1] = 0.0;         final[2] = 0.0;
                         final [3] = 1.0;         final[4] = 0.0;
                                                  final[5] = 1.0;
   return NoError;
}

/* Set covariance matrix to multiplicitive identity */
error cvrident  (covariance result) { return ids33 ((double *)result); }
/* Set weight matrix to multiplicitive identity */
error whtident  (weight     result) { return ids33 ((double *)result); }

/* ------------------- Add the basic entities ------------------------------- */
error addcoord (coordinate result, coordinate a, coordinate b) {
   if (!result || !a || !b) return ERRnil;
   result[0] = a[0] + b[0]; result[1] = a[1] + b[1]; result[2] = a[2] + b[2];
   return NoError;
}

error subcoord (coordinate result, coordinate a, coordinate b) {
   if (!result || !a || !b) return ERRnil;
   result[0] = a[0] - b[0]; result[1] = a[1] - b[1]; result[2] = a[2] - b[2];
   return NoError;
}


static error adds33 (double result[6], double a[6], double b[6]) {
   if (!result || !a || !b) return ERRnil;
   result[0] = a[0] + b[0]; result[1] = a[1] + b[1]; result[2] = a[2] + b[2];
                            result[3] = a[3] + b[3]; result[4] = a[4] + b[4];
                                                     result[5] = a[5] + b[5];
   return NoError;
}

error addcvr  (covariance result, covariance a, covariance b) {
   return adds33 ((double *)result, a, b);
}

error addwht  (weight     result, weight     a, weight     b) {
   return adds33 ((double *)result, a, b);
}

/* ---------------- Multiply coordinate by weight --------------------------- */

/* weight by coordinate */
error wxcmult (coordinate result, weight a, coordinate b) {
   double x, y, z;  /* Original vector */
   /* Note that saving these before use not only makes the addressing
    * cleaner, but also removes the restriction that a and result
    * be distinct vectors.
    */
   if (!result || !a || !b) return ERRnil;

   /* We save the original coordinate off here, so that this
    * actually works if result and b are the same location.
    */
   x = b[0]; y = b[1]; z = b[2];

   result[0] = a[0] * x  +  a[1] * y  +  a[2] * z;
   result[1] = a[1] * x  +  a[3] * y  +  a[4] * z;
   result[2] = a[2] * x  +  a[4] * y  +  a[5] * z; 

   return NoError;
}

/* ------------ convert between weights and covariances --------------------- */

static error invs33 (double result[6], double given[6]) {
 double temp[3][3]; double outtemp[3][3];
 error status;
 if (!result || !given) return ERRnil;

 temp[0][0] = given[0];     temp[0][1] = given[1];    temp[0][2] = given[2];
 temp[1][0] = given[1];     temp[1][1] = given[3];    temp[1][2] = given[4];
 temp[2][0] = given[2];     temp[2][1] = given[4];    temp[2][2] = given[5];

 if ((status = invns (3, outtemp, temp, temp)))  return status;
 result[0] =outtemp[0][0]; result[1] =outtemp[0][1]; result[2] =outtemp[0][2];
                           result[3] =outtemp[1][1]; result[4] =outtemp[1][2];
                                                     result[5] =outtemp[2][2];

 return NoError;
}

error wht2cvr (covariance result, weight     given) {
   return invs33 (result, given);
}

error cvr2wht (weight     result, covariance given) {
   return invs33 (result, given);
}

/* ------------------ Invert weight or covariance matrices ------------------ */

error cvrinv (covariance result, covariance given) {
   return invs33 (result, given);
}

error whtinv (weight result, weight given) {
   return invs33 (result, given);
}

/* ------------------ Scale weight or covariance ---------------------------- */
/*  (used for setting unit weight and covariance)  */
/* Note that a reference variance or reference weight can't be negative or
 * zero in any physically meaningful way.
 */

static error scls33 (double out[6], double in[6], double scale) {
   if (!out || !in)  return ERRnil;
   if (scale == 1.0) return NoError;
   out[0] = scale * in[0];   out[1] = scale * in[1];   out[2] = scale * in[2];
                             out[3] = scale * in[3];   out[4] = scale * in[4];
                                                       out[5] = scale * in[5];
   if (scale == 0.0) return ERRmeaning;
   return NoError;
}

error sclcvr (covariance result, covariance given, double reference) {
   return scls33 (result, given, reference);
}

error sclwht   (weight     result, weight     given, double reference) {
   return scls33 (result, given, reference);
}

/* ------------------------  Magnitudes ------------------------------------- */

static error det3p (double *result, double given[6]) {
   if (!result || !given) return ERRnil;
#define A00 given[0]
#define A01 given[1]
#define A02 given[2]
#define A10 A01
#define A11 given[3]
#define A12 given[4]
#define A20 A02
#define A21 A12
#define A22 given[5]
 *result =   A00 * (A11 * A22  -  A21 * A12)
           - A01 * (A10 * A22  -  A20 * A12)
           + A02 * (A10 * A21  -  A20 * A11);
 return NoError;
}

error detcvr (double *result, covariance given) {
   return det3p (result, given);
}
error detwht (double *result, weight     given) {
   return det3p (result, given);
}