/*
 *   THIS FILE IS UNDER RCS - DO NOT MODIFY UNLESS YOU HAVE
 *   CHECKED IT OUT USING THE COMMAND CHECKOUT.
 *
 *    $Id: Glock.cpp,v 1.2 2006/05/22 16:01:25 paulf Exp $
 *
 *    Revision history:
 *     $Log: Glock.cpp,v $
 *     Revision 1.2  2006/05/22 16:01:25  paulf
 *     added from hydra_proj, new version
 *
 *     Revision 1.3  2005/10/17 06:34:31  davidk
 *     Modified the way phase weighting is handled in the locator.  The person who
 *     originally put it in, did not understand how the least square solver worked and
 *     wrongly applied the weighting values.
 *     Now weighting is done in integral increments ranging from 0 to 4.
 *     Currently any phase with weight below (GLOCK_WEIGHT_GRANULARITY/2)
 *     is not passed to the solver.
 *
 *     Revision 1.2  2005/10/13 22:21:58  davidk
 *     Fixed a bug where the locator was performing multiple relocations with the same
 *     starting location, instead of refining the location by using the results of the
 *     previous location as the starting point for the next.
 *     Made an aesthetic change to the calculation of the depth coefficient.
 *
 *     Revision 1.1.1.1  2005/06/22 19:30:48  michelle
 *     new directory tree built from files in HYDRA_NEWDIR_2005-06-20 tagged hydra and earthworm projects
 *
 *     Revision 1.8  2005/02/28 23:42:11  davidk
 *     Downgraded a debug message.
 *
 *     Revision 1.7  2005/02/15 20:02:46  davidk
 *     Modified the gap calculation (for the origin) in the locator, so that it is limited
 *     to P-class (within 110 degrees distance) and PKPdf phases.
 *     This way some flukily associated S or ancillary phase doesn't affect the
 *     quality of the gap metric.
 *
 *     Revision 1.6  2004/12/02 17:51:20  davidk
 *     Incorporated phase-based LocWeight values, that cause different phases
 *     to contribute different amounts to affecting the location. Utilized LocWeight
 *     in weight values submittted to the Solver, and in determining RMS, so
 *     that picks that aren't weighted in the solution to not adversly skew
 *     the RMS value even if they have a high residual value. (This is particularly
 *     important with phases like P'P', that have huge residual values, but should
 *     be associated with the origin, yet not contribute to it's location.
 *
 *     Revision 1.5  2004/11/30 02:48:52  davidk
 *     Found bug where input params for the dtdz vertical slowness parameter
 *     were being input in degrees (same unit as dtdx).  Converted the output
 *     from degrees to km, before using it to adjust the origin depth.
 *
 *     Inverted the dtdz coefficient so that it was negative instead, the same way
 *     the other coefficients were.
 *
 *     Revision 1.4  2004/11/01 06:33:21  davidk
 *     Modified to work with new hydra traveltime library.
 *     Cleaned up struct ITravelTime references.
 *
 *     Revision 1.3  2004/09/16 01:09:38  davidk
 *     Added a configable variable for the number of Iterations that the Locator-Solver runs
 *     each time an origin is modified.  Initialized it to the previously hardcoded value of 3-net-1.
 *
 *     Cleaned up logging messages.
 *     Downgraded some debugging messages.
 *
 *     Revision 1.2  2004/04/01 22:09:18  davidk
 *     v1.11 update
 *     Now utilizes OPCalc routines for calculating Origin/Pick association params.
 *
 *     Revision 1.3  2003/11/07 22:42:29  davidk
 *     Added RCS Header.
 *     Added function Locate_InitOrigin() that initializes a blank
 *     origin Structure for use with the locator.
 *     Added CompareOrigins() function for comparing the "quality"
 *     of two origin estimates for the same quake.
 *     Used function in code that formerly compared origins based on
 *     RMS values.
 *
 *     Revision 1.3  2003/11/07 22:38:59  davidk
 *     Added RCS Header.
 *     Added two member functions: Locate_InitOrigin() and CompareOrigins().
 *
 *
 **********************************************************/

// glock.cpp

#include <windows.h>
#include <stdio.h>
#include <math.h>
#include <Debug.h>
#include <ISolve.h>
#include <opcalc.h>
#include "glock.h"
#include "GlockMod.h"
//#include "date.h"
//#include "str.h"
extern "C" {
#include "utility.h"
}

#define RAD2DEG  57.29577951308
#define DEG2RAD	0.01745329251994
#define MAX_BELIEVABLE_DISTANCE_FOR_P_PHASE 110.0

extern "C" {
void __cdecl qsortd (double *base, unsigned num);
}

extern CMod *pMod;

#define ITER_SUCCESS	0
#define	ITER_NODATA		1
#define	ITER_DEPTH		2

// Carl 120303
static double pPickDistArray[OPCALC_MAX_PICKS];
static double pPickAzmArray[OPCALC_MAX_PICKS];

/*
static int PickAzmComp(const void *az1, const void *az2) {
	double azm1;
	double azm2;
	memmove(&azm1, az1, sizeof(double));
	memmove(&azm2, az2, sizeof(double));
	if(azm1 < azm2)
		return -1;
	if(azm1 > azm2)
		return 1;
	return 0;
}

// End Carl 120303

static int PickDistComp(const void *elem1, const void *elem2) {
	PICK *pck1;
	PICK *pck2;
	memmove(&pck1, elem1, sizeof(PICK *));
	memmove(&pck2, elem2, sizeof(PICK *));
	CDebug::Log(DEBUG_MINOR_INFO,"Comp() %.2f %.2f\n", pck1->dDelta, pck2->dDelta);
	if(pck1->dDelta < pck2->dDelta)
		return -1;
	if(pck1->dDelta > pck2->dDelta)
		return 1;
	return 0;
}
***************************************************/

//---------------------------------------------------------------------------------------CWave
CGlock::CGlock() {
//	DebugOn();
	for(int i=0; i<4; i++)
		bFree[i] = true;
  CDebug::SetModuleName("CGlock");
}

//---------------------------------------------------------------------------------------~CWave
CGlock::~CGlock() {
}

//---------------------------------------------------------------------------------------Locate
// Locate earthquake with constraints. E.G. set tnez to "TZ" to allow free
// origin time and depth, tnez set to "TNE" is a fixed depth solution.
// T = Time, N = North, E = East, Z = Depth
int CGlock::Locate(char *ent, char *mask) {
	int i;
	int res;

  // Carl 120303  - reversed each one of the true/false definitions
	for(i=0; i<4; i++)
		bFree[i] = false;
	for(i=0; i<(int)strlen(mask); i++) {
		switch(mask[i]) {
		case 't':
		case 'T':
			bFree[0] = true;
			break;
		case 'n':
		case 'N':
			bFree[1] = true;
			break;
		case 'e':
		case 'E':
			bFree[2] = true;
			break;
		case 'z':
		case 'Z':
			bFree[3] = true;
			break;
		default:
			return 7001;
		}
	}
	res = Locate(ent);
	for(i=0; i<4; i++)
		bFree[i] = true;
	return res;
}

//---------------------------------------------------------------------------------------Locate
// Locate earthquake (retrieve data from glint, update event in glint)
int CGlock::Locate(char *ent) {
	PICK *pck;
	double torg;
	double dis;
	double azm;
  double dDistKM;
	int res;
	bool fix;
	bool btemp;
  static bool bParamsInitialized = false;



  if(!bParamsInitialized)
  {
    pGlnt = pMod->pGlint;
    pTT = pMod->pTT;
    OPCalc_SetTravelTimePointer(pTT);
    pSlv = pMod->pSolve;
    bParamsInitialized = true;
  }


	pOrg = pGlnt->getOrigin(ent);
	if(!pOrg)
		return 1;
  if(pOrg->nPh < 4)  // we need atleast 4 phases for hypocenter processing 
    return(1);

	torg = pOrg->dT;
  nPck = 0;
  int iPickRef=0;
	while(pck = pGlnt->getPicksFromOrigin(pOrg, &iPickRef)) 
  {
    // DK 011404  record the min and max pick id's used in this
    //            location for debugging purposes
    if(nPck == 0)
    {
      strcpy(pOrg->idMinPick, pck->idPick);
      strcpy(pOrg->idMaxPick, pck->idPick);
    }
    else
    {
      if(strcmp(pOrg->idMinPick,pck->idPick) > 0)
        strcpy(pOrg->idMinPick, pck->idPick);
      else if(strcmp(pOrg->idMaxPick,pck->idPick) < 0)
        strcpy(pOrg->idMaxPick, pck->idPick);
    }

		geo_to_km_deg(pOrg->dLat, pOrg->dLon, pck->dLat, pck->dLon, &dDistKM, &dis, &azm);
//		CDebug::Log(DEBUG_MINOR_INFO,"Locate(): PCK:%s t:%.2f lat:%.4f lon:%.4f elev:%.4f dis:%.2f azm:%.2f\n",
//			pck->idPick, pck->dT-torg, pck->dLat, pck->dLon, pck->dElev, dis, azm);
		pPick[nPck++] = pck;
	}

  // DK Added 081303
  ORIGIN oBest, oCurrent, oNext;
  memcpy(&oCurrent,pOrg,sizeof(ORIGIN));
  Locate_InitOrigin(&oNext, pOrg->idOrigin);
  Locate_InitOrigin(&oBest, pOrg->idOrigin);

	fix = false;
  /* Note we must loop through two times before we start looping
     through locator iterations.  Something about the first time
     through calcs residuals for the existing origin, and the
     second time loads the solver's matrix.  So the 3rd Iterate()
     call is the first one that results in an actual relocation.
   **************************************************************/
	for(nIter=0; nIter<(pMod->iNumLocatorIterations+2); nIter++) {
		if(fix) {
			btemp = bFree[3];
			bFree[3] = false;
    }  // end if(fix)
    res = Iterate(true, &oCurrent, &oNext);
	  CDebug::Log(DEBUG_MINOR_INFO,"Locate(): Trying location: %.4f %.4f %.4f - %.2f (%.f/%.1f-%3d)\n", 
                oCurrent.dLat, oCurrent.dLon, oCurrent.dZ, 
                oCurrent.dT, oCurrent.dAff, oCurrent.dRms, oCurrent.nEq);
		if(fix) {
			fix = false;
			bFree[3] = btemp;
    }  // end if(fix)
    if(CompareOrigins(&oBest, &oCurrent) > 0)
      memcpy(&oBest,&oCurrent, sizeof(ORIGIN));

		switch(res) {
		case ITER_SUCCESS:
			break;
		case ITER_NODATA:
			pOrg->nEq = 0;
			pOrg->dRms = 0;
			return 0;
			break;
		case ITER_DEPTH:
			fix = true;
			break;
		}
    // make the next origin the current one and reprocess
    memcpy(&oCurrent,&oNext, sizeof(ORIGIN));
  }  // end for 3 iterations
  /* process the last origin */
  res = Iterate(false, &oCurrent, &oNext);
	  CDebug::Log(DEBUG_MINOR_INFO,"Locate(): Trying location: %.4f %.4f %.4f - %.2f (%.1f-%3d)\n", 
                oCurrent.dLat, oCurrent.dLon, oCurrent.dZ, 
                oCurrent.dT, oCurrent.dRms, oCurrent.nEq);
    if(CompareOrigins(&oBest, &oCurrent) > 0)
      memcpy(&oBest,&oCurrent, sizeof(ORIGIN));

  /* reset the originpick information to the best. */
  res = Iterate(false, &oBest, &oNext);

  if(oBest.nPh > 4)
    memcpy(pOrg,&oBest, sizeof(ORIGIN));
  else
  {
	  CDebug::Log(DEBUG_MAJOR_INFO,"Locate(): Unable to generate location using "
                                 "atleast 4 phases for Origin %s nPh:%d nEq:%d\n",
                oBest.idOrigin, oBest.nPh, oBest.nEq);
    return(1);
  }

	CDebug::Log(DEBUG_MAJOR_INFO,"Locate(): Chose location : %.4f %.4f %.4f - %.2f (%.1f-%3d)\n", 
              oBest.dLat, oBest.dLon, oBest.dZ, 
              oBest.dT, oBest.dRms, oBest.nEq);
	return 0;
} // end Locate()

//---------------------------------------------------------------------------------------Iterate
int CGlock::Iterate(bool solve, ORIGIN * poCurrent, ORIGIN * poNext) 
{
	/*
  PICK *pck;
	TTT *ttt;
	double tres;
	double coef[4];
	int j;
	double tobs;
	double tcal;
	double d;
	double azm;
	double toa;
	double z;
	double sum;
	double torg;
	double lat;
	double lon;
	double dep;
  */

  int iRetCode;

	int i,j;
	double *stp;
	int nvar;
	int ivar;
	
	nvar = 0;
	for(i=0; i<4; i++)
		if(bFree[i]) nvar++;
	if(nvar < 1)
		solve = false;
	if(solve)
		pSlv->Init(nvar);

  iRetCode = ProcessLocation(solve, poCurrent, pPick, nPck);

	nIter++;
	if(poCurrent->nEq < 4)
		return ITER_SUCCESS;
	if(!solve)
		return ITER_SUCCESS;

	// Iterate solution
	stp = pSlv->Solve();
	j = 0;

  memcpy(poNext,poCurrent,sizeof(ORIGIN));
  poNext->nEq=0;
  poNext->dRms=10000.0;

	for(ivar=0; ivar<4; ivar++) {
		if(bFree[ivar]) {
			switch(ivar) {
			case 0:
				poNext->dT	+= stp[j++];
				break;
			case 1:
				poNext->dLat	+= stp[j++];
				break;
			case 2:
				poNext->dLon	+= stp[j++];
				break;
			case 3:
				poNext->dZ	+= stp[j++] * DEG2KM;
				break;
			}
		}
	}
	if(poNext->dZ < 0.0)
		return ITER_DEPTH;
	if(poNext->dZ > 800.0)
		return ITER_DEPTH;

  // DK 061903
  while(poNext->dLon > 180.0)
  {
    CDebug::Log(DEBUG_MAJOR_INFO,"Locate(): adjusting origin(%.2f,%6.2f,%7.2f,%3.0f) original(%.2f,%6.2f,%7.2f,%3.0f)\n",
                poNext->dT,poNext->dLat,poNext->dLon,poNext->dZ, poCurrent->dT,poCurrent->dLat,poCurrent->dLon,poCurrent->dZ);
    poNext->dLon-=360.0;
  }
  while(poNext->dLon < -180.0)
  {
    CDebug::Log(DEBUG_MAJOR_INFO,"Locate(): adjusting origin(%.2f,%6.2f,%7.2f,%3.0f) original(%.2f,%6.2f,%7.2f,%3.0f)\n",
                poNext->dT,poNext->dLat,poNext->dLon,poNext->dZ, poCurrent->dT,poCurrent->dLat,poCurrent->dLon,poCurrent->dZ);
    poNext->dLon+=360.0;
  }
  while(poNext->dLat > 90.0)
  {
    CDebug::Log(DEBUG_MAJOR_INFO,"Locate(): adjusting origin(%.2f,%6.2f,%7.2f,%3.0f) original(%.2f,%6.2f,%7.2f,%3.0f)\n",
                poNext->dT,poNext->dLat,poNext->dLon,poNext->dZ, poCurrent->dT,poCurrent->dLat,poCurrent->dLon,poCurrent->dZ);
    poNext->dLat=180.0 - poNext->dLat;
  }
  while(poNext->dLat < -90.0)
  {
    CDebug::Log(DEBUG_MAJOR_INFO,"Locate(): adjusting origin(%.2f,%6.2f,%7.2f,%3.0f) original(%.2f,%6.2f,%7.2f,%3.0f)\n",
                poNext->dT,poNext->dLat,poNext->dLon,poNext->dZ, poCurrent->dT,poCurrent->dLat,poCurrent->dLon,poCurrent->dZ);
    poNext->dLat=-180.0 - poNext->dLat;
  }

	// Update solution

  CDebug::Log(DEBUG_MINOR_INFO,"Locate(%s,%d): Solve moved origin(%.2f,%6.2f,%7.2f,%3.0f) original(%.2f,%6.2f,%7.2f,%3.0f)\n",
              poCurrent->idOrigin,poCurrent->iOrigin, poNext->dT,poNext->dLat,poNext->dLon,poNext->dZ, poCurrent->dT,poCurrent->dLat,poCurrent->dLon,poCurrent->dZ);
	return ITER_SUCCESS;

}  // end Iterate()

//---------------------------------------------------------------------------------------Affinity
// Calculates affinity factors for each associated phase. For now it is just the ratio
// of a stations distance to N times the modal distance, where N is to be determined.

#define GLOCK_WEIGHT_GRANULARITY  0.25

int CGlock::AddPickCoefficientsForSolve(PICK * pPick, const ORIGIN * pOrg)
{
  int j = 0;
  double coef[4];
  int i;

  if(bFree[0]) 
    coef[j++] = 1.0;
  if(bFree[1]) 
    coef[j++] = -cos(DEG2RAD*pPick->dAzm)*pPick->ttt.dtdx;
  if(bFree[2]) 
    coef[j++] = -sin(DEG2RAD*pPick->dAzm)*cos(DEG2RAD*pOrg->dLat) * pPick->ttt.dtdx;
  if(bFree[3]) 
    coef[j++] = -pPick->ttt.dtdz; 

  i = (pPick->ttt.dLocWeight + (GLOCK_WEIGHT_GRANULARITY/2)) / GLOCK_WEIGHT_GRANULARITY;
  if(pSlv)
  {
    for(j=0; j < i; j++)
      pSlv->Equation(coef, pPick->tRes);
    return(0);
  }
  else
  {
    return(-1);
  }
}  // end AddPickCoefficientsForSolve()

// Function called by Iterate to calculate Origin and Origin/Link 
// params, and to suggest better locations.
int CGlock::ProcessLocation(bool solve, ORIGIN * pOrg, PICK ** pPick, int nPck) 
{

  // Initialize origin vars
	int nEq = 0;
  int i;

	double sum = 0.0;
  double dWeight = 0.0;
  PICK * pck;
  int iRetCode;
  double dGap;

  PhaseType  ptPhase;
  PhaseClass pcPhase;

  // Set default RMS to absurd level.  Shouldn't this be an affinity setting.
  pOrg->dRms = 10000.0;


//  CDebug::Log(DEBUG_MAJOR_INFO,"Locate(I): Org(%6.3f,%8.3f,%5.1f - %.2f)\n",
//              pOrg->dLat, pOrg->dLon, pOrg->dZ, pOrg->dT);

	for(i=0; i<nPck; i++) 
  {
		pck = pPick[i];
    iRetCode = OPCalc_CalculateOriginPickParams(pck,pOrg);
    if(iRetCode == -1)
    {
      CDebug::Log(DEBUG_MINOR_ERROR,"ProcessLocation(): OPCalc_CalculateOriginPickParams  failed with "
                                    "terminal error.  See logfile.  Returning!\n");
      return(-1);
    }
    else if(iRetCode == 1)
    {
      // could not find TravelTimeTable entry for pOrg,pck
      CDebug::Log(DEBUG_MINOR_INFO,"ProcessLocation(): OPCalc_CalculateOriginPickParams() could not "
                                    "find TTT entry for Org,Pick!\n");
      continue;
    }

    // calculate solve coefficients if desired
    if(solve)
      AddPickCoefficientsForSolve(pck, pOrg);
    
    // accumulate statistics
    sum += pck->tRes*pck->tRes*pck->ttt.dLocWeight;

    dWeight += pck->ttt.dLocWeight;

    // copy the delta/azimuth of the pick to proper arrays for
    // further calculations
    pPickDistArray[nEq] = pck->dDelta;

    // DK 021105  Change gap calculation to only use P-Class phases
    // that way if we pull in random S, PPP, and SKiKP phases,
    // they won't affect the gap "quality" of the solution.
    ptPhase = GetPhaseType(pck->ttt.szPhase);
    pcPhase = GetPhaseClass(ptPhase);
    if(( (pcPhase == PHASECLASS_P) && (pck->dDelta < MAX_BELIEVABLE_DISTANCE_FOR_P_PHASE )) || 
       (ptPhase == PHASE_PKPdf)
      )
      pPickAzmArray[nEq] = pck->dAzm;
    else
      pPickAzmArray[nEq] = -1.0;


    // increment the number of picks used to locate the origin
    nEq++;
    
    // keep the arrays from being overrun
    if(nEq > OPCALC_MAX_PICKS)
      break;
  }  // end for each pick in the Pick Array

  // if we didn't use atleast one pick.  Give up and go home.
  if(nEq < 1)
	  return(1);


  // Record the number of equations used to calc the location
  pOrg->nEq = nEq;

  // Record the residual standard deviation
  pOrg->dRms = sqrt(sum/dWeight);

  /* Calculate Pick Azimuth statistics for the Origin (Gap)
   ****************************************************/
  // Sort the pick-azimuth array
  qsortd(pPickAzmArray, nEq);

  // If the Pick wasn't a P-class Pick, then we gave it a negative azm,
  // so igonre all of the negative azm picks (which are all at the beginning
  // since the array has been sorted
 	for(i=0; i<nEq-2; i++) 
  {
    if(pPickAzmArray[i] >= 0.0)
      break;
  }

  CDebug::Log(DEBUG_MINOR_INFO,
              "ProcessLocation(): Calculating Gap for Org %d(%5.1f, %6.1f, %3.0f) based on %d valid picks: ",
              pOrg->iOrigin, pOrg->dLat, pOrg->dLon, pOrg->dZ, nEq - i);
  // Calculate the gap for the last-to-first wrap
 	dGap = pPickAzmArray[i] + 360.0 - pPickAzmArray[nEq-1];
 	for(; i<nEq-1; i++) 
  {
    // for each gap between two picks, record only if largest so far
    if(pPickAzmArray[i+1] - pPickAzmArray[i] > dGap)
      dGap = pPickAzmArray[i+1] - pPickAzmArray[i];
 	}
 	pOrg->dGap = dGap;

  /* Calculate Pick Distance statistics for the Origin
   ****************************************************/
  // Sort the pick-distance array
  qsortd(pPickDistArray, nEq);

  // Calculate the minimum station distance for the origin
	pOrg->dDelMin = pPickDistArray[0];

  // Calculate the maximum station distance for the origin
	pOrg->dDelMax = pPickDistArray[nEq-1];

  // Calculate the median station distance for the origin
	pOrg->dDelMod = 0.5*(pPickDistArray[nEq/2] + pPickDistArray[(nEq-1)/2]);


  /* Calculate the Affinity for the Origin (and Picks) 
   ****************************************************/
  iRetCode = OPCalc_CalcOriginAffinity(pOrg, pPick, nPck);
  if(iRetCode)
  {
    CDebug::Log(DEBUG_MINOR_ERROR,
                "Error: ProcessLocation(): OPCalc_CalcOriginAffinity() return (%d)\n",
                iRetCode);
    return(iRetCode);
  }
     
  CDebug::Log(DEBUG_MINOR_INFO,"END Locate(): dRms:%.2f nEq:%d\n",
              pOrg->dRms, nEq);
  return(0);
}  // End Process Location()


// Function to initialize the comparison parameters for
// relative solution quality.
void  CGlock::Locate_InitOrigin(ORIGIN * pOrigin, const char * idOrigin)
{
  // set the comparison criteria to something really high/bad
  memset(pOrigin,0,sizeof(ORIGIN));
  pOrigin->dRms = 10000.0;
  strncpy(pOrigin->idOrigin, idOrigin, sizeof(pOrigin->idOrigin));
  pOrigin->idOrigin[sizeof(pOrigin->idOrigin)-1] = 0x00;
}


// Standard compare function.  returns -1 if Origin1 is "better"
// than Origin2, 1 if vice-versa, and 0 if they are of equal
// quality.
int CGlock::CompareOrigins(ORIGIN * po1, ORIGIN * po2)
{
  if(po1->dAff > po2->dAff)
    return(-1);
  if(po1->dAff < po2->dAff)
    return(1);
  //else
  return(0);
}  // end CompareOrigins()


