#include <string.h>
#include <stdlib.h>
#include <memory.h>
#include <stdio.h>
#include <math.h>
#include <tcl.h>

/* ----------------------------------------------------------------- */
/*  Peak detection using undecimated wavelet transform (UWT)         */
/* ----------------------------------------------------------------- */

/* Memory requirements */
/* tapped delay line: 24 registers */
/* noise buffer: 16 registers */

#define UWT_ZERO 1000
#define UWT_LEVEL 3

#define UWT_NOISE_BUFFER_SIZE 16
#define UWT_NOISE_BUFFER_SIZE_LOG2 4

#define UWT_TYPICAL_START_TIME 16
#define UWT_TYPICAL_NOISE_TIME 16
#define UWT_MAX_RISE_TIME 40
#define UWT_MIN_PEAK_DIST 60

#define UWT_SIGNAL_THRESHOLD 10

static void uwt_bior31_step(int input, int *tap,
  int *d_reg, int *a_reg, int *peak_reg, int *tmp1_reg, int *tmp2_reg, int *flag1_reg, int *flag2_reg, int *less_reg, int *more_reg,
  int *d, int *a, int *peak, int *is_min, int *is_max, int level)
{
    int i;
  	int	index1		  =	1 << (level - 1);
  	int	index2		  =	2 << (level - 1);
  	int	index3		  =	3 << (level - 1);
  	int	peak_index	=	((3 << (level - 1)) + 1) >> 1;
  	int	peak_shift	=	((level - 1) << 1) + (level - 1);

    int d_next, a_next, peak_next;
    int tmp1_next, tmp2_next, less_next, more_next;


    // Tapped delay line: shift one
    for(i = index3; i > 0; --i)
    {
        tap[i] = tap[i-1];
    }

    // Input in register 0
    tap[0] = input;

      *d		=	*d_reg;
      *a		=	*a_reg;
      *peak	=	*peak_reg;
      *is_min	=	*flag2_reg;
      *is_max	=	*flag1_reg;

  		tmp1_next = tap[index3] + (tap[index2] << 1) + tap[index2];
  		tmp2_next = (tap[index1] << 1) + tap[index1] + tap[0];

  		d_next = UWT_ZERO - *tmp1_reg + *tmp2_reg;
  		a_next = *tmp1_reg + *tmp2_reg;

  		more_next = (*d_reg > UWT_ZERO);
  		less_next = (*d_reg < UWT_ZERO);

  		peak_next = (tap[peak_index + 1] >> peak_shift);

			*flag2_reg = (*more_reg) && (!more_next);
			*flag1_reg = (*less_reg) && (!less_next);

     	*d_reg = d_next;
			*a_reg = a_next;
			*peak_reg = peak_next;

			*tmp1_reg = tmp1_next;
			*tmp2_reg = tmp2_next;
			*less_reg = less_next;
			*more_reg = more_next;


/*
    *d = UWT_ZERO - (tap[index3])
       - (tap[index2] << 1) - tap[index2]
       + (tap[index1] << 1) + tap[index1]
       + (tap[0]);

    *a = (tap[index3])
       + (tap[index2] << 1) + tap[index2]
       + (tap[index1] << 1) + tap[index1]
       + (tap[0]);

    *peak = (tap[peak_index] >> peak_shift);

    *is_min = (*d_reg < UWT_ZERO) && (*d >= UWT_ZERO) ? 1 : 0;
    *is_max = (*d_reg > UWT_ZERO) && (*d <= UWT_ZERO) ? 1 : 0;
*/
/*
    if(((*is_min) || (*is_max)) && (level == 3)) printf("peak -> %d -> %d -> %d\n", (*peak), (*d_reg), (*d));
*/
/*
    *d_reg = *d;
*/
}

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

static void uwt_process(int *input, int *noise, int *peak, int *is_min, int *is_max, int *is_valid, int n)
{
    int i, j, k;
    int sample;

    int mode = 0;

    int time_from_start = 0;
    int time_from_min = 0;
    int time_from_max = 0;

    int noise_average = 0;

    int noise_buffer[UWT_NOISE_BUFFER_SIZE];

    int pulse_height = 0;

    int size;
  	int d_reg[UWT_LEVEL];
  	int a_reg[UWT_LEVEL], peak_reg[UWT_LEVEL];
    int tmp1_reg[UWT_LEVEL], tmp2_reg[UWT_LEVEL];
    int flag1_reg[UWT_LEVEL], flag2_reg[UWT_LEVEL];
    int less_reg[UWT_LEVEL], more_reg[UWT_LEVEL];
  	int a, d;
  	int *tap[UWT_LEVEL];

    for(j = 0; j < UWT_NOISE_BUFFER_SIZE; ++j)
    {
        noise_buffer[j] = 0;
    }

  	// Tapped delay line
    for(j = 0; j < UWT_LEVEL; ++j)
    {
        size = ((3 << j) + 1)*sizeof(int);
        printf("%d -> %d\n", j+1, ((3 << j) + 1));
        fflush(stdout);
       	tap[j] = (int *) malloc(size);
      	memset(tap[j], 0, size);
      	d_reg[j] = UWT_ZERO;
      	a_reg[j] = UWT_ZERO;
      	peak_reg[j] = UWT_ZERO;
      	tmp1_reg[j] = UWT_ZERO;
      	tmp2_reg[j] = UWT_ZERO;
      	flag1_reg[j] = UWT_ZERO;
      	flag2_reg[j] = UWT_ZERO;
      	less_reg[j] = UWT_ZERO;
      	more_reg[j] = UWT_ZERO;
    }

    for(i = 0; i < n; ++i)
    {
        sample = input[i];
        is_valid[i] = 0;

        for(j = 0; j < UWT_LEVEL; ++j)
        {
            uwt_bior31_step(sample, tap[j],
              d_reg+j, a_reg+j, peak_reg+j, tmp1_reg+j, tmp2_reg+j, flag1_reg+j, flag2_reg+j, less_reg+j, more_reg+j,
              &d, &a, peak+i, is_min+i, is_max+i, j+1);
            sample = a;
        }
        
        if(is_min[i] && mode > 0)
        {
            for(j = 0; j < UWT_NOISE_BUFFER_SIZE - 1; ++j)
            {
                noise_buffer[j] = noise_buffer[j+1] + peak[i];
            }
            noise_buffer[UWT_NOISE_BUFFER_SIZE-1] = peak[i];

            noise_average = noise_buffer[0] >> UWT_NOISE_BUFFER_SIZE_LOG2;

            time_from_min = 0;
        }

        noise[i] = noise_average;

        switch(mode)
        {
            case 0:
                ++time_from_start;

                if(time_from_start >= UWT_TYPICAL_START_TIME)
                {
                    mode = 1;
                    time_from_start = 0;
                }
                break;

            case 1:
                if(is_min[i]) ++time_from_start;

                if(time_from_start >= UWT_TYPICAL_NOISE_TIME)
                {
                    mode = 2;
                    time_from_start = 0;
                }
                break;

            case 2:
                if(is_max[i])
                {
                    pulse_height = peak[i] - noise_average;

                    if(pulse_height > UWT_SIGNAL_THRESHOLD)
                    {
                        printf("%d -> %d -> %d\n", pulse_height, time_from_min, time_from_max);
                        if(time_from_min < UWT_MAX_RISE_TIME &&
                           time_from_max > UWT_MIN_PEAK_DIST
                           )
                        {
                            is_valid[i] = 1;
                        }
                        time_from_max = 0;
                    }
                }

                ++time_from_min;
                ++time_from_max;

                break;

             default:
                /* Do nothing */
                break;
        }
    }

    for(j = 0; j < UWT_LEVEL; ++j)
    {   
        free(tap[j]);
    }

}

/* ----------------------------------------------------------------- */
/*  Peak detection using "snake" algorithm                           */
/* ----------------------------------------------------------------- */

/* Memory requirements */
/* noise buffer: 32 registers */
/* peak buffer: 7 registers */

static void sum8(int *input, int *output, int n)
{
    int i, j;

  	// Tapped delay line
  	int *tap = (int *) malloc(8*sizeof(int));
  	memset(tap, 0, 8*sizeof(int));

    for(i = 0; i < n; ++i)
    {
        // Tapped delay line: shift one
  			for(j = 7; j > 0; --j)
  			{
  				tap[j] = tap[j-1];
  			}

  			// Input in register 0
  			tap[0] = input[i];

        output[i] = 0;

        for(j = 0; j < 8; ++j)
        {
          output[i] += tap[j];
        }

        output[i] = output[i] >> 3;

    }

    free(tap);
}

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

#define PEAK_BUFFER_SIZE 7
#define NOISE_BUFFER_SIZE 32
#define NOISE_BUFFER_SIZE_LOG2 5

#define TYPICAL_START_TIME 64
#define TYPICAL_RISE_TIME 12
#define TYPICAL_PILEUP_TIME 20

#define SIGNAL_THRESHOLD 10

static void csr_process(int *input, int *noise, int *peak, int *is_max, int n)
{
    int i, j, k;
    int sample;


    int mode = 0;
    int min_sample = 4095;

    int time_from_start = 0;
    int time_from_min = 0;
    int time_from_peak = 0;

    int noise_average = 0;
    int min_noise_average = 4095;

    int noise_buffer[NOISE_BUFFER_SIZE];
    int peak_buffer[PEAK_BUFFER_SIZE];

    int peak_value = 0;
    int pulse_height = 0;
    
    int is_raising, is_stable, is_stored, is_valid, is_pileup;

    for(j = 0; j < NOISE_BUFFER_SIZE; ++j)
    {
        noise_buffer[j] = 4095;
    }

    for(i = 0; i < n; ++i)
    {
        sample = input[i];
        noise[i] = 0;
        peak[i] = 0;
        is_max[i] = 0;

        if(mode == 0 || mode == 1)
        {
            for(j = 0; j < NOISE_BUFFER_SIZE - 1; ++j)
            {
                noise_buffer[j] = noise_buffer[j+1];
            }
            noise_buffer[NOISE_BUFFER_SIZE-1] = sample;

            noise_average = 0;
            for(j = 0; j < NOISE_BUFFER_SIZE; ++j)
            {
                noise_average += noise_buffer[j];
            }
            noise_average = noise_average >> NOISE_BUFFER_SIZE_LOG2;
        }

        noise[i] = noise_average;

        for(j = 0; j < PEAK_BUFFER_SIZE - 1; ++j)
        {
            peak_buffer[j] = peak_buffer[j+1];
        }
        peak_buffer[PEAK_BUFFER_SIZE-1] = sample;

        switch(mode)
        {
            case 0:
                if(noise_average < min_noise_average)
                {
                    min_noise_average = noise_average;
                }

                ++time_from_start;

                if(time_from_start >= TYPICAL_START_TIME)
                {
                    mode = 1;
                    noise_average = min_noise_average;
                    for(j = 0; j < NOISE_BUFFER_SIZE; ++j)
                    {
                        noise_buffer[j] = min_noise_average;
                    }
                }
                break;

            case 1:
                is_raising = 0;
                is_stable = 0;
                if(sample >= min_sample)
                {
                    is_stable = 1;
                }
                else
                {
                    min_sample = sample;
                }
                if(peak_buffer[0] < peak_buffer[3]) is_raising = 1;
                if(is_stable && is_raising)
                {
                   ++time_from_min;
                }
                else
                {
                   time_from_min = 0;
                   if(!is_raising) min_sample = peak_buffer[3];
                }
                if(time_from_min >= TYPICAL_RISE_TIME)
                {
                   mode = 2;
                   is_stored = 0;
                   is_valid = 0;
                }
                break;

            case 2:
                if(!is_stored)
                {
                    if(peak_buffer[3] > peak_buffer[0] &&
                       peak_buffer[3] > peak_buffer[6] &&
                       peak_buffer[3] >= peak_buffer[2] &&
                       peak_buffer[3] >= peak_buffer[4])
                    {
                        peak_value = peak_buffer[3];
                        time_from_peak = 3;
                        pulse_height = peak_value - noise_average;
                        if(pulse_height > SIGNAL_THRESHOLD)
                        {
                           is_valid = 1;
                           peak[i] = peak_value;
                           is_max[i] = 1;
                           is_stored = 1;
                        }
                        mode = 3;
                    }
                }
                ++time_from_min;
                is_pileup = 0;
                break;

            case 3:
                ++time_from_min;
                ++time_from_peak;
                if(time_from_min > TYPICAL_PILEUP_TIME ||
                   time_from_peak > TYPICAL_PILEUP_TIME)
                {
                    is_pileup = 1;
                }
                if(peak_buffer[3] < peak_buffer[0] &&
                   peak_buffer[3] < peak_buffer[6] &&
                   peak_buffer[3] <= peak_buffer[2] &&
                   peak_buffer[3] <= peak_buffer[4])
                {
                    min_sample = peak_buffer[3];
                    mode = 1;
                }
                break;

            default:
                /* Do nothing */
                break;
        }
    }
}

/* ----------------------------------------------------------------- */
/* Interface with Tcl                                                */
/* ----------------------------------------------------------------- */

static int UwtProcessObjCmdProc(ClientData clientData, Tcl_Interp *interp,
                                int objc, Tcl_Obj *CONST objv[])
{
  int size = 0;
  int i = 0;
  int level = 0;
  double value = 0.0;
  int *input, *noise, *peak, *is_min, *is_max, *is_valid;
  Tcl_Obj *result = NULL;
  Tcl_Obj *listnoise = NULL;
  Tcl_Obj *listpeak = NULL;
  Tcl_Obj *listmin = NULL;
  Tcl_Obj *listmax = NULL;
  Tcl_Obj *listvalid = NULL;

  if(objc != 2)
  {
    Tcl_WrongNumArgs(interp, 1, objv, "data");
		return TCL_ERROR;
  }

  if(TCL_OK != Tcl_ListObjLength(interp, objv[1], &size))
  {
    return TCL_ERROR;
  }

  input = (int *) malloc(size*sizeof(int));
  noise = (int *) malloc(size*sizeof(int));
  peak = (int *) malloc(size*sizeof(int));
  is_min = (int *) malloc(size*sizeof(int));
  is_max = (int *) malloc(size*sizeof(int));
  is_valid = (int *) malloc(size*sizeof(int));

  for(i = 0; i < size; ++i)
  {
    Tcl_ListObjIndex(interp, objv[1], i, &result);
    if(TCL_OK != Tcl_GetDoubleFromObj(interp, result, &value))
    {
      free(input);
      free(noise);
      free(peak);
      free(is_min);
      free(is_max);
      free(is_valid);
      return TCL_ERROR;
    }
    input[i] = (int) value;
/*
    printf("%d -> %g\n", i, input[i]);
*/
  }

  uwt_process(input, noise, peak, is_min, is_max, is_valid, size);

  result = Tcl_GetObjResult(interp);
  listnoise = Tcl_NewObj();
  listpeak = Tcl_NewObj();
  listmin = Tcl_NewObj();
  listmax = Tcl_NewObj();
  listvalid = Tcl_NewObj();
  for(i = 0; i < size; ++i)
  {
    Tcl_ListObjAppendElement(interp, listnoise, Tcl_NewIntObj(noise[i]));
    Tcl_ListObjAppendElement(interp, listpeak, Tcl_NewIntObj(peak[i]));
    Tcl_ListObjAppendElement(interp, listmin, Tcl_NewIntObj(is_min[i]*peak[i]));
    Tcl_ListObjAppendElement(interp, listmax, Tcl_NewIntObj(is_max[i]*peak[i]));
    Tcl_ListObjAppendElement(interp, listvalid, Tcl_NewIntObj(is_valid[i]*peak[i]));
  }

  Tcl_ListObjAppendElement(interp, result, listnoise);
  Tcl_ListObjAppendElement(interp, result, listpeak);
  Tcl_ListObjAppendElement(interp, result, listmin);
  Tcl_ListObjAppendElement(interp, result, listmax);
  Tcl_ListObjAppendElement(interp, result, listvalid);

  free(input);
  free(noise);
  free(peak);
  free(is_min);
  free(is_max);
  free(is_valid);

  return TCL_OK;
}

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

static int Sum8ObjCmdProc(ClientData clientData, Tcl_Interp *interp,
                          int objc, Tcl_Obj *CONST objv[])
{
  int size = 0;
  int i = 0;
  int level = 0;
  double value = 0.0;
  int *input, *output;
  Tcl_Obj *result = NULL;

  if(objc != 2)
  {
    Tcl_WrongNumArgs(interp, 1, objv, "data");
		return TCL_ERROR;
  }

  if(TCL_OK != Tcl_ListObjLength(interp, objv[1], &size))
  {
    return TCL_ERROR;
  }

  input = (int *) malloc(size*sizeof(int));
  output = (int *) malloc(size*sizeof(int));

  for(i = 0; i < size; ++i)
  {
    Tcl_ListObjIndex(interp, objv[1], i, &result);
    if(TCL_OK != Tcl_GetDoubleFromObj(interp, result, &value))
    {
      free(input);
      free(output);
      return TCL_ERROR;
    }
    input[i] = (int) value;
/*
    printf("%d -> %g\n", i, input[i]);
*/
  }

  sum8(input, output, size);

  result = Tcl_GetObjResult(interp);
  for(i = 0; i < size; ++i)
  {
    Tcl_ListObjAppendElement(interp, result, Tcl_NewIntObj(output[i]));
/*
    printf("%d -> %g\n", i, d[i]);
*/
	}

  free(input);
  free(output);

  return TCL_OK;
}

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

static int CsrProcessObjCmdProc(ClientData clientData, Tcl_Interp *interp,
                                int objc, Tcl_Obj *CONST objv[])
{
  int size = 0;
  int i = 0;
  int level = 0;
  double value = 0.0;
  int *input, *noise, *peak, *is_max;
  Tcl_Obj *result = NULL;
  Tcl_Obj *listnoise = NULL;
  Tcl_Obj *listpeak = NULL;
  Tcl_Obj *listmax = NULL;

  if(objc != 2)
  {
    Tcl_WrongNumArgs(interp, 1, objv, "data");
		return TCL_ERROR;
  }

  if(TCL_OK != Tcl_ListObjLength(interp, objv[1], &size))
  {
    return TCL_ERROR;
  }

  input = (int *) malloc(size*sizeof(int));
  noise = (int *) malloc(size*sizeof(int));
  peak = (int *) malloc(size*sizeof(int));
  is_max = (int *) malloc(size*sizeof(int));

  for(i = 0; i < size; ++i)
  {
    Tcl_ListObjIndex(interp, objv[1], i, &result);
    if(TCL_OK != Tcl_GetDoubleFromObj(interp, result, &value))
    {
      free(input);
      free(noise);
      free(peak);
      free(is_max);
      return TCL_ERROR;
    }
    input[i] = (int) value;
/*
    printf("%d -> %g\n", i, input[i]);
*/
  }

  csr_process(input, noise, peak, is_max, size);

  result = Tcl_GetObjResult(interp);
  listnoise = Tcl_NewObj();
  listpeak = Tcl_NewObj();
  listmax = Tcl_NewObj();
  for(i = 0; i < size; ++i)
  {
    Tcl_ListObjAppendElement(interp, listnoise, Tcl_NewIntObj(noise[i]));
    Tcl_ListObjAppendElement(interp, listpeak, Tcl_NewIntObj(peak[i]));
    Tcl_ListObjAppendElement(interp, listmax, Tcl_NewIntObj(is_max[i]));
  }

  Tcl_ListObjAppendElement(interp, result, listnoise);
  Tcl_ListObjAppendElement(interp, result, listpeak);
  Tcl_ListObjAppendElement(interp, result, listmax);

  free(input);
  free(noise);
  free(peak);
  free(is_max);

  return TCL_OK;
}

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

int Csr_Init(Tcl_Interp *interp)
{
    Tcl_CreateObjCommand(interp, "csr_uwt_process", UwtProcessObjCmdProc, 0, 0);
    Tcl_CreateObjCommand(interp, "csr_sum8", Sum8ObjCmdProc, 0, 0);
    Tcl_CreateObjCommand(interp, "csr_process", CsrProcessObjCmdProc, 0, 0);

    return Tcl_PkgProvide(interp, "csr", "0.1");
}