




/* The inner correlation calculated here returns the phase
   angle from the first vector to the second; it calculates
   the correlation of the conjugate of the first vector with
   the second.  The angle is returned in degrees.

   Tue  10-11-1988


*/


#include <stdio.h>
#include <math.h>

#ifndef M_PI
#define M_PI		3.14159265358979323846
#endif
#define good_float(x)  ( (x) < 0.99999e38 )

#if PROTOTYPE_ALLOWED
int inner_correlation(float *u0, float *v0, float *u1, float *v1, 
		      int n, double *amp, double *phase)
#else
int inner_correlation(u0, v0, u1, v1, n, amp, phase)
   float *u0, *v0, *u1, *v1;
   int n;
   double *amp, *phase;
#endif
{
   int i;
   int count;
   double u0m = 0.0, v0m = 0.0, u1m= 0.0, v1m= 0.0;
   double uu0 = 0.0, vv0 = 0.0, uu1 = 0.0, vv1 = 0.0;
   double u0u1 = 0.0, v0v1 = 0.0, u0v1 = 0.0, u1v0 = 0.0;
   double u0dm, v0dm, u1dm, v1dm;   /* demeaned */
   double co, quad, denom;

   /* Initialize the output, amp and phase, to zero, so they will
      be set in case of an early return due to lack of good points.
   */
   *amp = 0.0;
   *phase = 0.0;

   /* Calculate means first. Assume that if u is good, so is v.*/
   for (i=0, count = 0; i<n; i++)
   {
      if (good_float(u0[i]) && good_float(u1[i]))
      {
         u0m += u0[i];
         v0m += v0[i];
         u1m += u1[i];
         v1m += v1[i];
         count++;
      }
   }
   /* Two or more elements are needed for a valid calculation. */
   if (count < 2) return 0;

   u0m /= count;
   v0m /= count;
   u1m /= count;
   v1m /= count;

   for (i=0; i<n; i++)
   {
      if (good_float(u0[i]) && good_float(u1[i]))
      {
         /* Demean each element. */
         u0dm = u0[i] - u0m;
         v0dm = v0[i] - v0m;
         u1dm = u1[i] - u1m;
         v1dm = v1[i] - v1m;

         /* Accumulate the sums. */
         uu0 += u0dm * u0dm;
         vv0 += v0dm * v0dm;
         uu1 += u1dm * u1dm;
         vv1 += v1dm * v1dm;
         u0u1 += u0dm * u1dm;
         v0v1 += v0dm * v1dm;
         u0v1 += u0dm * v1dm;
         u1v0 += u1dm * v0dm;
      }
   }

   uu0 /= count ;
   vv0 /= count ;
   uu1 /= count ;
   vv1 /= count ;
   u0u1 /= count ;
   v0v1 /= count ;
   u0v1 /= count ;
   u1v0 /= count ;

   /* To get correlation from covariance, normalize by the
      product of the standard deviations.
   */
   denom = sqrt( (uu0 + vv0) * (uu1 + vv1) );
   if (denom == 0.0) return 0;
   co    = (u0u1 + v0v1) / denom;
   quad  = (u0v1 - u1v0) / denom;

   *amp     = sqrt( (co * co) + (quad * quad) );
   /* filter out floating point crash */
   if (co == 0)
      *phase = 90;
   else
      *phase   = atan2(quad, co) * (180 / M_PI);
   return count;
}

