csmbase.c

Go to the documentation of this file.

/*
Copyright 2026 Helmholtz-Zentrum Berlin für Materialien und Energie GmbH
<https://www.helmholtz-berlin.de>
 
This file is part of CSM.
 
CSM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
 
CSM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with CSM.  If not, see <https://www.gnu.org/licenses/>.
*/

 
/*____________________________________________________________*/
/*                      general Defines                       */
/*____________________________________________________________*/
 
 
#ifndef DOXYGEN_SKIP_THIS

# define _INCLUDE_POSIX_SOURCE

# define USE_DBG 0

# define USE_ERRLOGPRINTF 1

# define USE_SEM 1

# define USE_PSEM 0
 
/* the following macros affect the debug (dbg) module: */
 
# if USE_DBG

#  define DBG_ASRT_COMP 0

#  define DBG_MSG_COMP 0

#  define DBG_TRC_COMP  0

#  define DBG_LOCAL_COMP 1

#  define DBG_ASYNC_COMP 1
 
/* the following defines are for sci-debugging, they do not influence
   any header-files but are placed here since they are usually changed
   together with the above macros.*/

#  define CSMBASE_OUT_FILE "stdout"

#  define CSMBASE_TRC_LEVEL 0

#  define CSMBASE_FLUSHMODE 1
 
# else /* USE_DBG */
 
#  if !USE_ERRLOGPRINTF
#   define errlogPrintf printf
#  endif

#  define DBG_MSG_PRINTF2(f,x) errlogPrintf(f,x)

#  define DBG_MSG_PRINTF3(f,x,y) errlogPrintf(f,x,y)

#  define DBG_MSG_PRINTF4(f,x,y,z) errlogPrintf(f,x,y,z)

#  define DBG_MSG_PRINTF5(f,x,y,z,q) errlogPrintf(f,x,y,z,q)
 
# endif /* USE_DBG */
 
# if !USE_SEM
#  define SEM_TYPE int
#  define SEMTAKE(x)
#  define SEMGIVE(x)
#  define SEMDELETE(x)
#  define SEMCREATE(x) 0
 
# else /* !USE_SEM */
 
#  if USE_PSEM
#   define SEM_TYPE psem_mutex
#   define SEMTAKE(x) psem_mutex_take(&(x))
#   define SEMGIVE(x) psem_mutex_give(&(x))
#   define SEMDELETE(x) psem_mutex_remove(&(x))
#   define SEMCREATE(x) (!psem_mutex_create(&(x)))
#   define epicsShareFunc
#  else /* USE_PSEM */
#   define SEM_TYPE epicsMutexId
#   define SEMTAKE(x) epicsMutexLock(x)
#   define SEMGIVE(x) epicsMutexUnlock(x)
#   define SEMDELETE(x) epicsMutexDestroy(x)
#   define SEMCREATE(x) (NULL==(x= epicsMutexCreate()))
#  endif /* USE_PSEM */
 
# endif /* !USE_SEM */
 
#endif /* DOXYGEN_SKIP_THIS */

#define CSM_TRUE 1

#define CSM_FALSE 0
 
/*____________________________________________________________*/
/*                       Include-Files                        */
/*____________________________________________________________*/
 
#if USE_SEM
 
#if USE_PSEM
#include <psem.h>
#else
#include <epicsMutex.h>
#include <shareLib.h>
#endif
 
#endif
 
#if USE_DBG
#include <dbg.h>
#endif
 
#if USE_ERRLOGPRINTF
#include <errlog.h> /* epics error printf */
#endif
 
#include <ctype.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
 
/* math.h for definition of NAN */
#include <math.h>
 
#define epicsExportSharedSymbols
#include "csmbase.h"
 
/*____________________________________________________________*/
/*                         Defines                            */
/*____________________________________________________________*/
 
#ifndef NAN
#define NAN (-(0.0/0.0))
#endif
 
/*____________________________________________________________*/
/*                          Types                             */
/*____________________________________________________________*/
 
      /*................................................*/
      /*          the coordinate type (private)         */
      /*................................................*/

typedef struct
  {
    double value; 
    int    index; 
  } csm_coordinate;

typedef struct
  { csm_coordinate *coordinate;  
    int            no_of_elements;
    int            a_last;       
    int            b_last;       
  } csm_coordinates;
 
      /*................................................*/
      /*        the linear function type (private)      */
      /*................................................*/

typedef struct
  { double a;         
    double b;         
  } csm_linear_function;
 
      /*................................................*/
      /*           the 1d table type (private)          */
      /*................................................*/

typedef struct
  { csm_coordinates x;     
    csm_coordinates y;     
    double x_last;         
    double y_last;         
    csm_bool value_cached; 
  } csm_1d_functiontable;
 
      /*................................................*/
      /*          the 2d table type (private)           */
      /*................................................*/

typedef struct
  { csm_coordinates x;   
    csm_coordinates y;   
    double          *z;  
    double x_last;         
    double y_last;         
    double z_last;         
    csm_bool value_cached; 
  } csm_2d_functiontable;
 
      /*................................................*/
      /*           the function-type (private)          */
      /*................................................*/
 
#ifndef DOXYGEN_SKIP_THIS
typedef enum { CSM_NOTHING,  
               CSM_LINEAR,   
               CSM_1D_TABLE, 
               CSM_2D_TABLE  
             } csm_func_type;
#endif

struct csm_Function
  { SEM_TYPE semaphore;   
    double   last_x;      
    double   last_y;      
    double   last_dx;     
    double   last_dy;     
    double   last_z;      
    csm_bool on_hold;     
    csm_func_type type;   
 
    union { csm_linear_function    lf;   
            csm_1d_functiontable   tf_1; 
            csm_2d_functiontable   tf_2; 
          } f;            
 
  };
 
/*____________________________________________________________*/
/*                        Variables                           */
/*____________________________________________________________*/
 
      /*................................................*/
      /*           initialization (private)             */
      /*................................................*/

static csm_bool initialized= CSM_FALSE;
 
/*____________________________________________________________*/
/*                        Functions                           */
/*____________________________________________________________*/
 
 
    /*----------------------------------------------------*/
    /*             debug - managment (private)            */
    /*----------------------------------------------------*/
 
#if USE_DBG
 
static void csm_dbg_init(void)
  { static csm_bool csmbase_init= CSM_FALSE;
 
    if (csmbase_init)
      return;
    csmbase_init= CSM_TRUE;
    /* initialize the debug (dbg) module: use stdout for error-output */
    DBG_SET_OUT(CSMBASE_OUT_FILE);
    DBG_TRC_LEVEL= CSMBASE_TRC_LEVEL;
                           /* level 6: dense traces for debugging
                              level 5: enter/exit tracing
                              level 2: output less severe errors and warnings
                              level 1: output of severe errors
                              level 0: no output */
    DBG_FLUSHMODE(CSMBASE_FLUSHMODE);
  }
 
#endif /* USE_DBG */
 
    /*----------------------------------------------------*/
    /*        utilities for csm-coordinates (private)     */
    /*----------------------------------------------------*/
 
        /*              initialization                */

static void init_coordinates(csm_coordinates *c)
  {
    c->a_last=-1;
    c->b_last=-1;
    c->coordinate= NULL;
    c->no_of_elements=0;
  }

static csm_bool alloc_coordinates(csm_coordinates *c, int elements)
  { int i;
    csm_coordinate *co;
 
    if (c->no_of_elements==0) /* 1st time memory allocation */
      c->coordinate= malloc(sizeof(csm_coordinate)*elements);
    else
      c->coordinate= realloc(c->coordinate,
                             sizeof(csm_coordinate)*elements);
 
    if (c->coordinate==NULL) /* allocation error */
      { DBG_MSG_PRINTF2("error in csm:alloc_coordinates line %d,\n"
                        "allocation failed!\n", __LINE__);
        init_coordinates(c);
        return(CSM_FALSE);
      };
 
    for(i=c->no_of_elements, co= (c->coordinate + c->no_of_elements);
        i<elements;
        i++, co++)
      { co->value=0;
        co->index=i;
      };
 
    c->no_of_elements= elements;
    return(CSM_TRUE);
  }

static csm_bool resize_coordinates(csm_coordinates *c, int elements)
  { if (elements==c->no_of_elements)
      return(CSM_TRUE);
 
    c->no_of_elements= elements;
    if (NULL== (c->coordinate= realloc(c->coordinate,
                                       sizeof(csm_coordinate)*elements)))
      { DBG_MSG_PRINTF2("error in csm:resize_coordinates line %d,\n" \
                        "realloc failed!\n", __LINE__);
        return(CSM_FALSE);
      };
    return(CSM_TRUE);
  }

static void reinit_coordinates(csm_coordinates *c)
  { if (c->no_of_elements==0)
      return;
 
    free(c->coordinate);
    init_coordinates(c);
  }

static csm_bool init_matrix(double **z, int rows, int columns)
  { int i;
    double *zp;
 
    if (NULL== (zp= malloc((sizeof(double))*rows*columns)))
      { DBG_MSG_PRINTF2("error in csm:init_matrix line %d,\n" \
                        "malloc failed!\n", __LINE__);
        return(CSM_FALSE);
      };
    *z = zp;
    for (i= rows*columns; i>0; i--, *(zp++)=0);
    return(CSM_TRUE);
  }
 
        /*            x-compare for qsort             */

static int coordinate_cmp(const void *t1, const void *t2)
  { double x1= ((csm_coordinate *)(t1))->value;
    double x2= ((csm_coordinate *)(t2))->value;
 
    if (x1 < x2)
      return(-1);
    if (x1 > x2)
      return( 1);
    return(0);
  }
 
        /*              coordinate-sort               */

static void coordinate_sort( csm_coordinates *coords)
  {
    qsort( coords->coordinate, coords->no_of_elements,
           sizeof(csm_coordinate), coordinate_cmp);
  }

static void coordinate_update_backlinks( csm_coordinates *coords1,
                                         csm_coordinates *coords2)
  { int i,l;
    csm_coordinate *c,*d;
 
    l= coords1->no_of_elements;
    for (i=0, c= coords1->coordinate, d= coords2->coordinate; i<l; i++, c++)
      { d[c->index].index = i; };
  }
 
        /*      lookup an index in coordinates        */

 
#define ret(idx_a, idx_b, ret_code) \
  *a= idx_a;\
  *b= idx_b;\
  coords->a_last= idx_a;\
  coords->b_last= idx_b;\
  return(ret_code)
  
static int coordinate_lookup_index(csm_coordinates *coords,double x,
                                   int *a, int *b)
  /* 2: x was found, it is returned in a
     1: x was not found, but a and b define the closest interval around x
     0: x was not found at it is outside the closest interval a and b
    -1: error
  */
  /* the table should be x-sorted */
  { int i, ia, ib;
    double xt;
    csm_coordinate *c= coords->coordinate;
    double x_boundary;
    int last_idx= coords->no_of_elements-1;
    int a_last, b_last;
    int tries;
 
    if (coords->no_of_elements<2)
      { if (coords->no_of_elements<1)
          {
            DBG_MSG_PRINTF2("error in csm:coordinate_lookup_index %d,\n" \
                            "table has less than 1 element!\n", __LINE__);
            ret(0,0,-1);
           };
        coords->a_last= 0; coords->b_last= 0; 
        if (x== c[0].value)
          {
            ret(0,0,2);
          }
        else
          {
            ret(0,0,0);
          }
      };
 
    a_last= coords->a_last;
    b_last= coords->b_last;
    if (a_last<0)
      a_last= 0;
    if (b_last<0)
      b_last= last_idx;
 
    for(tries=1; tries<=3; tries++)
      {
        switch(tries)
          {
            case 1:
              break;
            case 2:
              b_last= a_last;
              a_last= a_last-1;
              break;
            case 3:
              b_last= a_last;
              a_last= 0;
              break;
          }
 
        x_boundary= c[a_last].value;
        if (x>x_boundary)
          break;
        if (x==x_boundary)
          {
            ret(a_last,a_last,2);
          }
        if (a_last==0)
          { /* left of area */
            ret(0,1, 0);
          }
      }
 
    for(tries=1; tries<=3; tries++)
      {
        switch(tries)
          {
            case 1:
              break;
            case 2:
              a_last= b_last;
              b_last= b_last+1;
              break;
            case 3:
              a_last= b_last;
              b_last= last_idx;
              break;
          }
 
        x_boundary= c[b_last].value;
        if (x<x_boundary)
          break;
        if (x==x_boundary)
          {
            ret(b_last,b_last,2);
          }
        if (b_last==last_idx)
          { /* right of area */
            ret(last_idx-1,last_idx, 0);
          }
      }
 
    ia= a_last;
    ib= b_last;
    /* necessary condition here:
       c[ia].value<= x <= c[ib].value
     */
    for (; (ib>ia+1); )
      { i= (ib+ia)/2;
        xt= c[i].value;
        if (xt<x)
          {
            ia= i;
            continue;
          }
        if (xt>x)
          {
            ib= i;
            continue;
          }
        /* exact match */
        ret(i,i,2);
      };
    ret(ia,ib,1);
  }
 
    /*----------------------------------------------------*/
    /*     utilities for csm_linear_function (private)    */
    /*----------------------------------------------------*/
 
        /*               calculation                  */

static double linear_get_y(csm_linear_function *p, double x)
  { return( (p->a) + (p->b)*x ); }

static double linear_get_x(csm_linear_function *p, double y)
  { return( (y - (p->a))/(p->b) ); }

static double linear_delta_get_y(csm_linear_function *p, double x)
  { return( (p->b)*x ); }

static double linear_delta_get_x(csm_linear_function *p, double y)
  { return( y/(p->b) ); }
 
    /*----------------------------------------------------*/
    /*    utilities for csm_1d_functiontable (private)    */
    /*----------------------------------------------------*/
 
        /*              initialization                */

static void init_1d_functiontable(csm_1d_functiontable *ft)
  { init_coordinates(&(ft->x));
    init_coordinates(&(ft->y));
    ft->value_cached= CSM_FALSE;
  }

static void reinit_1d_functiontable(csm_1d_functiontable *ft)
  { reinit_coordinates(&(ft->x));
    reinit_coordinates(&(ft->y));
    ft->value_cached= CSM_FALSE;
  }
 
        /*        lookup a value in the table         */

static double lookup_1d_functiontable(csm_1d_functiontable *ft, double x,
                                      csm_bool invert)
  { int res,a,b;
    double xa,xb,ya,yb;
    csm_coordinate c;
    csm_coordinates *xcoords;
    csm_coordinates *ycoords;
 
    if (!invert)
      { 
        if ((ft->value_cached) && (ft->x_last==x))
          return(ft->y_last);
        xcoords= &(ft->x);
        ycoords= &(ft->y);
      }
    else
      { 
        if ((ft->value_cached) && (ft->y_last==x))
          return(ft->x_last);
        xcoords= &(ft->y);
        ycoords= &(ft->x);
      };
 
    res= coordinate_lookup_index(xcoords, x, &a, &b);
 
 
    if (res==-1) /* error */
      {
        ft->value_cached= CSM_FALSE;
        return(NAN);
      }
    if ((res== 2)||(a==b)) /* exact match or table with just a single value */
      { 
        /* if the table has just a single value (only one [x,y] pair) we return
           the corresponding y value. */
        c= (xcoords->coordinate)[a];
        ft->value_cached= CSM_TRUE;
        if (!invert)
          {
            ft->x_last= x;
            return(ft->y_last= (ycoords->coordinate)[c.index].value);
          }
        else
          {
            ft->y_last= x;
            return(ft->x_last= (ycoords->coordinate)[c.index].value);
          }
      };
 
    c= (xcoords->coordinate)[a];
    xa= c.value;
    ya= (ycoords->coordinate)[c.index].value;
 
    c= (xcoords->coordinate)[b];
    xb= c.value;
    yb= (ycoords->coordinate)[c.index].value;
 
    ft->value_cached= CSM_TRUE;
 
    if (!invert)
      {
        ft->x_last= x;
        /* y= ya + (x-xa)/(xb-xa)*(yb-ya) */
        return(ft->y_last= ya + (x-xa)/(xb-xa) * (yb-ya));
      }
    else
      {
        ft->y_last= x;
        /* y= ya + (x-xa)/(xb-xa)*(yb-ya) */
        return(ft->x_last= ya + (x-xa)/(xb-xa) * (yb-ya));
      }
 
  }
 
    /*----------------------------------------------------*/
    /*    utilities for csm_2d_functiontable (private)    */
    /*----------------------------------------------------*/
 
        /*              initialization                */

static void init_2d_functiontable(csm_2d_functiontable *ft)
  { init_coordinates(&(ft->x));
    init_coordinates(&(ft->y));
    ft->value_cached= CSM_FALSE;
  }

static void reinit_2d_functiontable(csm_2d_functiontable *ft)
  { reinit_coordinates(&(ft->x));
    reinit_coordinates(&(ft->y));
    if (ft->z!=NULL)
      free(ft->z);
    ft->z= NULL;
    ft->value_cached= CSM_FALSE;
  }
 
        /*       lookup a value in the xy-table       */

static double lookup_2d_functiontable(csm_2d_functiontable *ft,
                                      double x, double y)
  { int res,xia,xib,yia,yib;
    double xa,xb,ya,yb;
    double zaa,zab,zba,zbb;
    double *z= ft->z;
    int no_of_columns;
 
    csm_coordinate cxa,cxb,cya,cyb;
    csm_coordinates *xcoords= &(ft->x);
    csm_coordinates *ycoords= &(ft->y);
 
    if (ft->value_cached)
      {
        if ((ft->x_last==x) && (ft->y_last==y))
          return(ft->z_last);
      }
 
    no_of_columns= ycoords->no_of_elements;
 
    res= coordinate_lookup_index(xcoords, x, &xia, &xib);
    if (res==-1) /* error */
      {
        ft->value_cached= CSM_FALSE;
        return(NAN);
      }
 
    res= coordinate_lookup_index(ycoords, y, &yia, &yib);
    if (res==-1) /* error */
      {
        ft->value_cached= CSM_FALSE;
        return(NAN);
      }
 
    /* res==2 with exact match is currently not treated separately */
 
    cxa= (xcoords->coordinate)[xia];
    cxb= (xcoords->coordinate)[xib];
    cya= (ycoords->coordinate)[yia];
    cyb= (ycoords->coordinate)[yib];
 
    xa= cxa.value; /* with exact match, xa==xb or ya==yb is possible */
    xb= cxb.value;
    ya= cya.value;
    yb= cyb.value;
 
    zaa= z[ cxa.index * no_of_columns + cya.index ];
    zab= z[ cxa.index * no_of_columns + cyb.index ];
    zba= z[ cxb.index * no_of_columns + cya.index ];
    zbb= z[ cxb.index * no_of_columns + cyb.index ];
 
#if 0
printf("x:%f y:%f xa:%f xb:%f ya:%f yb:%f\n",x,y,xa,xb,ya,yb);
printf("zaa:%f zab:%f zba:%f zbb:%f\n",zaa,zab,zba,zbb);
#endif
#if 1
    { double alpha = (xb==xa) ? 0 : (x-xa)/(xb-xa);
      double beta  = (yb==ya) ? 0 : (y-ya)/(yb-ya);
      if (alpha==0)               /* xb==xa ? */
        { 
          ft->value_cached= CSM_TRUE;
          ft->x_last= x;
          ft->y_last= y;
          if (beta==0)            /* yb==ya ? */
            return(ft->z_last= zaa);
          return(ft->z_last= zaa+ beta *(zab-zaa) );
        };
      if (beta==0)                /* yb==ya ? */
        {
          ft->value_cached= CSM_TRUE;
          ft->x_last= x;
          ft->y_last= y;
          return(ft->z_last= zaa+ alpha*(zba-zaa) );
        }
 
#if 0
printf(" ret: %f\n",
       zaa + beta*(zab-zaa)+alpha*(zba-zaa+beta*(zbb-zba-zab+zaa)));
#endif
      ft->value_cached= CSM_TRUE;
      ft->x_last= x;
      ft->y_last= y;
      return(ft->z_last= 
              zaa + beta*(zab-zaa)+alpha*(zba-zaa+beta*(zbb-zba-zab+zaa)) );
    };
#endif
 
#if 0
    { /* alternative according to formula given by J.Bahrdt */
      /* doesn't work when yb==ya or xb==xa */
      double delta= (yb-ya)*(xb-xa);
      double yb_  = yb - y;
      double xb_  = xb - x;
      double y_a  = y  - ya;
      double x_a  = x  - xa;
      return((zaa*yb_*xb_ + zab*y_a*xb_ + zba*yb_*x_a + zbb*y_a*x_a)/delta);
#endif
 
 }
 
    /*----------------------------------------------------*/
    /*              generic functions (public)            */
    /*----------------------------------------------------*/
 
double csm_x(csm_function *func, double y)
  { double ret;
 
    SEMTAKE(func->semaphore);
 
    if (func->on_hold)
      { ret= func->last_x;
        SEMGIVE(func->semaphore);
        return(ret);
      };
 
    switch (func->type)
      { case CSM_NOTHING:
               func->last_x= 0;
               break;
        case CSM_LINEAR:
               func->last_x= linear_get_x(&(func->f.lf), y);
               break;
        case CSM_1D_TABLE:
               func->last_x= lookup_1d_functiontable(&(func->f.tf_1),y,
                                                   CSM_TRUE);
               break;
        default:
               func->last_x=0;
               break;
      };
    ret= func->last_x;
    SEMGIVE(func->semaphore);
    return(ret);
  }
 
double csm_y(csm_function *func, double x)
  { double ret;
 
    SEMTAKE(func->semaphore);
 
    if (func->on_hold)
      { ret= func->last_y;
        SEMGIVE(func->semaphore);
        return(ret);
      };
 
    switch (func->type)
      { case CSM_NOTHING:
               func->last_y= 0;
               break;
        case CSM_LINEAR:
               func->last_y= linear_get_y(&(func->f.lf), x);
               break;
        case CSM_1D_TABLE:
               func->last_y= lookup_1d_functiontable(&(func->f.tf_1),x,
                                                   CSM_FALSE);
               break;
        default:
               func->last_y=0;
               break;
      };
    ret= func->last_y;
    SEMGIVE(func->semaphore);
    return(ret);
  }
 
double csm_dx(csm_function *func, double y)
  { double ret;
 
    SEMTAKE(func->semaphore);
 
    if (func->on_hold)
      { ret= func->last_dx;
        SEMGIVE(func->semaphore);
        return(ret);
      };
 
    if (func->type==CSM_LINEAR)
      func->last_dx= linear_delta_get_x(&(func->f.lf), y);
    else
      func->last_dx=0;
 
    ret= func->last_dx;
    SEMGIVE(func->semaphore);
    return(ret);
  }
 
double csm_dy(csm_function *func, double x)
  { double ret;
 
    SEMTAKE(func->semaphore);
 
    if (func->on_hold)
      { ret= func->last_dy;
        SEMGIVE(func->semaphore);
        return(ret);
      };
 
    if (func->type==CSM_LINEAR)
      func->last_dy= linear_delta_get_y(&(func->f.lf), x);
    else
      func->last_dy=0;
 
    ret= func->last_dy;
    SEMGIVE(func->semaphore);
    return(ret);
  }
 
double csm_z(csm_function *func, double x, double y)
  { double ret;
 
    SEMTAKE(func->semaphore);
 
    if (func->on_hold)
      { ret= func->last_z;
        SEMGIVE(func->semaphore);
        return(ret);
      };
 
    if (func->type==CSM_2D_TABLE)
      func->last_z= lookup_2d_functiontable(&(func->f.tf_2), x, y);
    else
      func->last_z=0;
 
    ret= func->last_z;
    SEMGIVE(func->semaphore);
    return(ret);
  }
 
    /*----------------------------------------------------*/
    /*                   initialization                   */
    /*----------------------------------------------------*/
 
      /*   initialize a csm_function object (private)   */

static void reinit_function(csm_function *func)
  { if (func->type==CSM_NOTHING)
      { return; };
    if (func->type==CSM_1D_TABLE)
      reinit_1d_functiontable( &(func->f.tf_1) );
    if (func->type==CSM_2D_TABLE)
      reinit_2d_functiontable( &(func->f.tf_2) );
 
    /* @@@@ CAUTION: currently there is no locking,
       no test wether another process is currently
       accessing the structure */
    func->type= CSM_NOTHING;
  }
 
      /*  scan a string for a list of doubles (private) */

static csm_bool str_empty_or_comment(char *st)
  { for(;*st!=0; st++)
      { if (isspace(*st))
          continue;
        if (*st=='#')
          return(CSM_TRUE);
        return(CSM_FALSE);
      };
    return(CSM_TRUE);
  }
 
      /*  scan a string for a list of doubles (private) */

static int strdoublescan(char *st, double *d, int no_of_cols)
  /* returns the number of numbers found in the line, d may be null,
     in this case, the numbers are only counted,
     if a line contains not enough numbers, (unequal no_of_cols) the missing
     ones are set to 0 */
  /* reads over leading whitespaces */
  /* uses strtok ! */
  { int i;
    char *p= st;      /* anything different from NULL */
    char buf[1024];
    char *str= buf;
    double *dptr;
 
    if ((no_of_cols<=0)||(no_of_cols>1000))
      return(0);
    for( ;NULL!=strchr( " \t\r\n", *st); st++);
    if (*st==0)
      return(0);
    strncpy(buf, st, 1023); buf[1023]=0;
    if (d!=NULL)
      for(dptr= d, i=0; i<no_of_cols; i++, *(dptr++)=0 );
    for(p= str, dptr= d, i=0; i<no_of_cols; i++, dptr++)
      {
        if (NULL== (p= strtok(str," \t\r\n")))
          return(i);
        str= NULL;
        if (d!=NULL)
          { if (1!=sscanf(p,"%lf",dptr))
              return(0); /* a kind of fatal error */
          };
      };
    return(no_of_cols);  /* everything that was expected was found */
  }
 
      /*          clear a function (public)             */
 
void csm_clear(csm_function *func)
  { SEMTAKE(func->semaphore);
    func->on_hold= CSM_TRUE;
    SEMGIVE(func->semaphore);
 
    reinit_function(func);
 
    func->on_hold= CSM_FALSE;
  }
 
      /*   free all data for a function (public)        */
 
void csm_free(csm_function *func)
  { csm_clear(func);
    SEMDELETE(func->semaphore);
    free(func);
  }
 
      /*          define a function (public)            */
 
void csm_def_linear(csm_function *func, double a, double b)
  /* y= a+b*x */
  { SEMTAKE(func->semaphore);
    func->on_hold= CSM_TRUE;
    SEMGIVE(func->semaphore);
 
    reinit_function(func);
 
    func->type= CSM_LINEAR;
    (func->f.lf).a= a;
    (func->f.lf).b= b;
 
    func->on_hold= CSM_FALSE;
  }
 
csm_bool csm_def_linear_offset(csm_function *func, double a)
  { if (func->type!=CSM_LINEAR)
      { DBG_MSG_PRINTF2("error in csm_def_linear_offset line %d,\n" \
                        "not a linear function!\n", __LINE__);
        return(CSM_FALSE);
      };
 
    SEMTAKE(func->semaphore);
    func->f.lf.a= a;
    SEMGIVE(func->semaphore);
 
    return(CSM_TRUE);
  }
 
csm_bool csm_fill_1d_table(double *x, double *y, int len, csm_function *func)
  /* fill the 1d table numbers of two arrays.
   * len is the number of rows in the table
   */
  {
    csm_1d_functiontable *ft= &(func->f.tf_1);
    csm_coordinate *xc, *yc;
    int row;
 
    SEMTAKE(func->semaphore);
    func->on_hold= CSM_TRUE;
    SEMGIVE(func->semaphore);
 
    reinit_function(func);
 
    init_1d_functiontable(ft);
 
    if (!alloc_coordinates(&(ft->x), len))
      {
        reinit_function(func);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    if (!alloc_coordinates(&(ft->y), len))
      {
        reinit_function(func);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    xc= (ft->x).coordinate;
    yc= (ft->y).coordinate;
 
    for(row=0; row<len; row++, xc++, yc++, x++, y++)
      {
        xc->value= *x;
        yc->value= *y;
        xc->index= row;
        yc->index= row;
      }
 
    func->type= CSM_1D_TABLE;
 
    coordinate_sort(&(ft->x));
    coordinate_update_backlinks(&(ft->x), &(ft->y));
    coordinate_sort(&(ft->y));
    coordinate_update_backlinks(&(ft->y), &(ft->x));
    func->on_hold= CSM_FALSE;
    return(CSM_TRUE);
  }
 
csm_bool csm_read_1d_table(char *filename, csm_function *func)
/* if len==0, the table length is determined by counting the number of lines
   in the file from the current position to it's end */
  { char line[128];
    long pos;
    long i;
    long errcount;
    csm_1d_functiontable *ft= &(func->f.tf_1);
    csm_coordinate *xc, *yc;
    int len;
    FILE *f;
    char dummy;
 
    if (NULL==(f=fopen(filename,"r"))) /* vxworks doesn't accept "rt" */
      { DBG_MSG_PRINTF2("error in csm_read_xytable line %d,\n" \
                        "file open error!\n", __LINE__);
        return(CSM_FALSE);
      };
 
    pos= ftell(f);
    for(len=0; NULL!=fgets(line, 127, f); len++);
    if (-1==fseek(f, pos, SEEK_SET))
      { fclose(f);
        return(CSM_FALSE);
      };
 
    SEMTAKE(func->semaphore);
    func->on_hold= CSM_TRUE;
    SEMGIVE(func->semaphore);
 
    reinit_function(func);
 
    init_1d_functiontable(ft);
 
    if (!alloc_coordinates(&(ft->x), len))
      { fclose(f);
        reinit_function(func);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    if (!alloc_coordinates(&(ft->y), len))
      { fclose(f);
        reinit_function(func);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    xc= (ft->x).coordinate;
    yc= (ft->y).coordinate;
 
    for(errcount=0, i=0;(len>0) && (NULL!=fgets(line, 127, f)); len--)
      { if (str_empty_or_comment(line))
          continue;
        if (2!=sscanf(line, " %lf %lf %c",
                      &(xc->value), &(yc->value), &dummy))
          { DBG_MSG_PRINTF5("warning[%s:%d]: the following line of the "
                   "data-file (%s) was not understood:\n%s\n",
                   __FILE__,__LINE__,filename,line);
            if (++errcount<4)
              continue;
            fclose(f);
            reinit_function(func);
            func->on_hold= CSM_FALSE;
            DBG_MSG_PRINTF3("error[%s:%d]: too many errors in file\n",
                          __FILE__,__LINE__);
            return(CSM_FALSE);
          };
        xc->index= i;
        yc->index= i;
        i++, xc++,yc++;
      };
    if (i<=0)
      { fclose(f);
        reinit_function(func);
        func->on_hold= CSM_FALSE;
        DBG_MSG_PRINTF3("error[%s:%d]: no data was found at all\n",
                      __FILE__,__LINE__);
        return(CSM_FALSE);
      };
 
    if (!resize_coordinates(&(ft->x), i))
      { fclose(f);
        reinit_function(func);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
    if (!resize_coordinates(&(ft->y), i))
      { fclose(f);
        reinit_function(func);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
    fclose(f);
 
    func->type= CSM_1D_TABLE;
 
    coordinate_sort(&(ft->x));
    coordinate_update_backlinks(&(ft->x), &(ft->y));
    coordinate_sort(&(ft->y));
    coordinate_update_backlinks(&(ft->y), &(ft->x));
    func->on_hold= CSM_FALSE;
    return(CSM_TRUE);
  }
 
csm_bool csm_read_2d_table(char *filename, csm_function *func)
/* file format:
    y1   y2   y3  ..
x1  z11  z12  z13 ...
x2  z21  z22  z23 ...
.    .    .    .
.    .    .    .
.    .    .    .
*/
  { char line[1024];
    long pos;
    long i,j,lines,errcount;
    double *buffer;
    double *zptr;
    csm_2d_functiontable *ft= &(func->f.tf_2);
    csm_coordinate *xc, *yc;
    FILE *f;
    int columns,rows;
 
    if (NULL==(f=fopen(filename,"r"))) /* vxworks doesn't accept "rt" */
      { DBG_MSG_PRINTF2("error in csm_read_xytable line %d,\n" \
                        "file open error!\n", __LINE__);
        return(CSM_FALSE);
      };
 
    if (NULL==fgets(line, 1024, f))
      { fclose(f);
        return(CSM_FALSE);
      };
 
    columns= strdoublescan(line, NULL, 512);
    if (columns<1)
      { fclose(f);
        return(CSM_FALSE);
      };
    if (NULL==(buffer= malloc(sizeof(double)*(columns+1))))
      { DBG_MSG_PRINTF2("MALLOC FAILED in file %s\n",__FILE__);
        fclose(f);
        return(CSM_FALSE);
      };
    if (columns!= (i= strdoublescan(line, buffer, columns)))
      { DBG_MSG_PRINTF3("unexpected err in line %d in file %s\n",
                        __LINE__,__FILE__);
        free(buffer);
        fclose(f);
        return(CSM_FALSE);
      };
 
    SEMTAKE(func->semaphore);
    func->on_hold= CSM_TRUE;
    SEMGIVE(func->semaphore);
 
    reinit_function(func);
    init_2d_functiontable(ft);
 
    if (!alloc_coordinates(&(ft->y), columns))
      { free(buffer);
        reinit_function(func);
        fclose(f);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    yc= (ft->y).coordinate;
 
    /* copy y-values into the csm_table_function - structure */
    { double *r= buffer;
      csm_coordinate *c= yc;
      for(i=0; i< columns; i++, c++, r++ )
        { c->value= *r;
          c->index= i;
        };
    };
 
    pos= ftell(f);
    for(lines=0; NULL!=fgets(line, 1024, f); lines++);
    if (-1==fseek(f, pos, SEEK_SET))
      { free(buffer);
        reinit_function(func);
        fclose(f);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
    rows= lines;
 
    if (!alloc_coordinates(&(ft->x), rows))
      { free(buffer);
        reinit_function(func);
        fclose(f);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    xc= (ft->x).coordinate;
 
    if (!init_matrix(&(ft->z), rows, columns))
      { free(buffer);
        reinit_function(func);
        fclose(f);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    zptr= ft->z;
 
    for(errcount=0, i=0; (lines>0) && (NULL!=fgets(line, 1024, f)); lines--)
      {
        if (str_empty_or_comment(line))
          continue;
        if (columns+1 != strdoublescan(line, buffer, columns+1))
          { DBG_MSG_PRINTF5("warning[%s:%d]: the following line of the "
                            "data-file (%s) was not understood:\n%s\n",
                            __FILE__,__LINE__,filename,line);
            if (++errcount<4)
              continue;
            fclose(f);
            reinit_function(func);
            func->on_hold= CSM_FALSE;
            DBG_MSG_PRINTF3("error[%s:%d]: too many errors in file\n",
                          __FILE__,__LINE__);
            return(CSM_FALSE);
          };
 
        xc->value= buffer[0];
        xc->index= i;
 
        zptr= &((ft->z)[i*columns]);
        for (j=0; j<columns; j++)
          zptr[j]= buffer[j+1];
 
        i++, xc++;
      };
 
    if (i<=0)
      { free(buffer);
        reinit_function(func);
        fclose(f);
        func->on_hold= CSM_FALSE;
        DBG_MSG_PRINTF3("error[%s:%d]: no data was found at all\n",
                      __FILE__,__LINE__);
        return(CSM_FALSE);
      };
 
    rows= i;
    if (!resize_coordinates(&(ft->x), rows))
      { free(buffer);
        reinit_function(func);
        fclose(f);
        func->on_hold= CSM_FALSE;
        return(CSM_FALSE);
      };
 
    fclose(f);
    free(buffer);
 
    func->type= CSM_2D_TABLE;
 
    coordinate_sort(&(ft->x));
    coordinate_sort(&(ft->y));
    func->on_hold= CSM_FALSE;
    return(CSM_TRUE);
  }
 
 
      /*         initialize the module (public)         */
 
void csm_init(void)
  {
    if (initialized)
      return;
    initialized= CSM_TRUE;
#if USE_DBG
    csm_dbg_init();
#endif
  }
 
      /*          create a new function object          */
 
csm_function *csm_new_function(void)
  {
    csm_function *f= malloc(sizeof(csm_function));
 
    if (f==NULL)
      return(NULL);
 
    f->type= CSM_NOTHING;
    f->last_x = 0;
    f->last_y = 0;
    f->last_dx= 0;
    f->last_dy= 0;
    f->last_z = 0;
    f->on_hold= CSM_FALSE;
    if (SEMCREATE(f->semaphore))
      { free(f);
        return(NULL);
      };
    return((csm_function *)f);
  }
 
    /*----------------------------------------------------*/
    /*                 debugging (public)                 */
    /*----------------------------------------------------*/

static void csm_pr_coordinates(csm_coordinates *c)
  { int i;
    printf("no . of elements: %d\n", c->no_of_elements);
    if (c->a_last==-1)
      printf("(still in initial-state)\n");
    else
      { printf("a_last:%d  b_last:%d\n",
        c->a_last, c->b_last);
      };
    for(i=0; i< c->no_of_elements; i++)
      printf(" [%03d]: %12f --> %3d\n",
             i,(c->coordinate)[i].value,(c->coordinate)[i].index);
  }

static void csm_pr_1d_table_elm(csm_1d_functiontable *t, int index)
  {
    /* print (x,y) in the order of the x coordinates. */
    csm_coordinate xc= (t->x).coordinate[index];
    printf("x: %15f   y: %15f\n",
           xc.value, ((t->y).coordinate)[xc.index].value);
  }

static void csm_pr_linear(csm_linear_function *lf)
  { printf("a:%f  b:%f  (y=a+b*x)\n", lf->a, lf->b); }

static void csm_pr_1d_table(csm_1d_functiontable *tf)
  { int i;
    int l= (tf->x).no_of_elements;
 
    if (tf->x.a_last==-1)
      printf("x-coord is still in initial-state\n");
    else
      printf("x-a_last:%d  x-b_last:%d\n", tf->x.a_last, tf->x.b_last);
    if (tf->y.a_last==-1)
      printf("x-coord is still in initial-state\n");
    else
      printf("y-a_last:%d  y-b_last:%d\n", tf->y.a_last, tf->y.b_last);
    printf("number of elements: %d\n", l);
    for(i=0; i<l; i++)
      csm_pr_1d_table_elm( tf, i);
  }

static void csm_pr_2d_table(csm_2d_functiontable *ft)
  { int i,j;
    int rows   = ft->x.no_of_elements;
    int columns= ft->y.no_of_elements;
    double *z= ft->z;
    csm_coordinate *xc= ft->x.coordinate;
    csm_coordinate *yc= ft->y.coordinate;
 
    printf("rows:%d  columns:%d\n",
           rows, columns);
    printf("%18s"," ");
    for (j=0; j<columns; j++)
      printf("%15f | ", yc[j].value);
    printf("\n");
    printf("------------------");
    for (j=0; j<columns; j++)
      { printf("---------------"); };
    for(i=0; i<rows; i++)
      { printf("%15f | ", xc[i].value);
        for (j=0; j<columns; j++)
          printf("%15f | ", z[columns*i+j]);
        printf("\n");
      };
  }
 
void csm_pr_func(csm_function *func)
  { if (func->on_hold)
      printf("function is on hold!\n");
    else
      printf("function is operational\n");
 
    printf("Last calculated values: x:%f y:%f z:%f dx:%f dy%f\n",
           func->last_x, func->last_y, func->last_z,
           func->last_dx, func->last_dy);
 
    printf("function type: \n");
    switch( func->type)
      { case CSM_NOTHING:
             printf("CSM_NOTHING\n");
             break;
        case CSM_LINEAR:
             printf("CSM_LINEAR\n");
             csm_pr_linear(&(func->f.lf));
             break;
        case CSM_1D_TABLE:
             printf("CSM_1D TABLE\n");
             printf("normal function:\n");
             csm_pr_1d_table(&(func->f.tf_1));
             break;
        case CSM_2D_TABLE:
             printf("CSM_2D_TABLE\n");
             csm_pr_2d_table(&(func->f.tf_2));
             break;
        default:
             printf("%d (unknown)\n", func->type);
             break;
      };
  }