/*
 Pseudo-random binary sequences (PRBS) are sequences of rectangular pulses, modulated 
 in width, which approximate a discrete time white noise and thus have a spectral
 content rich in frequency.
 
 Sec 5.3.2 of the Book: 
 Ioan D. Landau and Gianluca Zito, 
 Digital Control Systems: Design, Identification and Implementation, Springer, 2006.
 
* Author: Xue-Cheng Xi, 
* Email:	xixuecheng at gmail dot com
* Date:	Sep 28, 2007

* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2007 All rights reserved.
*
********************************************************************/


#ifndef RTAPI
#error This is a realtime component only!
#endif

#include <rtapi.h>		/* RTAPI realtime OS API */
#include <rtapi_app.h>		/* RTAPI realtime module decls */
#include <hal.h>		/* HAL public API decls */

/* module information */
MODULE_AUTHOR("Xue-Cheng Xi");
MODULE_DESCRIPTION("PRBS signal generator for EMC HAL");
MODULE_LICENSE("GPL");

static int num_chan = 2;	/* number of channels - default = 2 */
MODULE_PARM(num_chan, "i");
MODULE_PARM_DESC(num_chan, "number of channels");


/***********************************************************************
*                STRUCTURES AND GLOBAL VARIABLES                       *
************************************************************************/
typedef struct 
{
	hal_bit_t * enable;
	hal_bit_t * update;
	hal_float_t * output;

	hal_float_t ValInit;
	hal_float_t amp;
	hal_float_t offset;
	hal_s32_t	RegLenSet;
	hal_s32_t FreqDivSet;
	int	RegLen;
	int FreqDiv;
	int sbpa[11];
	int k1,k2;
	int FreqDivPos;
	int LastBit;
	int channel;
} hal_prbs_t;


/* pointer to array of prbs_t structs in shared memory, 1 per loop */
static hal_prbs_t *prbs_array;

/* component ID */
static int comp_id;		

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/

static int export_prbs(int num, hal_prbs_t * addr);
static void calc_prbs(void *arg, long period);
static void InitPrbs(hal_prbs_t *prbs,int RegLen,int FreqDiv,double ValInit,double amp,double offset);
static double GenPrbs(hal_prbs_t * prbs);
static void InitPRBS(hal_prbs_t * prbs);
static void UpdateParam(hal_prbs_t * prbs);

/***********************************************************************

*                       INIT AND EXIT CODE                             *
************************************************************************/


int rtapi_app_main(void)
{
    int n, retval;

    /* connect to the HAL */
    comp_id = hal_init("prbs");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "prbs: ERROR: hal_init() failed\n");
	return -1;
    }

    rtapi_print_msg(RTAPI_MSG_INFO, "prbs: installed sample function\n");

	/* allocate shared memory for prbs data */
    prbs_array = hal_malloc(num_chan * sizeof(hal_prbs_t));
    if (prbs_array == 0) 
	{
		rtapi_print_msg(RTAPI_MSG_ERR,"prbs: input: hal_malloc() failed\n");
		hal_exit(comp_id);
		return -1;
    }	

	/* export all the variables for each prbs */
    for (n = 0; n < num_chan; n++) 
	{
		/* export all vars */
		retval = export_prbs(n, &(prbs_array[n]));
		if (retval != 0) 
		{
			rtapi_print_msg(RTAPI_MSG_ERR, "prbs: input: prbs %d var export failed\n", n);
			hal_exit(comp_id);
			return -1;
		}
		InitPRBS(&(prbs_array[n]));
    }


    rtapi_print_msg(RTAPI_MSG_INFO, "prbs: installed %d prbs loops\n", num_chan);
    return 0;
}

void rtapi_app_exit(void)
{

    hal_exit(comp_id);
}


/***********************************************************************
*            REALTIME prbs FUNCTIONS				      			   *
************************************************************************/

static void calc_prbs(void *arg, long period)
{
	hal_prbs_t *prbs;

	// transfer data from HAL 
    prbs = arg;

	if( *(prbs->enable)==1){
		*(prbs->output)=GenPrbs(prbs);	
	}
	
	/* update new parameters*/	
	if( *(prbs->update)==1){
		UpdateParam(prbs);
		*(prbs->update)=0;
	}

}

static void InitPRBS(hal_prbs_t * prbs)
{
	int RegLen,FreqDiv;
	double ValInit,amp,offset;

	RegLen = prbs->RegLen;
	FreqDiv = prbs->FreqDiv;
	ValInit = prbs->ValInit;
	amp = prbs->amp;
	offset = prbs->offset;
	InitPrbs(prbs, RegLen, FreqDiv, ValInit, amp, offset);
}

static void UpdateParam(hal_prbs_t * prbs)
{
	int RegLen,FreqDiv;
	double ValInit,amp,offset;
	// RegLenSet and FreqDivSet from HAL pin
	RegLen = prbs->RegLenSet;
	FreqDiv = prbs->FreqDivSet;
	ValInit = prbs->ValInit;
	amp = prbs->amp;
	offset = prbs->offset;
	InitPrbs(prbs, RegLen, FreqDiv, ValInit, amp, offset);

	// check the outside in case of being outside of range
	prbs->RegLenSet = prbs->RegLen;
	prbs->FreqDivSet = prbs->FreqDiv;

}

static void InitPrbs(hal_prbs_t *prbs,int RegLen,int FreqDiv,double ValInit,double amp,double offset)
{
	int i;

	/* register length should be between 2 and 11 */
	if( 2<=RegLen && RegLen<=11){
		prbs->RegLen = RegLen;
	}else if(RegLen<2){
		prbs->RegLen = 2;
	}else if(RegLen>11){
		prbs->RegLen = 11;
	}
	/* frequency divider should be between 1 and 4*/
	if( 1<=FreqDiv && FreqDiv<=4){
		prbs->FreqDiv = FreqDiv;
	}else if(FreqDiv<1){
		prbs->FreqDiv = 1;
	}else if(FreqDiv>4){
		prbs->FreqDiv = 4;
	}

	prbs->ValInit   = ValInit;
	prbs->amp   = amp;
	prbs->offset   = offset;
	prbs->FreqDivPos = 1;
	
	for (i=0; i<11; i++) {	prbs->sbpa[i] = 1;}

	prbs->k1 = prbs->RegLen-1;
	prbs->k2 = prbs->RegLen;
	if (prbs->RegLen == 3)  {prbs->k1 = 1;}
	if (prbs->RegLen == 5)  {prbs->k1 = 3;}
	if (prbs->RegLen == 7)  {prbs->k1 = 4;}
	if (prbs->RegLen == 9)  {prbs->k1 = 5;}
	if (prbs->RegLen == 10) {prbs->k1 = 7;}
	if (prbs->RegLen == 11) {prbs->k1 = 9;}

}

static double GenPrbs(hal_prbs_t * prbs)
{
	int uiu,j;
	double out;
	if(prbs->FreqDivPos==1){
        uiu = -prbs->sbpa[prbs->k1-1]*prbs->sbpa[prbs->k2-1];
	    if (prbs->RegLen == 8){
			uiu = -prbs->sbpa[2-1]*prbs->sbpa[3-1]*prbs->sbpa[4-1]*prbs->sbpa[8-1];
		}

        for (j=prbs->RegLen-1; j >= 1; j--){
			prbs->sbpa[j] = prbs->sbpa[j-1];
		}
    	prbs->sbpa[0] = uiu;
		prbs->LastBit = uiu;

	} else {
		uiu = prbs->LastBit;
	}

	if(prbs->FreqDiv>=2){
		prbs->FreqDivPos++;
		if(prbs->FreqDivPos == prbs->FreqDiv+1){ 
			prbs->FreqDivPos=1;
		}
	}

   	out = uiu * prbs->amp + prbs->offset;
	return out;
}


/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/

static int export_prbs(int num, hal_prbs_t * addr)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);
	addr->channel = num;

    /* export pins */
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.enable", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->enable), comp_id);
    if (retval != 0){
		return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.update", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->update), comp_id);
    if (retval != 0){
		return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.output", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(addr->output), comp_id);
    if (retval != 0){
		return retval;
    }



	/* export parameters */
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.reglen", num);
    retval = hal_param_s32_new(buf, HAL_RW, &(addr->RegLenSet), comp_id);
    if (retval != 0){
		return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.freqdiv", num);
    retval = hal_param_s32_new(buf, HAL_RW, &(addr->FreqDivSet), comp_id);
    if (retval != 0){
		return retval;
    }

    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.amp", num);
    retval = hal_param_float_new(buf, HAL_RW, &(addr->amp), comp_id);
    if (retval != 0){
		return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.offset", num);
    retval = hal_param_float_new(buf, HAL_RW, &(addr->offset), comp_id);
    if (retval != 0){
		return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.val-init", num);
    retval = hal_param_float_new(buf, HAL_RW, &(addr->ValInit), comp_id);
    if (retval != 0){
		return retval;
    }

  	/* init all structure members */
    *(addr->enable) = 0;
    *(addr->update) = 0;
    *(addr->output) = 0;

	addr->FreqDiv =1;
	addr->RegLen = 11;
	addr->ValInit = 0.0;
	addr->amp = 0.005;
	addr->offset = 0.0;

	/* export function for this loop */
    rtapi_snprintf(buf, HAL_NAME_LEN, "prbs.%d.calc", num);
    retval = hal_export_funct(buf, calc_prbs, &(prbs_array[num]), 1, 0, comp_id);
    if (retval != 0) 
    {
	rtapi_print_msg(RTAPI_MSG_ERR, "prbs: calcs_prbs funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    
	/* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}