/***********************************************************************
 *  
 * generic_functions.c
 * 
 *           Tim Tuttle
 *           Begun on May 26, 1991
 *           Last revision:
 *
 * This file contains many useful functions that do not rely on
 * the condor system, and therefore they may be used in any
 * C program. 
 *
 * Copyright 1993 Massachusetts Institute of Technology
 *
 */

#include <stdio.h>
#include <math.h>
#include <sys/file.h>
#include <sys/time.h>

#define MAX_NUM_OF_COEFS 100

/* In order for the functions that return a value that is not "int"
 * to work correctly in other files, they must be explicitly declared
 * as external variables (e.g.: "extern float absval()"). */
float absval();
int deriv();
int fderiv();
float maximum();
int truncate();
float decimal();
int between();
double get_time();
int spatial_interpolation();
int open_file();
int create_file();
int close_file();
int wait();
int export_vector_to_file();
int export_1vector_to_file();
int export_2vectors_to_file();
void print_vector();
float fir_filter();


/* Function absval() returns the absolute value of a float, num. */

float absval(num)
float num;
{
  if(num < 0.0)
    return -num;
  else
    return num;
}



/* Function deriv() takes a set of x-y values:  a pointer to a vector of 
 * doubles representing the y-values and a pointer to a vector of doubles
 * representing the x-values.  Additionally, the size of both vectors, size,
 * is also required.  This function takes the derivative of the y-vector
 * with respect to x and fills the vector of doubles provided in the argument
 * list. */

int deriv(y_vector, x_vector, dydx, size)
double y_vector[];
double x_vector[];
double dydx[];
int size;
{
  int i;

  for(i = 0; i < (size-1); i++)
    dydx[i] = (y_vector[i+1] - y_vector[i]) / (x_vector[i+1] - x_vector[i]);
  dydx[size] = dydx[size-1];
}


/* Function fderiv() takes a set of x-y values:  a pointer to a vector of 
 * floats representing the y-values and a pointer to a vector of floats
 * representing the x-values.  Additionally, the size of both vectors, size,
 * is also required.  This function takes the derivative of the y-vector
 * with respect to x and fills the vector of floats provided in the argument
 * list. */

int fderiv(y_vector, x_vector, dydx, size)
float y_vector[];
float x_vector[];
float dydx[];
int size;
{
  int i;

  for(i = 0; i < (size-1); i++)
    dydx[i] = (y_vector[i+1] - y_vector[i]) / (x_vector[i+1] - x_vector[i]);
  dydx[size] = dydx[size-1];
}



/* Function maximum takes two floats as arguments and returns the larger of
 * the two. */

float maximum(num1, num2)
float num1, num2;
{
  if(num1 >= num2)
    return num1;
  else
    return num2;
}



/* Function truncate() takes a float argument and returns its integer
 * value rounded (or truncated) toward the lower of the two integer bounds. */

int truncate(number)
float number;
{
  if(number >= 0)
    return(((int) number));
  else
    return(((int) number) - 1);
}



/* Function decimal() takes a float argument and returns the number
 * containing only the points after the decimal point. */

float decimal(number)
float number;
{
  return(number - (float) truncate(number));
}



/* Function between() takes three float arguments: number, bound1, and
 * bound2.  The function returns a true value (1) if the number is
 * between or equal to either of the bounding values.  The function
 * returns a false value if the number is outside of the number range
 * bounded by bound1 and bound2.  */

int between(number, bound1, bound2)
float number;
float bound1;
float bound2;
{
  if(bound1 <= bound2)
    {
      if((number >= bound1) && (number <= bound2))
	return 1;
      else
	return 0;
    }
  else
    {
      if((number >= bound2) && (number <= bound1))
	return 1;
      else
	return 0;
    }
}
    


/* Function get_time() returns the number of seconds and milliseconds
 * since 00:00:00 000 Jan 1, 1970.  The value of get_time() is a double-
 * precision number, for example: 634526354.570000.  In order to run this 
 * function, the following include statement "#include <sys/time.h>" must
 * be called.  */

double get_time()
{
  int gettimeofday();
  struct timeval tp;
  struct timezone tzp;
  double return_time;

  if ( gettimeofday(&tp,&tzp) == -1 ) {
    fprintf(stderr, "Couldn't get the time of day");
    exit( -1 );
  }

  return_time = (double) (tp.tv_sec) + ((double) (tp.tv_usec))/1E6;
  return(return_time);
}



/* Function spatial_interpolation() takes an input data set and
 * a position data set and calculates an output vector which
 * consists of a linear interpolation of the input data set
 * sampled at constant position intervals as dictated by the
 * position data set.  This function is mainly useful for
 * converting time-sampled data for spatial FFT's.  The seven
 * arguments of this function are:
 *
 * (1) interpolated_data[] - the vector to put the interpolated
 *     output data into, 
 * (2) size1 - the length of the interpolated_data[] vector
 *     (and the number of points to interpolate),
 * (3) input_vector[] - the vector that contains the data to 
 *     be interpolated,
 * (4) reference_position[] - the vector that contains the
 *     position data to use to determine the interpolation step,
 *     and
 * (5) size2 - the length of both the input_vector[] and the
 *     reference_position[] vectors.
 * (6) start_reference - a float number that is the reference
 *     position to start the interpolation from (to interpolate
 *     the data from the beginning of the data set, set this
 *     parameter equal to the first element in the reference
 *     position vector.
 * (7) end_reference - a float number that is the reference
 *     position on which to end the interpolation (to 
 *     interpolate the data to the end of the data set, set this
 *     parameter equal to the last element in the reference
 *     position vector.
 *
 * NOTE: the input_vector[] and reference_position[] should have
 * been sampled with the same servo rate so that the points are
 * separated by the same time step.  
 *
 * NOTE: the reference_position[] vector should have constantly 
 * increasing or decreasing values with respect to time.  If it 
 * does not, the first interpolated position in the vector that 
 * matches a target position will be used to find the associated 
 * output value.  All repeated position points in the interpolated 
 * vector will be bypassed. If the end_reference is not the largest
 * or smallest value in the data set, only the points up to the 
 * first end_reference value that is encountered will be 
 * interpolated.
 *
 * NOTE: the number of points in the interpolated_data[] vector
 * should be greater than or equal to the number of points in the
 * input and the reference position vectors to preserve the resolution
 * of the original data.
 */

spatial_interpolation(interpolated_data, size1, input_vector, reference_position, 
		      size2, start_reference, end_reference)
float interpolated_data[];
float input_vector[];
float reference_position[];
int size1, size2;
float start_reference, end_reference;
{
  int i=0;
  int j;
  int k = 1;
  int start_index, end_index, points_used;
  float start_point;
  float delta_pos;
  float target_pos;
  float reference_interval;
  float data_interval;
  float fraction;

  /* Search through the reference vector to find the start_reference then 
   * interpolate to find the starting input position. */
  while(!between(start_reference, reference_position[i], reference_position[i+1]))
    {
      if(i == (size2-2))
	{
	  fprintf(stderr, "\n ERROR: procedure spatial_interpolation()");
	  fprintf(stderr, "\n Start point not within reference data range.\n");
	  exit(-1);
	}
      i++;
    }
  /* perform the linear interpolation */
  reference_interval = (reference_position[i+1] - reference_position[i]);
  if(reference_interval == 0.0)
    {
      start_point = input_vector[i];
    }
  else
    {
      fraction = ((start_reference - reference_position[i]) / reference_interval);
      data_interval = (input_vector[i+1] - input_vector[i]);
      start_point = (input_vector[i] + (fraction * data_interval));
    }

  /* record the index of the start point */
  if(start_reference == reference_position[i+1])
    start_index = i+1;
  else
    start_index = i;

  i = 0;
  /* Search through the reference vector to find the end_reference. */
  while(!between(end_reference, reference_position[i], reference_position[i+1]))
    {
      if(i == (size2-2))
	{
	  fprintf(stderr, "\n ERROR: procedure spatial_interpolation()");
	  fprintf(stderr, "\n End point not within reference data range.\n");
	  exit(-1);
	}
      i++;
    }

  /* record the index of the end point */
  end_index = i+1;

  /* calculate the actual number of input data points used in the algorithm */
  points_used = end_index - start_index;

  if(size1 < points_used)
    {
      printf("\n WARNING! The number of points to interpolate is");
      printf("\n less than the number input data points used, and");
      printf("\n therefore your data resolution will be lost.\n");
    }
  printf("\n\n Starting the spatial interpolation...\n");
  printf("\n Initial data point: %f", start_point);
  printf("\n There are %d input data points within this range.\n", points_used);

  /* set the value of the position interpolation increment */
  delta_pos = (end_reference - start_reference)/((float) size1);
  
  printf("\n %d points will be interpolated over this range at a", size1);
  printf("\n position increment of %f.\n", delta_pos);

  /* set the target position at which to interpolate a data point */
  target_pos = start_reference + delta_pos;

  /* store the first value in the interpolated_data[] vector */
  interpolated_data[0] = start_point;

  /* now begin the main data processing loop and perform inter-
   * polation as follows: step through the reference input vector
   * in delta_pos steps and calculate the corresponding value 
   * in the input data vector at each target position by linearly 
   * interpolating the two input vectors on the same interval.  
   * If the target position, target_pos, is not within the current
   * reference position intervalexceeds the reference then go to 
   * the next interval of the input vectors. Note that the second 
   * condition in the while loop ensures that the interpolated_data[]
   * vector is not overfilled.  */
  for(j = start_index; j < end_index; j++)
    {
      while(between(target_pos, reference_position[j], reference_position[j+1])
	    && (k < size1))
	{
	  reference_interval = (reference_position[j+1] -
				reference_position[j]);
	  if(reference_interval == 0.0)
	    {
	      interpolated_data[k] = input_vector[j];
	      k = k + 1;
	      target_pos = target_pos + delta_pos;
	    }
	  else
	    {
	      fraction = ((target_pos - reference_position[j]) /
			  reference_interval);
	      data_interval = (input_vector[j+1] - input_vector[j]);
	      interpolated_data[k] = (input_vector[j] +
				      (fraction * data_interval));
	      k = k + 1;
	      target_pos = target_pos + delta_pos;
	    }
	}
    }
}


/*
  for(j = 1; j < size2; j++)
    { 
      while ((absval(reference_position[j]) >= absval(target_pos)) && 
	     (reference_position[j] != reference_position[j-1]) &&
	     (absval(reference_position[size2-1] - (delta_pos/4.0)) > 
	      absval(target_pos)))
	{
	  reference_interval = (reference_position[j] -
				reference_position[j-1]);
	  fraction = ((target_pos - reference_position[j-1]) /
		      reference_interval);
	  data_interval = (input_vector[j] - input_vector[j-1]);
	  interpolated_data[k] = (input_vector[j-1] +
				  (fraction * data_interval));
	  k = k + 1;
	  target_pos = target_pos + delta_pos;
	}
    }
}
*/


/* Function open_file() is a generic file-opening function.  It can
 * be used to open files for both reading and writing.  This function
 * takes three arguments: (1) the address of the variable to store
 * the file pointer, (2) the unix directory pathname for the file, and 
 * (3) the filename.  After checking to make sure the file can be 
 * opened, this function returns the file pointer in the first
 * argument.  */

open_file(addr_of_file_ptr, pathname, filename)
FILE **addr_of_file_ptr;
char pathname[];
char filename[];
{
  char fullname[400];

  sprintf(fullname, "%s%s", pathname, filename);
  printf("\n Opening file %s:\n", fullname);

  if (((*addr_of_file_ptr) = fopen(fullname,"r+")) == NULL)
    {
      fprintf(stderr,"FAILED!!...exiting.\n");
      exit(0);
    }
  printf(" ...successful.\n", fullname);
  return(1);
}



/* Function create_file() is the same as open_file() except that it
 * can only be used for writing, and if the file does not exist, it
 * is created. */

create_file(addr_of_file_ptr, pathname, filename)
FILE **addr_of_file_ptr;
char pathname[];
char filename[];
{
  char fullname[400];

  sprintf(fullname, "%s%s", pathname, filename);
  printf("\n Opening file %s:\n", fullname);

  if (((*addr_of_file_ptr) = fopen(fullname,"w")) == NULL)
    {
      fprintf(stderr,"FAILED!!...exiting.\n");
      exit(0);
    }
  printf(" ...successful.\n", fullname);
  return(1);
}


/* Function close_file() can be used to close any file. */

close_file(file_pointer)
FILE *file_pointer;
{
  if (fclose(file_pointer) != 0)
    printf("\n FAILED to close file!! \n");
  return(1);
}



/* Function wait() takes a single float argument, the number of
 * seconds to wait, and pauses for that length of time (as measured
 * by the clock) before returning control back to the program. */

int wait(seconds)
float(seconds);
{
  double initial_time;

  initial_time = get_time();
  while((get_time() - initial_time) < seconds);

  return(1);
}




/* Function export_vector_to_file() takes a file pointer, a vector
 * of data and the size of the vector and outputs the data to the
 * file, one line per value. */

export_vector_to_file(file_pointer, vector, size)
FILE *file_pointer;
float vector[];
int size;
{
  int i;

  printf("\n Exporting data to file...\n");
  for (i = 0; i < size; i++)
    {
      fprintf(file_pointer, "%f\n", vector[i]);

      if(i % 100 == 0) 
	{
	  printf(". ");
	  fflush(stdout);
	}
    }
}





/* Function export_1vector_to_file() takes a file pointer, a vector
 * of data and the size of the vector and outputs the data to the
 * file, one line per value.  Note that each line is terminated by
 * a semi-colon - a format used by MatrixX.  */

export_1vector_to_file(file_pointer, vector, size)
FILE *file_pointer;
float vector[];
int size;
{
  int i;

  printf("\n Exporting data to file...\n");
  for (i = 0; i < size; i++)
    {
      fprintf(file_pointer, "%f;\n", vector[i]);

      if(i % 100 == 0) 
	{
	  printf(". ");
	  fflush(stdout);
	}
    }
}



/* Function export_2vectors_to_file() takes a file pointer, two
 * vectors and the size of the vectors and outputs the data in 
 * both vectors to a file.  The data is formatted with one pair
 * per line, the vector1 value first and the corresponding vector2 
 * value second. This format is suitable for Xgraph and many
 * other plotting utilities. */

export_2vectors_to_file( file_pointer, vector1, vector2, size)
FILE *file_pointer;
float vector1[];
float vector2[];
int size;
{
  int i;

  printf("\n Exporting data to file...\n");
  for (i = 0; i < size; i++)
    {
      fprintf(file_pointer, "%10.6f %10.6f\n", vector1[i], vector2[i]);

      if(i % 100 == 0) 
	{
	  printf(". ");
	  fflush(stdout);
	}
    }
}



/* Function print_vector() prints out the given vector
 * on the screen.  */

void print_vector(vector, rows)
float *vector;
int rows;
{
  int i;
  
  printf("\n\n");
  
  for (i = 0; i <= rows; i++)
    {
      printf("%f\n", vector[i]);
    }
  printf("\n");
}






/* Function fir_filter() can be called within a servo_loop or any
 * processing loop which proceeds through the data points in a
 * data vector.  It is used to remove certain frequency components
 * from the incoming signal by using a digital linear non-recursive
 * filtering technique (i.e. fir = finite impulse response).  This
 * technique creates each filtered data point by summing each incoming
 * data point with a number of previous data points (specified by
 * num_of_coefs) multiplied by a corresponding coefficient stored in
 * the vector coefs[].  This vector of coefs[] can be generated using
 * the function create_low_pass_filter(), above.  The three coefficients
 * required by this function are: (1) the current float data point
 * being processed in the data loop, (2) the pointer to the vector of
 * floats containing the coefficients to multiply the previous data
 * points by, and (3) the number of terms in the digital filter
 * equation or the number of coefficients in the coefs[] vector.
 * For an example of how to use this function, see the file: 
 * test_filter.c.  
 * NOTE: to use this function more than once in a program, the function
 *       initialize_fir_filter() must be used.  */

/* First define the global variables that are used by this function and
 * initialize them appropriately.  Notice that the vector which is used
 * to store the old data is only initialized for MAX_NUM_OF_COEFS memory
 * locations...be sure not to exceed this number of terms in the digital
 * filter.  The old_data[] vector is initialized to zero by default. */
static int filter_count = 0;
float filtered_point = 0.0;
static float old_data[MAX_NUM_OF_COEFS];

float fir_filter(data_point, coefs, num_of_coefs)
float data_point;
float coefs[];
int num_of_coefs;
{
  int i;
  int local_count;

  filtered_point = 0.0;
  local_count = (filter_count % num_of_coefs);
  old_data[local_count] = data_point;
  
  for(i = 0; i < num_of_coefs; i++)
    filtered_point = filtered_point + (coefs[i] * 
				       old_data[(local_count + i) % num_of_coefs]);

  filter_count++;
  return(filtered_point);
}




/* Function initialize_fir_filter() should be called before any
 * applications of the function fir_filter() after the first
 * one. */

int initialize_fir_filter()
{
  int i;

  filter_count = 0;
  
  for(i = 0; i < MAX_NUM_OF_COEFS; i++)
    old_data[i] = 0.0;
}