/*
 Subroutines for data acquisition and processing
 
 Copyright (c) 2008 Dimtel, Inc., All Rights Reserved

 $Id: subDataAcq.c,v 1.2 2012/11/11 05:48:57 dim Exp $
 $Date: 2012/11/11 05:48:57 $
 $Author: dim $
 $Revision: 1.2 $
 $Log: subDataAcq.c,v $
 Revision 1.2  2012/11/11 05:48:57  dim
 Updates in 2009-2012 range

 Revision 1.1.1.1  2009/06/02 20:25:59  dim
 Initial import


*/

#include <dbDefs.h>
#include <registryFunction.h>
#include <subRecord.h>
#include <aSubRecord.h>
#include <epicsExport.h>
#include <dbAccess.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <sys/time.h>

/* FFT code definitions */
#include <complex.h>
#include <fftw3.h>

/* Local stuff */
#include "LLRF.h"
#include "cmath.h"
#include "spf.h"
#include "dacq.h"

extern acq_data snapshot;
extern pthread_mutex_t snapshot_mutex;

extern int USB_Debug;

proc_data vectors;

typedef long (*processMethod)(subRecord *precord);

static long subDataAcqInit(subRecord *psub, processMethod process)
{
  psub->val = 0;
  return 0;
}

void *DataProcThread(void *ptr)
{
  int k, m, diff;
  double tmp[ADC_LENGTH];

  /* FFTW structures */
  fftw_complex *fftout;
  fftw_plan p;
  
  /* Timing structures */
  struct timeval t1, t2;

  while (1)
  {
    /* Lock the data structure before processing */
    pthread_mutex_lock(&vectors.data_mutex);
    
    if (vectors.trigger != TRIG_NEW) /* No new data */
    {
      pthread_mutex_unlock(&vectors.data_mutex);
      usleep(T_PROC_WAIT); /* Sleep */
    }
    else
    {
      if (USB_Debug == 1)
        gettimeofday(&t1, NULL);

      if (vectors.first)
      {
        /* FFT output/input */
        fftout  = fftw_malloc(sizeof(fftw_complex)*(ADC_LENGTH/2 + 1));
        /* FFT computation - use tmp as input data vector */
        p = fftw_plan_dft_r2c_1d(ADC_LENGTH, tmp, fftout, FFTW_ESTIMATE);
        vectors.first = 0;
      }

      for (k = 0; k < 4; k++)
      {
        /* Fill in the input data */
        for (m = 0; m < ADC_LENGTH; m++)
          tmp[m] = (double)vectors.adc[k][m];
          
        /* Transform it */    
        fftw_execute(p);
    
        /* Save squares of absolute values in mag */
        for (m = 0; m <= ADC_LENGTH/2; m++)
            vectors.mag[k][m] = creal_loc(fftout[m]*
                                conj_loc(fftout[m]))/ADC_LENGTH;
      }
  
      /* Extract square root of mag and scale, define frequency vector */
      for (k = 0; k < 4; k++)
      {
        for (m = 0; m <= ADC_LENGTH/2; m++)
          vectors.mag[k][m] = 10.0*log10(vectors.mag[k][m]+1e-10);
        vectors.mag[k][0] = vectors.mag[k][1];
      }

      /* Do the averaging */
      if (!vectors.scale_update)
        for (k = 0; k < 4; k++)
          for (m = 0; m <= ADC_LENGTH/2; m++)
            vectors.mag[k][m] = vectors.alpha*vectors.mag_old[k][m] +
                                (1.0 - vectors.alpha)*vectors.mag[k][m];
                             
      for (k = 0; k < 4; k++)                              
        for (m = 0; m <= ADC_LENGTH/2; m++)
          vectors.mag_old[k][m] = vectors.mag[k][m];
      
      if (vectors.scale_update)
      {
        for (m = 0; m <= ADC_LENGTH/2; m++)
          vectors.freq[m] = (float)m/vectors.us_per_sample/ADC_LENGTH;
        for (m = 0; m < ADC_LENGTH; m++)
          vectors.tsc[m] = (float)(m * vectors.us_per_sample);

        dbPutLink(vectors.db_tsc, DBR_FLOAT, vectors.tsc, ADC_LENGTH);
        dbPutLink(vectors.db_freq, DBR_FLOAT, vectors.freq, ADC_LENGTH/2+1);
        vectors.scale_update = 0;
      }

      if (USB_Debug == 1)
      {
        gettimeofday(&t2, NULL);
        diff = deltaT(&t1, &t2);
        printf("Elapsed time (DataProcThread): %.3f ms\n", ((double)diff)/1000.0);
      }

      for (k = 0; k < 4; k++)
      {
        dbPutLink(vectors.db_adc[k], DBR_SHORT,
                  (short *)vectors.adc+k*ADC_LENGTH, ADC_LENGTH);
        dbPutLink(vectors.db_mag[k], DBR_DOUBLE,
                  (double *)vectors.mag+k*(ADC_LENGTH/2+1), ADC_LENGTH/2+1);
      }

      vectors.trigger = 0;
      /* Unlock the data structure. subDataAcqbeamproc will get
         the mutex, copy out the results and reload the new data,
         if available  */
      pthread_mutex_unlock(&vectors.data_mutex);
    }
  }
  return NULL;
}

static long subDataAcqProc(subRecord *psub)
{
  /*
   * Inputs:  A = Sampling frequency
              B = Averaging factor
   * Outputs:
   *          C = ADC0 data
   *          D = ADC1 data
   *          E = ADC2 data
   *          F = ADC3 data
   *          G = time vector
   *          H = ADC0 spectrum
   *          I = ADC1 spectrum
   *          J = ADC2 spectrum
   *          K = ADC3 spectrum
   *          L = frequency vector
   *          
   */
  
  int k, m;
  static int first = 1;
  static double fsamp;
  static pthread_t proc_thread;

  if (USB_Debug == 2)
  {
        printf("Record %s called subDataAcqbeamproc(%p)\n",
               psub->name, (void*) psub);
  }

  if (first)
  {
    /* First-time init */
    pthread_mutex_init(&vectors.data_mutex, NULL);
    vectors.trigger = 0;
    vectors.us_per_sample = 1/psub->a;
    fsamp = psub->a;
    vectors.scale_update = 1;
    vectors.first = 1;
    first = 0;
    
    /* Create the processing thread */
    pthread_create(&proc_thread, NULL, DataProcThread, NULL);
  }
  
  /* Try to get the lock */
  if (pthread_mutex_trylock(&vectors.data_mutex) == 0)
  {
    if (pthread_mutex_trylock(&snapshot_mutex) == 0)
    {
      if (snapshot.new != 0) /* New data is available */
      {
        for (k = 0; k < ADC_LENGTH; k++)
          for (m=0; m < 4; m++)
            vectors.adc[m][k] = snapshot.adc[m][k];
        vectors.alpha = 1.0 - 1.0/psub->b;
        vectors.trigger = TRIG_NEW;
        if (fsamp != psub->a)
        {
          vectors.us_per_sample = 1/psub->a;
          fsamp                 = psub->a;
          vectors.scale_update  = 1;
        }
        
        /* DB links */
        vectors.db_adc[0] = &psub->inpc;
        vectors.db_adc[1] = &psub->inpd;
        vectors.db_adc[2] = &psub->inpe;
        vectors.db_adc[3] = &psub->inpf;
        vectors.db_tsc    = &psub->inpg;
        vectors.db_mag[0] = &psub->inph;
        vectors.db_mag[1] = &psub->inpi;
        vectors.db_mag[2] = &psub->inpj;
        vectors.db_mag[3] = &psub->inpk;
        vectors.db_freq   = &psub->inpl;
        snapshot.new = 0;
      }
      pthread_mutex_unlock(&snapshot_mutex);
    }    
    /* Unlock the mutex. If data is available processing will start
       within T_PROC_WAIT microseconds */
    pthread_mutex_unlock(&vectors.data_mutex);
  }
  
  return 0;
}

static long subDataAcqbeamstat(aSubRecord *pgsub)
{
  /*
   * Inputs:  A = mean vector
   *          B = rms vector
   *          C = frequency vector
   *          D = spectrum vector
   *          E = Spectral search lower frequency (kHz)
   *          H = Spectral search upper frequency (kHz)
   *          I = Spectral search flag (0 - min, 1 - max)
   *          F = Harmonic number
   *          G = Number of samples
   * Outputs: A = mean around the turn
   *          B = peak-to-peak mean
   *          C = RMS around the turn
   *          D = max RMS
   *          E = Marker amplitude (dB)
   *          F = Marker frequency (kHz)
   *          G = Marker amplitude (vector of 2)
   *          H = Marker frequency (vector of 2)
   */
  
  int j, k, m, flag=0;
  long turns, frlen;
  double mean_max, mean_min, center, base, marker_amp[2], marker_freq[2];
  double amp_pp, peak;
  float *rms, *mean, *freq, *spec;
  static long samples_per_turn = 0;

  if (USB_Debug == 2)
        printf("Record %s called subDataAcqbeamstat(%p)\n",
               pgsub->name, (void*) pgsub);

  if (samples_per_turn == 0)
  {
    if (*(long *)pgsub->f == 0)
      return 0;
    else
      samples_per_turn = *(long *)pgsub->f;
  }

  turns = *(long *)pgsub->g/samples_per_turn;
  frlen = turns/2 + 1;
  mean = (float *) pgsub->a;
  rms  = (float *) pgsub->b;
  freq = (float *) pgsub->c;
  spec = (float *) pgsub->d;
  
  /* Compute average mean, put it in E */
  spf_mean_f(mean, (double *)pgsub->vala, 1, samples_per_turn);
  /* Compute RMS, put it in G */
  spf_rms_f(rms, (double *)pgsub->valc, 1, samples_per_turn);
  /* Compute max RMS, put it in H */
  spf_max_f(rms, (double *)pgsub->vald, samples_per_turn);

  /* Find max and min of the mean, separate along the centerline */
  spf_max_f(mean, &mean_max, samples_per_turn);
  spf_min_f(mean, &mean_min, samples_per_turn);
  center = (mean_max + mean_min) / 2;
  
  base = peak = 0.0;
  j = m = 0;
  for (k = 0; k < samples_per_turn; k ++)
    if (mean[k] > center)
    {
      peak += mean[k];
      j++;
    }
    else
    {
      base += mean[k];
      m++;
    }
  if (j != 0)
  {
    peak /= j;
    base /= m;
    /* Peak-to-peak amplitude (average orbit) */
    amp_pp = peak - base;
  }
  else
    amp_pp = 0.0;
    
  *(double *)pgsub->valb = amp_pp;
  
  /* Marker search */
  for (k = frlen - 1; k >= 0; k--)
    if (freq[k] <= *(double *)pgsub->e)
      break;
    else if (freq[k] <= *(double *)pgsub->h)
    {
      if (flag == 0)
      {
        marker_amp[0] = spec[k];
        marker_freq[0] = freq[k];
        flag = 1;
      }
      else if (*(char *)pgsub->i == 1)
      {
        if (spec[k] > marker_amp[0])
        {
          marker_amp[0] = spec[k];
          marker_freq[0] = freq[k];
        }
      }
      else
        if (spec[k] < marker_amp[0])
        {
          marker_amp[0] = spec[k];
          marker_freq[0] = freq[k];
        }
    }
  *(double *)pgsub->vale = marker_amp[0];
  *(double *)pgsub->valf = marker_freq[0];
  
  marker_amp[1] = marker_amp[0]; marker_freq[1] = marker_freq[0];
  memcpy(pgsub->valg, marker_amp, 2*sizeof(double));
  memcpy(pgsub->valh, marker_freq, 2*sizeof(double));

  return 0;
}


static registryFunctionRef subDataAcqRef[] = {
  {"subDataAcqInit", (REGISTRYFUNCTION)subDataAcqInit},
  {"subDataAcqProc", (REGISTRYFUNCTION)subDataAcqProc},
  {"subDataAcqbeamstat", (REGISTRYFUNCTION)subDataAcqbeamstat}
};


static void subDataAcqRegistrar(void)
{
  registryFunctionRefAdd(subDataAcqRef, NELEMENTS(subDataAcqRef));
}
epicsExportRegistrar(subDataAcqRegistrar);
