#include <stdio.h>
#include <stdlib.h>

#include <epicsExport.h>
#include <registryFunction.h>
#include <aSubRecord.h>
#include <epicsTypes.h>
#include <recGbl.h>
#include <alarm.h>
#include <dbAccess.h>
#include <menuFtype.h>
#include <errlog.h>

static long SUCCESS =  0;
static long ERROR   = -1;

#define NCAVS 8

long
asub_cryo_init(struct aSubRecord *asub_p) {

	if( asub_p == NULL)
		return ERROR;

	epicsUInt32 *inelm = &(asub_p->noa);
	epicsEnum16 *itype = &(asub_p->fta);
	epicsUInt32 *onelm = &(asub_p->nova);
	epicsEnum16 *otype = &(asub_p->ftva);
	int i;

	for ( i = 0 ; i < NCAVS ; i++ ) {

		if ( *inelm != 1 ) {
			epicsPrintf("%s asub_cryo_init: input %i allocated "
				"%i elements but should be 1\n", asub_p->name, i, *inelm);
			return ERROR;
		}
		if ( *itype != menuFtypeDOUBLE ) {
			epicsPrintf("%s asub_cryo_init: input %i type is %i "
				"but should be %i\n", asub_p->name, i, *itype, menuFtypeDOUBLE);
			return ERROR;
		}
		if ( *onelm != 1 ) {
			epicsPrintf("%s asub_cryo_init: output %i allocated "
				"%i elements but should be 1\n", asub_p->name, i, *onelm);
			return ERROR;
		}
		if ( *otype != menuFtypeDOUBLE ) {
			epicsPrintf("%s asub_cryo_init: output %i type is %i "
				"but should be %i\n", asub_p->name, i, *otype, menuFtypeDOUBLE);
			return ERROR;
		}
		inelm++;
		itype++;
		onelm++;
		otype++;
	}

	return SUCCESS;
}

long
asub_cryo(struct aSubRecord *asub_p) 
{
	if( asub_p == NULL)
		return SUCCESS;

	/* dissipated power values, in, out, and last out */
	double  **in_p  = (double **)&asub_p->a,
			**out_p = (double **)&asub_p->vala,
			**out_last_p = (double **)&asub_p->ovla;

	/* mask of valid dissipated power and heartbeat counters */
	epicsUInt32 *valid = (epicsUInt32 *)asub_p->vali,
				*valid_last = (epicsUInt32 *)asub_p->ovli,
				*hb = (epicsUInt32 *)asub_p->valj,
				*hb_last = (epicsUInt32 *)asub_p->ovlj;

	/* read dissipated power severity and timestamp */
	DBLINK *link_p = (DBLINK *)&asub_p->inpa;
	epicsEnum16 sevr[NCAVS];

	/* store last timestamp */
	epicsTimeStamp timestamp;
	epicsUInt32 *ts = (epicsUInt32 *)asub_p->valk,
				*ts_last = (epicsUInt32 *)asub_p->ovlk;

	int i, diff;
	short debug = (asub_p->tpro > 1) ? 1 : 0;
	int cnt, mask; /* manipulate heartbeat counter mask */

	*valid = 0;     /* Initialize to all invalid */
	*hb = *hb_last; /* Initialize to previous heartbeat mask */

	if ( debug ) {
		printf("%s asub_cryo:\n     last valid mask 0x%x last heartbeat mask 0x%x\n",
			asub_p->name, *valid_last, *hb_last);
	}

	for ( i = 0; i < NCAVS; i++ ) {
		dbGetSevr(&link_p[i], &sevr[i]);
		dbGetTimeStamp(&link_p[i], &timestamp);
		ts[i*2]     = timestamp.secPastEpoch;
		ts[i*2 + 1] = timestamp.nsec;
		diff = (ts[i*2] != ts_last[i*2]) || (ts[i*2 + 1] != ts_last[i*2 + 1]);
    	if ( debug ) {
			printf("     cavity %i sevr %i diss power %f\n", i + 1, sevr[i], *in_p[i]);
			printf("     new timestamp %i s %i ns, prev %i s %i ns\n", ts[i*2], ts[i*2 + 1], ts_last[i*2], ts_last[i*2 + 1]);
		}
		if ( (sevr[i] != INVALID_ALARM) && diff ) {
			*out_p[i] = *in_p[i];
			*valid |= 1<<i;

			/* Increment heartbeat counter */
			mask = 3<<(i*2);
			cnt = (*hb & mask) >> (i*2); /* Last value */
			cnt = (cnt >= 3)?0:cnt+1; /* Increment or roll over */
			*hb &= ~mask; /* Clear value */
			*hb |= (cnt)<<(i*2);
			
			if ( debug ) {
				printf("            valid diss power, last %f, in %f, out %f\n",
					*out_last_p[i], *in_p[i], *out_p[i]);
				printf("            mask 0x%x cnt 0x%x hb 0x%x\n",	mask, cnt, *hb);
			}
		}
	}

	if ( debug ) {
		printf("     final valid mask 0x%x and heartbeat mask 0x%x\n", *valid, *hb);
	}

	return SUCCESS;
}

/* From set of inputs, calculate maximum value
 * and index of maximum value
 * Values to compare must start at input A
 * and be contingous 
 * Hard-coded number of inputs, currently:
 */

#define MAX_VALS 4

long
asub_max_index(struct aSubRecord *asub_p) 
{
	if( asub_p == NULL)
		return SUCCESS;

    if(asub_p->fta!=menuFtypeDOUBLE
            || asub_p->ftb!=menuFtypeDOUBLE
            || asub_p->ftc!=menuFtypeDOUBLE
            || asub_p->ftd!=menuFtypeDOUBLE
            || asub_p->ftva!=menuFtypeDOUBLE)
    {
        (void)recGblSetSevr(asub_p, COMM_ALARM, INVALID_ALARM);
        return ERROR;
    }

	/* Inputs, values to find max in */
	double vals[MAX_VALS] = {
			*(double *)asub_p->a, 
			*(double *)asub_p->b, 
			*(double *)asub_p->c,
			*(double *)asub_p->d
	};

	int i;
	short debug = (asub_p->tpro > 1) ? 1 : 0;

	if ( debug ) {
		printf("%s MAX_VALS %i\n", asub_p->name, (int)MAX_VALS);
		for (i = 0; i < MAX_VALS; i++ )
			printf("     %ith value %f\n", i, vals[i]);
	}	

	/* Outputs: max value & index of max value */
	double *max =   (double *)asub_p->vala;
	epicsInt32 *index = (epicsInt32 *)asub_p->valb;

	/* Initialize to first limit in list */
	*max = vals[0]; 
	*index = 0;

	/* If duplicate values, last one wins */
	for ( i = 1; i < MAX_VALS; i++ ) {
		if ( vals[i] <= *max ) {
			*max = vals[i];
			*index = i;
		}
	}

	if ( debug )
		printf("     final max value %f index %i\n", *max, *index);
	
	return SUCCESS;
}

long
asub_timer_exec(struct aSubRecord *asub_p) 
{
	if( asub_p == NULL)
		return SUCCESS;

    if(asub_p->fta!=menuFtypeDOUBLE
            || asub_p->ftb!=menuFtypeDOUBLE)
    {
        (void)recGblSetSevr(asub_p, COMM_ALARM, INVALID_ALARM);
        return ERROR;
    }

	/* Inputs:
	 *   number of iterations
	 *   iteration period in seconds
	 *   flag indicating beginning of sequence
	 *   last count of iterations complete
	 */
	double n      = *(double *)asub_p->a, 
		   period = *(double *)asub_p->b;
	epicsInt32 flag = *(epicsInt32 *)asub_p->c,
	           cnt_inp  = *(epicsInt32 *)asub_p->d,
	           enable  = *(epicsInt32 *)asub_p->e;

	short debug = (asub_p->tpro > 1) ? 1 : 0;

	epicsInt32 *clear = (epicsInt32 *)asub_p->vala;
	double *timer_start = (double *)asub_p->valb;
	epicsInt32 *go = (epicsInt32 *)asub_p->valc;
	epicsInt32 *cnt = (epicsInt32 *)asub_p->vald;

	*go = 1;
	*clear = 1;

	time_t timer_start_raw, time_now_raw;
	double time_sec = 0.0, time_now = 0.0;

	char timestr[40];
	struct tm *tm_p;

	*cnt = cnt_inp; /* Intialize new cnt to previous */

	if ( debug ) {
		printf("%s n %.0f period %f cnt_inp %i enable %i\n", asub_p->name, n, period, cnt_inp, enable);
	}

	/* If disabled, reset flag and cnt to effectively cancel the sequence */
	if (!enable) {
		*cnt = 65535;
		if ( debug ) {
			printf("%s !enable, set cnt to %i, process clear and exit\n", asub_p->name, *cnt);
		}
		dbPutLink(&asub_p->outa, asub_p->ftva, asub_p->vala, asub_p->neva);
		dbPutLink(&asub_p->outd, asub_p->ftvd, asub_p->vald, asub_p->nevd);
		return ERROR;
	}

	/* Flag indicates start of series of iterations
	 * Reset timer, clear count
	 */
	if ( flag ) {
		time(&timer_start_raw);
		time_sec = difftime(timer_start_raw,0); 
		*timer_start = time_sec; /* Store timer start for later iterations */
		*cnt = 0;
		if ( debug ) {
			printf("    flag %i timer_start %f %f cnt_inp %i cnt %i\n", 
				flag, *timer_start, time_sec, cnt_inp, *cnt);
		}
	}
	else if ((*cnt >= n) || (*cnt < 0)) {
		if ( debug ) {
			printf("    cnt -1; waiting for wake-up\n"); 
		}	
		return ERROR;
	}
	else {
		*timer_start = *(double *)asub_p->ovlb;
		if ( debug ) {
			printf("    flag %i timer_start %f prev %f cnt %i\n", 
				flag, *timer_start, *(double *)asub_p->ovlb, *cnt);
		}
	}

	time(&time_now_raw);
	time_now = difftime(time_now_raw,0);
	tm_p = localtime(&time_now_raw);
	strftime(timestr, 40, "%c", tm_p);

	if ( debug ) {
		printf("    %s time_now %f s timer_start %f s diff %f m cnt %i\n", 
			timestr, time_now, *timer_start, (time_now - *timer_start)/60, *cnt);
	}	

	if ( (time_now - *timer_start) >= (*cnt * period) ) {
		printf("    **** %s time for rack checkout, diffs %f %f\n", 
			timestr, (time_now - *timer_start), (*cnt * period));
		errlogPrintf("%s start automated rack self-check\n", asub_p->desc);
		*cnt = *cnt + 1;
		if ( *cnt >= n ) {
			*cnt = 65535;
		}
		return SUCCESS;
	}
	else {
		return ERROR;
	}
}

static registryFunctionRef rf_sub_seq[] = {
	{"asub_cryo_init", (REGISTRYFUNCTION) &asub_cryo_init},
	{"asub_cryo", (REGISTRYFUNCTION) &asub_cryo},
	{"max_index", (REGISTRYFUNCTION) &asub_max_index},
	{"timer_exec", (REGISTRYFUNCTION) &asub_timer_exec}
};

static
void rfsubRegister(void) {
    registryFunctionRefAdd(rf_sub_seq, NELEMENTS(rf_sub_seq));
}

#include <epicsExport.h>

epicsExportRegistrar(rfsubRegister);