/*------------------------------------------------------------------------
                      F I T C E N T E R
                      
   A simple peak finding and center of gravity determination module for SICS.
   
   copyright: see copyright.h
   
   Mark Koennecke, October 1997
-----------------------------------------------------------------------------*/
#include <limits.h>
#include <math.h>
#include "sics.h"
#include "counter.h"
#include "motor.h"
#include "scan.h"
#include "fitcenter.h"
#define THRESHOLD .1
/*--------------------------------------------------------------------------*/
  typedef struct __FitCenter {
                               pObjectDescriptor pDes;
                               pScanData pScan;
                               long *lCounts;
                               float *fAxis;
                               char pName[132];
                               float fCenter;
                               float fStddev;
                               long lPeak;
                               float FWHM;
                               int iNP;
                              } FitCenter;
/*-------------------------------------------------------------------------*/
 pFit CreateFitCenter(pScanData pScan)
 {
   pFit pNew = NULL;
   
   pNew = (pFit)malloc(sizeof(FitCenter));
   if(!pNew)
   {
     return NULL;
   }
   memset(pNew,0,sizeof(FitCenter));
   
   pNew->pDes = CreateDescriptor("FitCenter");
   if(!pNew->pDes)
   {
     free(pNew);
     return NULL;
   }
   pNew->pScan = pScan;
   return pNew;
 }
/*-------------------------------------------------------------------------*/
 void DeleteFitCenter(void *pData)
 {
    pFit self = NULL;
    
    self = (pFit)pData;
    assert(self);
    
    if(self->pDes)
    {
      DeleteDescriptor(self->pDes);
    }
    if(self->lCounts)
    {
      free(self->lCounts);
    }
    if(self->fAxis)
    {
      free(self->fAxis);
    }
    free(self);
 }     
/*--------------------------------------------------------------------------*/
 static int Init(pFit self)
 {
    pMotor pMot = NULL;
    float fZero, fSign;
    int i;
 
    /* collect data first */
    self->iNP = GetScanNP(self->pScan);
    if(self->lCounts)
    {
      free(self->lCounts);
      self->lCounts = NULL;
    }
    if(self->fAxis)
    {
      free(self->fAxis);
      self->fAxis = NULL;
    }
    self->lCounts = (long *)malloc(self->iNP * sizeof(long));
    self->fAxis = (float *)malloc(self->iNP * sizeof(float));
    if( (!self->lCounts) || (!self->fAxis) )
    {
      return 0;
    }
    
    GetScanCounts(self->pScan,self->lCounts, self->iNP);
    GetScanVar(self->pScan,0,self->fAxis,self->iNP);
    GetScanVarName(self->pScan,0,self->pName,131);
    
    /* correct fAxis for softzero points and sign
       when the scan variable is a motor */
    pMot = FindMotor(pServ->pSics,self->pName);
    if(pMot)
    {
      i = MotorGetPar(pMot,"softzero",&fZero);
      i = MotorGetPar(pMot,"sign",&fSign);
      assert(i);
      for(i = 0; i < self->iNP; i++)
      {
         self->fAxis[i] -= fZero;
         self->fAxis[i] *= fSign;
      }
    }
    
    return 1;
 }   
/*-------------------------------------------------------------------------*/
  static int FindMax(pFit self)
  {
     long lMax = LONG_MIN;
     int i, iMax;
     
     for(i = 0, iMax = 0; i < self->iNP; i++)
     {
       if(self->lCounts[i] > lMax)
       {
         lMax = self->lCounts[i];
         iMax = i;
       }
     }
     
     return iMax;
  }    
/*-------------------------------------------------------------------------*/  
  static int CalculateFitIntern(pFit self)
  {
     int i,iLeft, iRight, iRet,iPeak, iLast;
     long lHalf;
     float fFrac, fDelta, fStep, fLeft, fRight, fNenner, fTest;
     float fLFrac, fRFrac, fSum, fCI;     

     iLeft = 0;

     /* find the maximum counts */
     iRet = FindMax(self);
     self->lPeak = self->lCounts[iRet];
     /* a default fit is the peak maximum. This
        helps optimise to do the right thing if no proper calculation
        could be performed
     */    
     self->fCenter = self->fAxis[iRet];
     iPeak = iRet;

     if(self->lPeak < 3)
     {
       return -3;
     }

     /* find FWHM, first from Right then from left */
     iRet =1;
     lHalf = self->lPeak/2;
     iLeft = iPeak;
     while( (self->lCounts[iLeft]  > lHalf) && (iLeft > 0) )
     {
       iLeft--;
     }
     if(iLeft == 0)
     {
       iRet = -1;
       iLeft++;
     }
     fNenner = (float)self->lCounts[iLeft+1] - (float)self->lCounts[iLeft];
     if(fNenner < 0.)
     {
       fTest = -fNenner;
     }  
     else
     {
       fTest = fNenner;
     }
     if(fTest < .00001)
     {
       return -4;
     }
     fFrac = ((float)self->lCounts[iLeft+1] - (float)lHalf) / fNenner;
     fLeft = self->fAxis[iLeft] + 
             fFrac * (self->fAxis[iLeft+1] - self->fAxis[iLeft]);
     fLFrac = fFrac;      
    
     iRight = iPeak;
     while( (self->lCounts[iRight] > lHalf) && (iRight < self->iNP) )
     {
       iRight++;
     }
     if(iRight >= self->iNP - 1)
     {
       iRet = -2;
       iRight = self->iNP - 1;
     } 
     fNenner = (float)self->lCounts[iRight-1] - (float)self->lCounts[iRight];
     if(fNenner < 0.)
     {
       fTest = -fNenner;
     }  
     else
     {
       fTest = fNenner;
     }
     if(fTest < .00001)
     {
       return -4;
     }
     fFrac = ((float)self->lCounts[iRight-1] - (float)lHalf) / fNenner;
     fRight = self->fAxis[iRight] - 
            fFrac * (self->fAxis[1] - self->fAxis[0]);                        
     fRFrac = fFrac;
     
     if(iRight != iLeft)
     {
       self->FWHM = fRight - fLeft;
     }
     else
     {
       self->FWHM = 0.07;
     }
/*
     self->fCenter = fLeft + (fRight - fLeft)/2.;
*/
     fSum = lHalf*(fLFrac);
     fNenner = lHalf;
     /* center of gravity calculation between iLeft and iRight */
     for(i = iLeft+1; i < iRight; i++)
     {
       fSum   += self->lCounts[i]*(fLFrac + i - iLeft);
       fNenner += self->lCounts[i];
     }
     fSum += (iRight - 1 + fRFrac + fLFrac - iLeft )*lHalf;
     fNenner += lHalf;
     fCI = fSum/fNenner;
     /* iLeft is zero, zero point shift by lFrac */
     fCI -= fLFrac;
     fTest = fNenner;
     fNenner = modff(fCI, &fSum);
     self->fCenter = self->fAxis[(int)(fSum)+iLeft] + 
                  fNenner*(self->fAxis[iLeft+1] - self->fAxis[iLeft]);

     /* error calculation */
     fTest = 2*(sqrtf(fTest))/fTest;
     /* because we are really summing from iLeft */ 
     fTest = fCI*fTest;             
     if(fTest < 0)
        fTest = - fTest;
     self->fStddev = fTest*(self->fAxis[iLeft+1] - self->fAxis[iLeft]);

     return iRet;
  }  
/*--------------------------------------------------------------------------*/
  int CalculateFit(pFit self)
  {  
      int iRet;
      
      
     /* initialise */
     iRet = Init(self);
     if(!iRet)
     {
       return iRet;
     }
     return CalculateFitIntern(self);
  }
/*------------------------------------------------------------------------*/
  int CalculateFitFromData(pFit self, float *fAxis, long *lCounts, int iLength)
  {
     int iRet;
     float *fOld;
     long *lOld;
     int iOld;
     
     /* save old data. The normal handling is to have a local copy of
        data with fit. This is handled differently in this function, where
        it is assumed, that the data is owned by the caller. In order not
        to upset fit's scheme for data handling data pointers are swapped.
        This is more efficient then copying the data around without real 
        need.
      */
     iOld = self->iNP;
     fOld = self->fAxis;
     lOld = self->lCounts;
     
     /* put in new */
     self->iNP = iLength;
     self->fAxis = fAxis;
     self->lCounts = lCounts;
 
     iRet = CalculateFitIntern(self);
     
     /* reset data pointers */
     self->iNP = iOld;
     self->fAxis = fOld;
     self->lCounts = lOld;
     
     return iRet;    
  }  
/*------------------------------------------------------------------------*/
  void GetFitResults(pFit self, float *fCenter, float *fStdDev,
                     float *FWHM, float *fMax)
  {
        assert(self);
       *fCenter = self->fCenter;
       *FWHM    = self->FWHM;
       *fMax    = (float)self->lPeak;       
       *fStdDev = self->fStddev;
  }  
/*--------------------------------------------------------------------------*/
  int DriveCenter(pFit self, SConnection *pCon, SicsInterp *pSics)
  {
     int iRet;
     
     assert(self);
     assert(pCon);
     assert(pSics);
     
     iRet = StartMotor(pServ->pExecutor, pSics,pCon,self->pName,self->fCenter);
     if(!iRet)
     {
       return 0;
     }
     
     iRet = Wait4Success(pServ->pExecutor);
     switch(iRet)
     {
       case DEVINT:
                 if(SCGetInterrupt(pCon) == eAbortOperation)
                 {
                       SCSetInterrupt(pCon,eContinue);
                       SCSetError(pCon,OKOK);
                       SCWrite(pCon,"Driving Aborted",eStatus);
                 }
                 return 0;
                 break;
        case DEVDONE:
                 SCWrite(pCon,"Driving to center done",eStatus);
                 break;
        default:
                 SCWrite(pCon,"WARNING: driving to center finished with problems",
                         eWarning);
                 break;                          
                                                                           
     }
     return 1;
  }
     
/*--------------------------------------------------------------------------*/
  int FitFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
                 int argc, char *argv[])
  {
     pFit self = NULL;
     pScanData pScan = NULL;
     pDummy pDum = NULL;
     CommandList *pCom = NULL;
     char pBueffel[132];
     int iRet, iRet1;
     
     if(argc < 2)
     {
       SCWrite(pCon,"ERROR: MakeFit expected the name of a scan command as a parameter",
               eError);
       return 0;        
     }
     
     pCom = FindCommand(pSics,argv[1]);
     if(!pCom)
     {
       sprintf(pBueffel,"ERROR: scan command %s not found",argv[1]);
       SCWrite(pCon,pBueffel,eError);
       return 0;
     }
     pDum = (pDummy)pCom->pData;
     if(!pDum)
     {
       sprintf(pBueffel,"ERROR: scan command %s not found",argv[1]);
       SCWrite(pCon,pBueffel,eError);
       return 0;
     }
     if(strcmp(pDum->pDescriptor->name,"ScanObject") != 0)
     {
       sprintf(pBueffel,"ERROR: %s is no scan command",argv[1]);
       SCWrite(pCon,pBueffel,eError);
       return 0;
     }
     
     pScan = (pScanData)pDum;
     self = CreateFitCenter(pScan);
     if(!self)
     {
       SCWrite(pCon,"ERROR: Failure to allocate Peak & Center command",eError);
       return 0;
     }  
     
     iRet = AddCommand(pSics,"peak",FitWrapper,DeleteFitCenter,self);
     iRet1 = AddCommand(pSics,"center",CenterWrapper,NULL,self);
     if(!iRet || !iRet1)
     {
        sprintf(pBueffel,"ERROR: duplicate commands peak and center not created",argv[2]);
        SCWrite(pCon,pBueffel,eError);
        DeleteFitCenter((void *)self);
        return 0;
     }
     return 1;
  }  
/*--------------------------------------------------------------------------*/
  int FitWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
                 int argc, char *argv[])
  {
     pFit self = NULL;
     int iRet;
     char pBueffel[256];
     
     self = (pFit)pData;
     assert(self);
     
     iRet = CalculateFit(self);
     switch(iRet)
     {
       case 0:
           SCWrite(pCon,"ERROR: failure to fit your data!",eError);
           return 0;
           break;
       case -1:
           SCWrite(pCon,"WARNING: could not find left hand half width",eWarning);
           SCWrite(pCon,"Fit Results most certainly dodgy, SICS suggests measuring a full peak",eWarning);
           break;
       case -2:
           SCWrite(pCon,"WARNING: could not find right hand half width",eError);
           SCWrite(pCon,"Fit Results most certainly dodgy, SICS suggests measuring a full peak",eWarning);
           break;
       case -3:
           SCWrite(pCon,"ERROR: No counts found in Fit!",eError);
           return 0;
           break;
       case -4:
           SCWrite(pCon,"ERROR: Insufficient counts in peak",eError);
           return 0;
           break;                
     }         
    
     /*
       This is a little feature to get the peak without rubbish for
       the TAS routines
     */
     if(argc > 1)
     {
       strtolower(argv[1]);
       if(strcmp(argv[1],"value") == 0)
       {
         sprintf(pBueffel,"%f", self->fCenter);
         SCWrite(pCon,pBueffel,eValue);
         return 1;
       } 
     }
     
     /* print results */
     sprintf(pBueffel,"Estimated Peak Center: %f, StdDev: %f \n", 
             self->fCenter,self->fStddev);
     SCWrite(pCon,pBueffel,eValue);
     sprintf(pBueffel,"Maximum peak height: %ld\n", self->lPeak);
     SCWrite(pCon,pBueffel,eValue);
     sprintf(pBueffel,"Approximate FWHM: %f\n",self->FWHM);                                                    
     SCWrite(pCon,pBueffel,eValue);
     
     return 1;
 }
/*---------------------------------------------------------------------------*/
  int CenterWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
                 int argc, char *argv[])
  {
     pFit self = NULL;
     int iRet;
     
     self = (pFit)pData;
     assert(self);
          
     return DriveCenter(self,pCon,pSics);
   }
