#define EXTERN  extern
#include        "define.h"

extern  float  PX,PY;

correctphase()
   {
   int          i,end;
   float        I,Q,n,m,*iqptr;

   /* rotate the doppler offset vector */
   /* use a double buffer to avoid weird values */
   if((*Flag & 1))
      {
      I  = PX * *Sp1 - PY * *Sp2;
      PY = PX * *Sp2 + PY * *Sp1;
      PX = I;
      }
   else
      {
      I  = PX * *Sp3 - PY * *Sp4;
      PY = PX * *Sp4 + PY * *Sp3;
      PX = I;
      }
   
   /* read in the measured phase of gate0, conjugate, and normalize */
   I = *G0invmag * *IQPTR;
   Q = *G0invmag * - *(IQPTR + 1);
   
   /* subtract-out the phase of the rotating vector */
   n = I * PX - Q * PY;  /* real part */
   m = I * PY + Q * PX;  /* imaginary part */
   
   /* estimate the new inverse magnitude of gate 0 */
   *G0invmag *= 1.5 - 0.5 * (n * n + m * m);  /* iterate to normalize n + jm */
   
   /* don't let it correct beyond 1/(2048 * 16384 * 64(MAXPW)) */
   if(*G0invmag < 4.6566e-10)  *G0invmag = 4.6566e-10;
   /* and less than 1/(1024 * 16384 * 2(MINPW) ) */
   if(*G0invmag > 2.9802e-8)  *G0invmag = 2.9802e-8;
   
   /* correct all of the remaining gates with m and n */
   for(i=0,end=*Gates,iqptr=IQPTR; i<end; i++)
      {
      I = *iqptr;
      Q = *(iqptr+1);
      *iqptr++ = n * I - m * Q;
      *iqptr++ = m * I + n * Q;
      }
   }