/*-------------------------------------------------------------------------
                                 O P T I M I S E
                                 
    This module optimises a peak with respect to various parameters.                           
    The working of this is described in more detail in optimise.w
    
    copyright: see copyright.h
    
    Mark Koennecke, March 1998
--------------------------------------------------------------------------*/
#include <stdlib.h>
#include <math.h>
#include <tcl.h>
#include <assert.h>
#include "fortify.h"
#include "sdynar.h"
#include "sics.h"
#include "counter.h"
#include "motor.h"
#include "drive.h"
#include "scan.h"
#include "scan.i"
#include "fitcenter.h"
#include "optimise.h"

#ifdef CYGNUS
#define MAXFLOAT 9999999.99
#endif

#define ABS(x) (x < 0 ? -(x) : (x))    

/*----------------- local variable structure -----------------------------*/

   typedef struct {
                    char *pName;
                    float fStep;
                    int iStep;
                    float fPrecision;
                    float fCenter;
                    float fShift;
                    pIDrivable pDriv; 
                    void       *pData;
                    int iLost;
                  } OVarEntry, *pOVarEntry;
/*-------------------------------------------------------------------------*/
   static void FreeOVar(void *pData)
   {
      pOVarEntry self = NULL;
      
      self = (pOVarEntry)pData;
      if(!self)
           return;
           
      if(self->pName)
      {
        free(self->pName);
      }     
      free(self);
   }  
                 
/*-------------------- the optimise structure -----------------------------*/

   typedef struct __OptimiseStruct {
                                     pObjectDescriptor pDes;
                                     int               iMaxCycles;
                                     CounterMode       eCount;
                                     float             fPreset;
                                     int               iChannel;
                                     float             fThreshold;
                                     int               iVar;
                                     pDynar            pVariables;
                                     pScanData         pScanner;
                                     pFit              pPeakFitter;
                                    } Optimiser;
                                    
/*--- dummy functions to stop scan from writing scan files --------------*/       

  static int DummyHeader(pScanData self)
  {
    return 1;
  }  
  static int DummyHeader2(pScanData self,int iPoint)
  {
    return 1;
  }

/*------------------------------------------------------------------------*/

  pOptimise CreateOptimiser(pCounter pCount)
  {
    pOptimise pNew = NULL;
    
    assert(pCount);
    
    /* get some memory */
    pNew = (pOptimise)malloc(sizeof(Optimiser));
    if(!pNew)
    {
      return NULL;
    }
    memset(pNew,0,sizeof(Optimiser));
    
    /* initialise a few defaults */
    pNew->fThreshold = 100;
    pNew->iMaxCycles = 7;
    pNew->iChannel = 0;
    pNew->eCount = eTimer;
    pNew->iVar = 0;
    pNew->fPreset = 10000.;
    
    pNew->pDes = CreateDescriptor("PeakOptimiser");
    if(!pNew->pDes)
    {
      free(pNew);
      return NULL;
    }
    
    pNew->pScanner = CreateScanObject(NULL,NULL,pCount);
    if(!pNew->pScanner)
    {
      DeleteDescriptor(pNew->pDes);
      free(pNew);
      return NULL;
      
    }
    pNew->pScanner->WriteHeader     = DummyHeader;
    pNew->pScanner->WriteScanPoints = DummyHeader2;
    
    pNew->pPeakFitter = CreateFitCenter(pNew->pScanner);
    if(!pNew->pPeakFitter)
    {
       DeleteDescriptor(pNew->pDes);
       DeleteScanObject(pNew->pScanner);
       free(pNew);
       return NULL;
    }
    
    pNew->iVar = 0;
    pNew->pVariables = CreateDynar(0,10,10,FreeOVar);
    if(!pNew->pVariables)
    {
       DeleteFitCenter(pNew->pPeakFitter);
       DeleteDescriptor(pNew->pDes);
       DeleteScanObject(pNew->pScanner);
       free(pNew);
       return NULL;
    }
    
    return pNew;
  }  
/*--------------------------------------------------------------------------*/
  void DeleteOptimiser(void *pData)
  {
     pOptimise self = NULL;
     
     self = (pOptimise)pData;
     if(!self)
        return;
      
     if(self->pVariables)
     {
        DeleteDynar(self->pVariables);
     }   
        
     if(self->pPeakFitter)
     {       
       DeleteFitCenter(self->pPeakFitter);
     }
     if(self->pDes)
     {  
       DeleteDescriptor(self->pDes);
     }
     if(self->pScanner)
     {  
       DeleteScanObject(self->pScanner);
     }  
     
     free(self);
  }
/*--------------------------------------------------------------------------*/
  void OptimiserClear(pOptimise self)
  {
     assert(self);
     self->iVar = 0;
  }    
/*--------------------------------------------------------------------------*/
  int OptimiserAdd(pOptimise self,
                   char *pName, float fStep, int nStep, float fPrecision)
  {
      OVarEntry sNeu;
      CommandList *pCom = NULL;
   
      /* find and check our object */
      pCom = FindCommand(pServ->pSics,pName);
      if(!pCom)
      {
        return 0;
      }
      sNeu.pData = pCom->pData;
      sNeu.pDriv = GetDrivableInterface(sNeu.pData);
      if(!sNeu.pDriv)
      {
        return 0;
      }
      sNeu.fStep = fStep;
      sNeu.iStep = nStep;
      sNeu.fPrecision = fPrecision;
      sNeu.pName = strdup(pName);
      sNeu.iLost = 0;
      
      DynarPutCopy(self->pVariables,self->iVar,&sNeu,sizeof(OVarEntry));
      self->iVar++;
      
      return 1;
  }    
/*--------------------------------------------------------------------------*/
  int OptimiserSetPar(pOptimise self, char *name, float fVal)
  {
     if(strcmp(name,"maxcycles") == 0)
     {
         self->iMaxCycles = (int)fVal;
         return 1;
     }
     else if(strcmp(name,"threshold") == 0)
     {
       self->fThreshold = fVal;
       return 1;
     }
     else if(strcmp(name,"channel") == 0)
     {
       self->iChannel = (int)fVal;
       return 1;
     }
     else if(strcmp(name,"preset") == 0)
     {
       self->fPreset = fVal;
       return 1;
     }
     else if(strcmp(name,"countmode") == 0)
     {
       if(fVal < 0.05)
       {
         self->eCount = eTimer;
       }
       else
       {
         self->eCount = ePreset;
       }
       return 1;
     }
     else
     {
       return 0;
     }
  }      
/*--------------------------------------------------------------------------*/
  int OptimiserGetPar(pOptimise self, char *name, float *fVal)
  {
     if(strcmp(name,"maxcycles") == 0)
     {
         *fVal = self->iMaxCycles;
         return 1;
     }
     else if(strcmp(name,"threshold") == 0)
     {
       *fVal = self->fThreshold;
       return 1;
     }
     else if(strcmp(name,"channel") == 0)
     {
       *fVal = self->iChannel;
       return 1;
     }
     else if(strcmp(name,"preset") == 0)
     {
       *fVal = self->fPreset;
       return 1;
     }
     else if(strcmp(name,"countmode") == 0)
     {
       if(self->eCount == eTimer)
       {
         *fVal = 0.;
       }
       else
       {
         *fVal = 1.0;
       }
       return 1;
     }
     else
     {
       return 0;
     }
  }      
/*---------------------------------------------------------------------------*/
  static int OptimiserInit(pOptimise self,SConnection *pCon)
  {
     int i, iRet;
     void *pData;
     pOVarEntry pOVar;
     float fVal;
     
     /* initialise each variable entry with the current position and
         set the shift to something senseless.
     */    
     for(i = 0; i < self->iVar; i++)
     {
        DynarGet(self->pVariables,i,&pData);
        pOVar = (pOVarEntry)pData;
        if(FindMotor(pServ->pSics,pOVar->pName) != NULL)
	{
            iRet = MotorGetSoftPosition(pOVar->pData,pCon,&fVal);      
            if(iRet == 1)
	    {
              pOVar->fCenter = fVal;
            }
            else
	    {
              pOVar->fCenter = -1000.;
            }
	}
        else
	{
            pOVar->fCenter = pOVar->pDriv->GetValue(pOVar->pData,pCon);
	}
        if(pOVar->fCenter < -900.)
        {
           return SCANERROR;
        } 
        pOVar->fShift = MAXFLOAT;
        pOVar->iLost = 0;
     }
     self->pScanner->iChannel = self->iChannel;
     return 1;
  }   
/*-------------------------------------------------------------------------*/
  static int CenterVariable(pOptimise self, SConnection *pCon, int i)
  {
     pOVarEntry pOvar;
     void *pData;
     int iRet, iReturn;
     float fStart;
     float fNewCenter, fStdDev, FWHM, fMax; 
     char pBueffel[512], cData[80];
     
     assert(self);
     assert( (i >= 0) && (i < self->iVar));
     assert(pCon);
     
     /* get variable data */
     DynarGet(self->pVariables,i,&pData);
     pOvar = (pOVarEntry)pData;
     
     /* do a scan first */
     fStart = pOvar->fCenter - (pOvar->iStep/2)*pOvar->fStep; 
     ClearScanVar(self->pScanner);
     iRet = AddScanVar(self->pScanner, pServ->pSics,pCon,
                       pOvar->pName, fStart, pOvar->fStep);
     if(!iRet)
     {
       return 0;
     } 
     sprintf(pBueffel,"Trying hard to optimise variable %s",pOvar->pName);
     SCWrite(pCon,pBueffel,eWarning);
     iRet = SilentScan(self->pScanner,pOvar->iStep,self->eCount,
                       self->fPreset,pServ->pSics,pCon);
     if(!iRet)
     {
        return SCANABORT;
     }                  
     
     /* try to find a fit for this one */
     iRet = CalculateFit(self->pPeakFitter);
     if(iRet == 0)
     {
        return SYSERROR;
     }
     GetFitResults(self->pPeakFitter, &fNewCenter,&fStdDev, &FWHM, &fMax);

     /* write some diagnostic messages */
     strcpy(pBueffel,"Peak Diagnosis: \n");
     sprintf(cData,"New %s position: %f \n",pOvar->pName,fNewCenter);
     switch(iRet)
     {
         case 1:
               strcat(pBueffel,"Peak found in scan range \n");
               strcat(pBueffel,cData);
               iReturn = 1;
               break;
         case -1:
               strcat(pBueffel,"Left half of peak missing \n");
               strcat(pBueffel,
                 "Extending scan range and setting position to maximum \n");
               strcat(pBueffel,cData);
               iReturn = VARREDO;
               break;
         case -2:
               strcat(pBueffel,"Right half of peak missing \n");
               strcat(pBueffel,
                 "Extending scan range and setting position to maximum \n");
               strcat(pBueffel,cData);
               iReturn = 2;
               break;
         case -3:
         case -4:
               strcat(pBueffel,"No data in scan \n");
               break;
         default:
               strcat(pBueffel,cData);
               sprintf(pData,"Fitting ended with error code %d \n",iRet);
               strcat(pBueffel,cData);
               break;
     }
     SCWrite(pCon,pBueffel,eWarning);

     if(fMax < self->fThreshold)
     {
         SCWrite(pCon,"Peak may be lost, increasing scan range",eWarning);
         pOvar->iLost++;
         if(pOvar->iLost > 2)
         {
            return PEAKLOST;
         }
     }
     else
     {
        pOvar->iLost = 0;
     }
     

     /* act upon the fits results */
     if(iRet != 1)      {
        /* not enough scan data for a proper evaluation of the 
           peak. But the peak fitter has given us the maximum of
           the counts. What we do is, we set the center to the
           maximum, increase the scan width and mark the variable
           for a redo in the next cycle by leaving the fShift
           high. But we do this only if the maximum is above a threshold,
           otherwise we just increase the scan range and leave the thing
           where it is.
        */
        if(fMax > self->fThreshold)
        {
           pOvar->fCenter = fNewCenter;
        }   
        pOvar->iStep += pOvar->iStep;
     }
     else /* the success case */
     {
        pOvar->fShift = ABS(pOvar->fCenter - fNewCenter);
        sprintf(pBueffel,"%s shifted by %8.2f ",pOvar->pName,pOvar->fShift);
        SCWrite(pCon,pBueffel,eWarning);
        pOvar->fPrecision = 3*fStdDev;
        sprintf(pBueffel,"%s precision set to 3*StdDev = %8.3f",
                pOvar->pName, 3*fStdDev);
        SCWrite(pCon,pBueffel,eWarning);
        pOvar->fCenter = fNewCenter;
     }
     /* drive to the new center */
     iRet = Drive(pCon,pServ->pSics,pOvar->pName,pOvar->fCenter);
     if(!iRet)
     {
       return DRIVEERROR;
     }
     
     return iReturn;                 
  }
/*---------------------------------------------------------------------------*/
  static int CheckSuccess(pOptimise self)
  {
    int i, iTest;
    pOVarEntry pOvar;
    void *pData;
    
    assert(self);
    for(i = 0; i < self->iVar; i++)
    {
      DynarGet(self->pVariables,i,&pData);
      pOvar = (pOVarEntry)pData;
      if(pOvar->fShift > pOvar->fPrecision)
      {
         return 0;
      }
    }
    return 1;
  }   
/*---------------------------------------------------------------------------*/
  int OptimiserRun(pOptimise self, SConnection *pCon)
  {
     int i, iRet, iCycle, iRedoVar = 0;
     char pBueffel[256];

     assert(self);
     
     if(self->iVar < 1)
     {
       SCWrite(pCon,"ERROR: Nothing to optimise",eError);
       return 0;
     }
     
     iRet = OptimiserInit(self,pCon);
     if(!iRet)
     {
       return iRet;
     }
     for(iCycle = 0; iCycle < self->iMaxCycles; iCycle++)
     {
        sprintf(pBueffel,"Optimiser cycle %d of %d started",iCycle, self->iMaxCycles);
        SCWrite(pCon,pBueffel,eStatus);
        for(i = iRedoVar; i < self->iVar; i++)
        {
           iRet = CenterVariable(self,pCon,i);
           if(iRet <= 0)
           {
             return iRet;
           }
           if(iRet == VARREDO)
           {
              iRedoVar = i;
              break;
           }   
           iRedoVar = 0;
        }
        iRet = CheckSuccess(self);
        if(iRet)
        {
          return 1;
        }
     }
     return MAXCYCLE;
  }         
/*--------------------------------------------------------------------------*/
  int MakeOptimiser(SConnection *pCon, SicsInterp *pSics, void *pData,
                    int argc, char *argv[])
  {
     CommandList *pCom;
     pICountable pCt = NULL;
     pCounter pCount;
     pOptimise pNew = NULL;
     char pBueffel[256];
     int iRet;
     
     /* check no of args */
     if(argc < 3)
     {
       SCWrite(pCon,"EEROR: Not enough arguments to create optimiser",eError);
       return 0;
     }
     
     /* 2 argument must be counter name */
     pCom = FindCommand(pSics,argv[2]);
     if(!pCom)
     {
        sprintf(pBueffel,"ERROR: Expected counter name, cannot find %s",argv[2]);
        SCWrite(pCon,pBueffel,eError);
        return 0;
     }
     pCt = GetCountableInterface(pCom->pData);
     if(!pCt)
     {
        sprintf(pBueffel,"ERROR: Expected counter name, BUT %s is NO counter",argv[2]);
        SCWrite(pCon,pBueffel,eError);
        return 0;
     }
     pCount = (pCounter)pCom->pData;
     
     pNew = CreateOptimiser(pCount);
     if(!pNew)
     {
        SCWrite(pCon,"ERROR: cannot create Optimiser",eError);
        return 0;
     }
     
     iRet = AddCommand(pSics,argv[1],OptimiserAction,
                       DeleteOptimiser,pNew);
     if(!iRet)
     {
       sprintf(pBueffel,"ERROR: Duplicate Command %s NOT created", argv[1]);
       SCWrite(pCon,pBueffel,eError);
       return 0;
     }
     return 1;
  }  
/*-------------------------------------------------------------------------*/
  int OptimiserAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                      int argc, char *argv[])
  {
     pOptimise self = NULL;
     char pBueffel[1024];
     int iRet;
     double d;
     float fStep, fPrec, fVal;
     int iStep;
     
     self = (pOptimise)pData;
     assert(self);
     assert(pCon);
     
     
     if(argc < 2)
     {
        sprintf(pBueffel,"ERROR: Insufficient arguments to %s",argv[0]);
        SCWrite(pCon,pBueffel,eError);
        return 0;
     }
     
     strtolower(argv[1]);
     if(strcmp(argv[1],"addvar") == 0)
/*--------- addvar */
     {
          /* check rights */
          if(!SCMatchRights(pCon,usUser))
          {
             SCWrite(pCon,"ERROR: You are not authorised to do this!",eError);
             return 0;
          }
          /* check no of args */
          if(argc < 6)
          {
               sprintf(pBueffel,"ERROR: Insufficient arguments to %s addvar",argv[0]);
               SCWrite(pCon,pBueffel,eError);
               return 0;
          }
          /* convert arguments to types */
          iRet = Tcl_GetDouble(pSics->pTcl,argv[3],&d);
          if(iRet != TCL_OK)
          {
            sprintf(pBueffel,"ERROR: expected float value for step but got %s",
                    argv[3]);
            SCWrite(pCon,pBueffel,eError);
            return 0;        
          }
          fStep = (float)d;
          iRet = Tcl_GetInt(pSics->pTcl,argv[4],&iStep);
          if(iRet != TCL_OK)
          {
            sprintf(pBueffel,"ERROR: expected integer value for iStep but got %s",
                    argv[3]);
            SCWrite(pCon,pBueffel,eError);
            return 0;        
          }
          iRet = Tcl_GetDouble(pSics->pTcl,argv[5],&d);
          if(iRet != TCL_OK)
          {
            sprintf(pBueffel,"ERROR: expected float value for precision but got %s",
                    argv[3]);
            SCWrite(pCon,pBueffel,eError);
            return 0;        
          }
          fPrec = (float)d;
          iRet = OptimiserAdd(self,argv[2],fStep,iStep,fPrec);
          if(!iRet)
          {
            sprintf(pBueffel,
              "ERROR: cannot optimise variable %s, mistyped?",argv[2]);
            SCWrite(pCon,pBueffel,eError);
            return 0;
          }
          SCSendOK(pCon);
          return 1;
     } 
/*----- clear */
     else if(strcmp(argv[1],"clear") == 0)
     {
          /* check rights */
          if(!SCMatchRights(pCon,usUser))
          {
             SCWrite(pCon,"ERROR: You are not aurhorised to do this!",eError);
             return 0;
          }
          OptimiserClear(self);
          SCSendOK(pCon);
          return 1;
     }     
/*-------- run */
     else if(strcmp(argv[1],"run") == 0)
     {
          /* check rights */
          if(!SCMatchRights(pCon,usUser))
          {
             SCWrite(pCon,"ERROR: You are not aurhorised to do this!",eError);
             return 0;
          }
          iRet = OptimiserRun(self,pCon);
          switch(iRet)
          {
            case PEAKLOST:
                  SCWrite(pCon,"ERROR: lost the peak, sorry!",eError);
                  return 0;
                  break;
            case MAXCYCLE:
                  sprintf(pBueffel,"ERROR: could not optimise peak in %d cycles",
                         self->iMaxCycles);
                  SCWrite(pCon,pBueffel,eError);
                  return 0;
                  break;
            case SCANERROR:
                  SCWrite(pCon,"ERROR: failed to scan the peak",eError);
                  return 0;
                  break;
            case SCANABORT:
                  SCWrite(pCon,"ERROR: Scan was aborted, Optimiser follows",eError);
                  return 0;
                  break;
            case DRIVEERROR:
                  SCWrite(pCon,"ERROR: Failure to drive variable to new position",eError);
                  return 0;
                  break;
            case 1:
                  SCWrite(pCon,"At long last, I finished optimising the peak",eValue);
                  return 1;
                  break;
            default:
                  SCWrite(pCon,"ERROR: Unidentified error krept into Optimiser",eError);
                  return 0;
                  break;            
          }        
     }
/* ------ count mode */
     if(strcmp(argv[1],"countmode") == 0)
     {
        if(argc > 2) /* set case */
        {
          /* check rights */
          if(!SCMatchRights(pCon,usUser))
          {
             SCWrite(pCon,"ERROR: You are not aurhorised to do this!",eError);
             return 0;
          }
          if(strcmp(argv[2],"timer") == 0)
          {
            fVal = 0.;
          }
          else if(strcmp(argv[2],"monitor") == 0)
          {
            fVal = 1.;
          }
          else
          {
            SCWrite(pCon,"ERROR: Invalid parameter for countmode",eError);
            return 0;
          }
          OptimiserSetPar(self,"countmode",fVal);
          SCSendOK(pCon);
          return 1;
        }
        else /* get case */
        {
          OptimiserGetPar(self,"countmode",&fVal);
          if(fVal < 0.05)
          {
            sprintf(pBueffel,"%s.countmode = timer",argv[0]);
          }
          else
          {
            sprintf(pBueffel,"%s.countmode = monitor", argv[0]);
          }
          SCWrite(pCon,pBueffel,eValue);
          return 1;
        }
     }       
/*------ can be other pars */
     else
     {
        if(argc > 2) /* set case */
        {
          /* check rights */
          if(!SCMatchRights(pCon,usUser))
          {
             SCWrite(pCon,"ERROR: You are not aurhorised to do this!",eError);
             return 0;
          }
          iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&d);
          if(iRet != TCL_OK)
          {
            sprintf(pBueffel,"ERROR: expected numeric value for %s but got %s",
                    argv[1],argv[2]);
            SCWrite(pCon,pBueffel,eError);
            return 0;        
          }
          fVal = (float)d;
          
          iRet = OptimiserSetPar(self,argv[1],fVal);
          if(iRet)
          {
              SCSendOK(pCon);
              return 1;
          }
          else
          {
            sprintf(pBueffel,"ERROR: parameter %s not known",argv[1]);
            SCWrite(pCon,pBueffel,eError);
            return 0;
          }    
        }
        else /* get case */
        {
          iRet = OptimiserGetPar(self,argv[1],&fVal);
          if(!iRet)
          {
            sprintf(pBueffel,"ERROR: parameter %s not known",argv[1]);
            SCWrite(pCon,pBueffel,eError);
            return 0;
          }    
          sprintf(pBueffel,"%s.%s = %f",argv[0],argv[1],fVal);
          SCWrite(pCon,pBueffel,eValue);
          return 1;
        }
     }        
  }                                        
