/*-----------------------------------------------------------------------
                          M A X I M I Z E

  This optimises a peak against a motor or virtual motor. The algorithm
  used is the same as in the program difrac by P. White and E. Gabe. The
  algorithm used is more efficient then the one based on scans as in
  fitcenter and optimise as it does not need as many counting operations.

  The algorithm works as follows:

  Data is kept in an array of length MAXPTS. We start at the middle of
  the array.  

  - From the current position we first step to the left side until we reach
    a point where the counts are less then half maximum or to many points.  
    Now there are four possible situations:

    - The peak is still to our left. Reposition and do again.
    - No peak found or peak is normal, i.e max close to 50. In either case
      proceed with right side.
    - peak all on left side. Calculate some indexes and calculate meridian.

  - From the current position go to the right side until we reach the
    point where counts are less then half maximum or the maximum number 
    of pints has been reached. Again four cases:
 
     - The peak is still to our right. Reposition and do again.
     - No peak found: error return.
     - Normal peak: calculate meridian.
     - peak all on left side. Calculate some indexes and calculate meridian.

  - Calculate the median of the peak.


  copyright: see copyright.h


  Initial coding: Mark Koennecke, November 1999
  -------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "countdriv.h"
#include "counter.h"
#include "drive.h"
#include "maximize.h"

#define MAXPTS 100
#define DEBUG 1

    typedef struct __MAXIMIZE {
                                pObjectDescriptor pDes;
                                pCounter pCount;
                                int i360;
                              }Maxxii;

/*-----------------------------------------------------------------------
  put into 360 degree range if 360 degree flag is active.
*/
   static float in360(pMax self, float fVal)
   {
       if(self->i360)
       {
          if(fVal < 0.)
             fVal = 360. + fVal;
          if(fVal > 360.)
             fVal = fVal - 360.;
       }
       return fVal;
   }
/*---------------------------------------------------------------------*/
   static int maxDrive(void *pObject, char *pVarName,
                       float fPos, SConnection *pCon)
   {
      pDummy pDum;
      int status;
      char pBueffel[132];
      long lTask;

      /* start */
      pDum = (pDummy)pObject;
      status = StartDevice(pServ->pExecutor,
                           pVarName,
                           pDum->pDescriptor,
                           pObject,
                           pCon,
                           fPos);
      if(!status)
      {
         sprintf(pBueffel,"ERROR: failed to start %s",pVarName);
         SCWrite(pCon,pBueffel,eError);
         return 0;
      }

      /* wait */
      lTask = GetDevexecID(pServ->pExecutor);
      if(lTask > 0)
      {
        TaskWait(pServ->pTasker,lTask);
      }
      else
      {
         return 0;
      }

      /* check interrupts */
      if(SCGetInterrupt(pCon) >= eAbortScan)
      {
         SCWrite(pCon,"ERROR: Maximizing interrupted",eError);
         return 0;
      }
      return 1;
   }
/*----------------------------------------------------------------------*/
   static int maxCount(void *pCount, CounterMode eMode, 
                       float fPreset, long *lCts, SConnection *pCon)
   {
      int iRet;

      SetCounterMode((pCounter)pCount,eMode);
      iRet = DoCount((pCounter)pCount, fPreset, pCon,1);
      if(!iRet)
      {
         return 0;
      }
      *lCts = GetCounts((pCounter)pCount,pCon);
      if(SCGetInterrupt(pCon) >= eAbortScan)
      {
          SCWrite(pCon,"ERROR: maximizing aborted",eError);
          return 0;
      }
      return 1;
   }
                      
/*-----------------------------------------------------------------------*/
   int MaximizePeak(pMax self, void *pVar, char *pVarName,
                    float fStep, CounterMode eMode,
                    float fPreset, SConnection *pCon)
   {
      float x[MAXPTS], y[MAXPTS];
      float fMax, fCent, fS, fArea, fStart, fPos, fDA, fDisc;
      int iBot, iTop, iMax, i, iRet, iSkip, iTMin, iTMax;
      pIDrivable pDriv = NULL;
      long lCts, lMax, lMin;
      char pBueffel[132];

      /* get drivable interface */
      pDriv = GetDrivableInterface(pVar);
      if(!pDriv)
      {
          SCWrite(pCon,"ERROR: variable is NOT drivable, cannot maximize",
                  eError);
          return 0;
      }

      start:
         lMax = 0;
         lMin = 999999999999;
         fStart = pDriv->GetValue(pVar,pCon);
         if(fStart < -999999.)
	 {
            return 0;
         }

         /* search to the left until out of space or lCts < lMax/2. */
         SCWrite(pCon,"Searching for low angle boundary..",eWarning); 
         for(i = MAXPTS/2; i >= 0; i--)
         {
	   /* drive motor */
            fPos = fStart - (MAXPTS/2 - i)*fStep;
            fPos = in360(self,fPos);
            if(maxDrive(pVar,pVarName,fPos,pCon) != 1)
	    {
                return 0;
            }            
            x[i] = pDriv->GetValue(pVar,pCon);
            /* count */                         
            if(maxCount(self->pCount,eMode,fPreset, &lCts,pCon) != 1)
	    {
               return 0;
            }
            /* print a message */
            sprintf(pBueffel,"%5d %8.2f %ld",i,x[i],lCts);
            SCWrite(pCon,pBueffel,eWarning);                   

            /* store counts and some logic */
            if(lCts < 1)
	    {
              y[i] = 0.;
            }
            else
	    {
              y[i] = (float)lCts;
            }
            if(lCts > lMax)
	    {
               lMax = lCts;
               iMax = i;
            }
            if(lCts < lMin)
	    {
               lMin = lCts;
            }
            if(lCts < lMax/2)
	    {
                break;
            }
         }
         /* some logic: the four cases */
         iSkip = 0;
         iBot = i;
         /* first case: peak out of range: do again */
         if( (iMax == 0) && (lMax/2 > lMin) )
	 {
           goto start;
         } 
         /* no peak found or normal peak: continue at other side */
         if( (i < 1) || (y[MAXPTS/2] > lMax/2) )
         {
           iSkip = 0;
         } 
         else
         {
             /* next case: all of the peak in measured half:
                find max value and skip the right half  
             */
             for(i = MAXPTS/2; i > 0; i--)
	     {
               if(y[i] > lMax/2)
	       {
                 iTop = i + 1;
                 break;
               }
             }
             iSkip = 1;
         }

         /* 
           do the same to the right hand side, but only if required 
	 */
         if(iSkip == 0)
         {
             lMin = 100000;
             lMax = -100000;
             SCWrite(pCon,"Searching for high angle boundary..",eWarning); 
             for(i = MAXPTS/2; i < MAXPTS; i++)
	     {
	       /* drive motor */
	       fPos = fStart + (i - MAXPTS/2) * fStep;
	       fPos = in360(self,fPos);
	       if(maxDrive(pVar,pVarName,fPos,pCon) != 1)
		 {
		   return 0;
		 }            
	       x[i] = pDriv->GetValue(pVar,pCon);
	       /* count */                         
	       if(maxCount(self->pCount,eMode,fPreset, &lCts,pCon) != 1)
		 {
		   return 0;
		 }
	       /* print a message */
	       sprintf(pBueffel,"%5d %8.2f %ld",i,x[i],lCts);
	       SCWrite(pCon,pBueffel,eWarning);                   

            /* store counts and some logic */
	       if(lCts < 1)
		 {
		   y[i] = 0.;
		 }
	       else
		 {
		   y[i] = (float)lCts;
		 }
	       if(lCts > lMax)
		 {
		   lMax = lCts;
		   iMax = i;
		 }
	       if(lCts < lMin)
		 {
		   lMin = lCts;
		 }
	       if(lCts < lMax/2)
		 {
		   break;
		 }
	     }
             /* some logic again: four cases */
             iTop = i;
             iTop++;
             /* first case: peak is at high angle side */
             if( (i > MAXPTS-2) && (lMax*0.5 > lMin) )
	     {
                goto start;
             }
             /* second case: no peak */
             if( (iTop > MAXPTS-2) )
	     {
                SCWrite(pCon,"ERROR: no peak found!",eError);
                return 0;
             }
             /* third case: normal peak */
             if(y[MAXPTS/2] >= 0.5*lMax)
	     {
               iTop--;
             } 
             /* 
                fourth case: the peak is completely at the high angle
                side 
	     */
             else
	     {
                for(i = MAXPTS/2; i < MAXPTS; i++)
	        {
                  if(y[i] > lMax/2)
	          {
                    iBot = i - 1;
                     break;
                   }
                 }
                iTop--;
             }
         } /* end of iSkip */           

         if( (iBot < 2) || (iTop > MAXPTS-2) || (lMax < lMin*2) )
         {
            SCWrite(pCon,"ERROR: no peak found!",eError);
            return 0;
         }
#ifdef DEBUG
         sprintf(pBueffel,"iBot = %d, iTop = %d, lMax = %d",
                 iBot, iTop, lMax);
         SCWrite(pCon,pBueffel,eWarning);
#endif     
    
         /* calculate median */
         fArea = 0.;
         for(i = iBot; i < iTop; i++)
	 {
            fArea += (x[i+1] - x[i])*(y[i+1] + y[i])*.25;
         }                  
         fS = 0.;
         for(i = iBot; i < iTop; i++)
	 {
           fS += (x[i+1] -x[i])*(y[i+1] +y[i]) *.5;
           if(fS > fArea)
           {
             break;
           }
         }
         fS -= .5*(x[i+1] - x[i])*(y[i+1] + y[i]);
         fDA = fArea - fS;
         if(y[i+1] == y[i])
	 {
             fCent = x[i] + fDA/y[i];
         }
         else
	 {
               fS = (y[i+1] - y[i])/(x[i+1] -x[i]);
               if( (y[i]*y[i] + 2. * fS * fDA) < 0.)
	       {
                 SCWrite(pCon,"ERROR calculating median",eError);
                 return 0;
               }
               fDisc = sqrt(y[i]*y[i] + 2.* fS * fDA);
               fCent = x[i] +(fDisc -y[i])/fS;              
          }
	 /* finished ! */
	 maxDrive(pVar,pVarName,fCent,pCon);
         sprintf(pBueffel,"Found peak center at %8.2f", fCent);
         SCWrite(pCon,pBueffel,eValue);
         return 1;
   }
/*------------------------------------------------------------------*/
   static void MaxKill(void *pData)
   {
      pMax self = (pMax)pData;
      if(self == NULL)
         return;

      if(self->pDes)
         DeleteDescriptor(self->pDes);
      free(self);
   }
/*-------------------- wrap-i-wrap ----------------------------------*/
   int MaximizeFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
                       int argc, char *argv[])
   {
      pMax pNew = NULL;
      CommandList *pCom = NULL;
      pICountable pCts = NULL;

      if(argc < 2)
      {
        SCWrite(pCon,
         "ERROR: insufficient number of arguments to MaximizeFactory",
          eError);
        return 0;
      }

      /* find a countable */
      pCom = FindCommand(pSics,argv[1]);
      if(pCom == NULL)
      {
         SCWrite(pCon,"ERROR: counter not found in MaximizeFactory",
                 eError);
         return 0;        
      }
      pCts = GetCountableInterface(pCom->pData);
      if(!pCts)
      {
        SCWrite(pCon,"ERROR: argument to MaximizeFactory is no counter",
                eError);
        return 0;
      }
      pNew = (pMax)malloc(sizeof(Maxxii));
      pNew->pDes = CreateDescriptor("Maximizer");
      pNew->pCount = pCom->pData;
      pNew->i360 = 0;

      AddCommand(pSics,"max",MaximizeAction,MaxKill,pNew);
      return 1;
   }
/*------------------------------------------------------------------*/
   int MaximizeAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                       int argc, char *argv[])
   {
      pMax self = NULL;
      CommandList *pCom = NULL;
      char pBueffel[256];
      double dVal;
      float fStep, fPreset;
      CounterMode eCount;
      int iRet;

      self = (pMax)pData;
      assert(self);
      assert(pCon);

      /* check privilege */
      if(!SCMatchRights(pCon,usUser))  
      {
        return 1;
      }

      /* enough arguments ?*/
      if(argc < 5)
      {
        SCWrite(pCon,"ERROR: Insufficient number of arguments to max",
                eError);
        return 0;
      }      

      /* decode arguments */
      pCom = FindCommand(pSics,argv[1]);
      if(!pCom)
      {
        sprintf(pBueffel,"ERROR: object %s NOT found!",
                argv[1]);
        SCWrite(pCon,pBueffel,eError);
        return 0;         
      }    
      if(!pCom->pData)
      {
        sprintf(pBueffel,"ERROR: object %s Invalid!!",
                argv[1]);
        SCWrite(pCon,pBueffel,eError);
        return 0;         
      }    

      iRet = Tcl_GetDouble(pSics->pTcl,argv[2], &dVal);
      if(iRet != TCL_OK)
      {
         sprintf(pBueffel,"ERROR: failed to convert %s to number",
                 argv[2]);
         SCWrite(pCon,pBueffel,eError);
         return 0;    
      }
      fStep = (float)dVal;

      iRet = Tcl_GetDouble(pSics->pTcl,argv[4], &dVal);
      if(iRet != TCL_OK)
      {
         sprintf(pBueffel,"ERROR: failed to convert %s to number",
                 argv[4]);
         SCWrite(pCon,pBueffel,eError);
         return 0;    
      }
      fPreset = (float)dVal;

      /* convert countmode */
      strtolower(argv[3]);
      if(strcmp(argv[3],"timer") == 0)
      {
        eCount = eTimer;
      }
      else
      {
        eCount = ePreset;
      }
      return MaximizePeak(self,pCom->pData, argv[1],fStep,eCount,
                           fPreset, pCon);
   }
