#include <stdio.h>
#include <stdlib.h>
#include <time.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 *inelm2 = &(asub_p->nok);
	epicsEnum16 *itype2 = &(asub_p->ftk);
	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 ( *inelm2 != 1 ) {
			epicsPrintf("%s asub_cryo_init: input %i allocated "
				"%i elements but should be 1\n", asub_p->name, i, *inelm2);
			return ERROR;
		}
		if ( *itype2 != menuFtypeDOUBLE ) {
			epicsPrintf("%s asub_cryo_init: input %i type is %i "
				"but should be %i\n", asub_p->name, i, *itype2, menuFtypeSHORT);
			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++;
		inelm2++;
		itype2++;
		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;

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

	/* global external disable for heartbeats */
	epicsInt16 disabled = *(epicsInt32 *)asub_p->i;

	/* bit mask of per-cavity external disable for heartbeats */
	epicsInt16 cav_disabled_mask = *(epicsInt32 *)asub_p->j;
	epicsInt16 cav_disabled[NCAVS];

	if ( debug ) {
		printf("%s asub_cryo\n  External disable heartbeats:\n"
				"    Global: %s\n    Cavity individual disable mask 0%x:\n",
			asub_p->name, disabled ? "DISABLED" : "ENABLED", cav_disabled_mask);
	}

    for ( i = 0; i < NCAVS; i++ ) {
		cav_disabled[i] = disabled || ((cav_disabled_mask & (1<<i)) >> i);
		if ( debug ) {
			printf("      Cavity %i: individual %s final %s\n",
				i+1, ((cav_disabled_mask & (1<<i)) >> i) ? "DISABLED" : "ENABLED",
				cav_disabled[i] ? "DISABLED" : "ENABLED");
		}
	}

	/* to read DBLINK severities for rfpermit and dissipated power */
	DBLINK *link_p;
	epicsEnum16 sevr[NCAVS];

	/* per-cavity latched interlock permit and its severity */
	link_p = (DBLINK *)&asub_p->inpk;
	epicsInt16 cav_permit[NCAVS];

	if ( debug ) {
		printf("  RFPERMIT per cavity:\n");
	}

	for ( i = 0; i < NCAVS; i++ ) {
		cav_permit[i] = *(epicsInt32 *)(asub_p->k + 8*i*sizeof(epicsInt32));
		dbGetSevr(&link_p[i], &sevr[i]);
		if ( (sevr[i] == INVALID_ALARM) ) {
			cav_disabled[i] = 1;
			if ( debug ) {
				printf("    Cavity %i RFPERMIT INVALID. Disable heartbeat\n", i+1);
			}
		}
		else if ( debug ) {
			printf("    Cavity %i RFPERMIT: %s\n",
				i+1, cav_permit[i] ? "OKAY" : "FAULT");
		}
	}

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

	/* read dissipated power severity and timestamp */
	link_p = (DBLINK *)&asub_p->inpa;

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

	int diff;
	int cnt, mask; /* manipulate heartbeat counter mask */

	*valid = 0;     /* Initialize to all invalid */
	*hb = *hb_last; /* Initialize to previous heartbeat mask */
	*override = 0;  /* Initialize to no power values forced to zero */

	if ( debug ) {
		printf("  Heartbeat checks %s:\n    Last valid mask 0x%x last heartbeat mask 0x%x\n",
			disabled ? "DISABLED" : "ENABLED", *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 ) {

			if ( (cav_permit[i] == 0) && (cav_disabled[i] == 0)) {
				*out_p[i] = 0.0;
				*override |= 1<<i;
			}
			else {
				*out_p[i] = *in_p[i];
			}

			if ( debug ) {
				printf("    Valid diss power, last %f, in %f, out %f, RFPERMIT override: %s\n",
					*out_last_p[i], *in_p[i], *out_p[i], !cav_permit[i] ? "TRUE" : "FALSE" );
			}

			if ( 0 == cav_disabled[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("    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 5

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->fte!=menuFtypeDOUBLE
            || asub_p->ftva!=menuFtypeDOUBLE
            || asub_p->ftvc!=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,
			*(double *)asub_p->e
	};

	epicsInt32 hwmode = *(epicsInt32 *)asub_p->u;

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

	if ( debug ) {
		printf("%s MAX_VALS %i hw mode %i\n", asub_p->name, (int)MAX_VALS, hwmode);
		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,
			*max_ignoring_hwmode = (double *)asub_p->valc;

	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_ignoring_hwmode = *max = vals[i];
			*index = i;
		}
	}

	if ( debug )
		printf("     before hwmode check max value %f index %i\n", *max, *index);

	/* If cavity offline, set max ADES to 0 and update limit reason */
	if ( hwmode == 2 ) {
		*max = 0;
		*index = 15;
		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;
	}
}

/* rotate MAX_NOTES instances of mode changes */
#define MAX_NOTES 5
/* character array sizes */
#define MAX_NOTE_SIZE 80

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


    if(asub_p->ftb!=menuFtypeCHAR
            || asub_p->ftc!=menuFtypeSTRING
            || asub_p->ftvc!=menuFtypeCHAR
            || asub_p->ftvd!=menuFtypeSTRING)
    {
        (void)recGblSetSevr(asub_p, COMM_ALARM, INVALID_ALARM);
        return ERROR;
    }

/* Inputs */
	epicsInt32 hwmode_des = *(epicsInt32 *)asub_p->a;

	char *why_in = (char *)asub_p->b;
	char *who_in = (char *)asub_p->c;

	char *why_out = (char *)asub_p->valc;
	char *who_out = (char *)asub_p->vald;

/* Outputs */
	epicsInt32 *hwmode = (epicsInt32 *)asub_p->valb;

	char *msg_out = (char *)asub_p->valq;

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

	if ( (0 == who_in[0]) || (0 == why_in[0]) ) {
		snprintf( (char *)msg_out, MAX_STRING_SIZE, "Failed. Missing who or why");
		if ( debug ) {
			printf("%s %s", asub_p->name, (char *)msg_out);
		}
		return ERROR;
	}

	snprintf( (char *)msg_out, MAX_STRING_SIZE, "Success");
	*hwmode = hwmode_des;

	if ( debug ) {
		printf("%s Set HWMODE to %i\n", asub_p->name, *hwmode);
	}

	/* Clear input notes */
	why_out[0] = '\0';
	who_out[0] = '\0';

	return SUCCESS;
}

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

    if(asub_p->fta!=menuFtypeCHAR
            || asub_p->ftc!=menuFtypeCHAR
            || asub_p->ftg!=menuFtypeCHAR
            || asub_p->ftk!=menuFtypeCHAR
            || asub_p->fto!=menuFtypeCHAR
            || asub_p->ftva!=menuFtypeCHAR
            || asub_p->ftve!=menuFtypeCHAR
            || asub_p->ftvi!=menuFtypeCHAR
            || asub_p->ftvm!=menuFtypeCHAR
            || asub_p->ftvq!=menuFtypeCHAR

            || asub_p->ftb!=menuFtypeSTRING
            || asub_p->ftd!=menuFtypeSTRING
            || asub_p->fte!=menuFtypeSTRING
            || asub_p->ftf!=menuFtypeSTRING
            || asub_p->fth!=menuFtypeSTRING
            || asub_p->fti!=menuFtypeSTRING
            || asub_p->ftj!=menuFtypeSTRING
            || asub_p->ftl!=menuFtypeSTRING
            || asub_p->ftm!=menuFtypeSTRING
            || asub_p->ftn!=menuFtypeSTRING
            || asub_p->ftp!=menuFtypeSTRING
            || asub_p->ftq!=menuFtypeSTRING
            || asub_p->ftr!=menuFtypeSTRING
            || asub_p->ftt!=menuFtypeSTRING

            || asub_p->ftvb!=menuFtypeSTRING
            || asub_p->ftvc!=menuFtypeSTRING
            || asub_p->ftvd!=menuFtypeSTRING
            || asub_p->ftvf!=menuFtypeSTRING
            || asub_p->ftvg!=menuFtypeSTRING
            || asub_p->ftvh!=menuFtypeSTRING
            || asub_p->ftvj!=menuFtypeSTRING
            || asub_p->ftvk!=menuFtypeSTRING
            || asub_p->ftvl!=menuFtypeSTRING
            || asub_p->ftvn!=menuFtypeSTRING
            || asub_p->ftvo!=menuFtypeSTRING
            || asub_p->ftvp!=menuFtypeSTRING
            || asub_p->ftvr!=menuFtypeSTRING
            || asub_p->ftvs!=menuFtypeSTRING
            || asub_p->ftvt!=menuFtypeSTRING)
    {
        (void)recGblSetSevr(asub_p, COMM_ALARM, INVALID_ALARM);
        return ERROR;
    }

/* Inputs */

	char *why_in[MAX_NOTES] = {
			(char *)asub_p->a,
			(char *)asub_p->c,
			(char *)asub_p->g,
			(char *)asub_p->k,
			(char *)asub_p->o
	};

	char *who_in[MAX_NOTES] = {
			(char *)asub_p->b,
			(char *)asub_p->d,
			(char *)asub_p->h,
			(char *)asub_p->l,
			(char *)asub_p->p
	};

	char *ts_in[MAX_NOTES-1] = {
			(char *)asub_p->e,
			(char *)asub_p->i,
			(char *)asub_p->m,
			(char *)asub_p->q
	};

	char *modestr_in[MAX_NOTES-1] = {
			(char *)asub_p->f,
			(char *)asub_p->j,
			(char *)asub_p->n,
			(char *)asub_p->r
	};

	epicsInt32 hwmode = *(epicsInt32 *)asub_p->s;

	char *name   = (char *)asub_p->t;

/* Outputs */

	char *why_out[MAX_NOTES] = {
			(char *)asub_p->vala,
			(char *)asub_p->vale,
			(char *)asub_p->vali,
			(char *)asub_p->valm,
			(char *)asub_p->valq
	};

	char *who_out[MAX_NOTES] = {
			(char *)asub_p->valb,
			(char *)asub_p->valf,
			(char *)asub_p->valj,
			(char *)asub_p->valn,
			(char *)asub_p->valr
	};

	char *ts_out[MAX_NOTES] = {
			(char *)asub_p->valc,
			(char *)asub_p->valg,
			(char *)asub_p->valk,
			(char *)asub_p->valo,
			(char *)asub_p->vals
	};

	char *modestr_out[MAX_NOTES] = {
			(char *)asub_p->vald,
			(char *)asub_p->valh,
			(char *)asub_p->vall,
			(char *)asub_p->valp,
			(char *)asub_p->valt
	};

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

	time_t rawtime;
	struct tm *tmp_p;

	for (i = 0; i < MAX_NOTES; i++ ) {
		snprintf( (char *)who_out[i], MAX_STRING_SIZE, (char *)who_in[i]);
		snprintf( (char *)why_out[i], MAX_NOTE_SIZE, (char *)why_in[i]);
		if ( i == 0 ) {
			rawtime = time(NULL);
			tmp_p = localtime(&rawtime);
			strftime( ts_out[i], MAX_STRING_SIZE, "%m/%d/%Y %H:%M:%S", tmp_p );

/* Must match definitions in hwmodeCavCommon.template, statCav.template */
			if ( hwmode == 0 )
				snprintf( modestr_out[i], MAX_STRING_SIZE, "To ON");
			else if ( hwmode == 1 )
				snprintf( modestr_out[i], MAX_STRING_SIZE, "To MNT");
			else if ( hwmode == 2 )
				snprintf( modestr_out[i], MAX_STRING_SIZE, "To OFF");
			else if ( hwmode == 3 )
				snprintf( modestr_out[i], MAX_STRING_SIZE, "To MDN");
			else if ( hwmode == 4 )
				snprintf( modestr_out[i], MAX_STRING_SIZE, "To RDY");
			else {
				printf("%s Unsupported mode value %i. Check database for HWMODE\n",
					asub_p->name, hwmode);
			}
		}
		else {
			j = i-1;
			snprintf( (char *)ts_out[i], MAX_STRING_SIZE, (char *)ts_in[j]);
			snprintf( (char *)modestr_out[i], MAX_STRING_SIZE, (char *)modestr_in[j]);
		}

		if ( debug ) {
			printf("%s New history %ith entry:\n    %s %s\n    %s %s\n", asub_p->name, i+1,
				(char *)who_out[i], (char *)why_out[i],
				(char *)ts_out[i], (char *)modestr_out[i]);
		}
	}

	errlogPrintf("%sHWMODE Changed %s, by %s, %s\n", name, modestr_out[0], who_out[0], why_out[0]);

	return SUCCESS;
}

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

    if(asub_p->ftva!=menuFtypeCHAR)
    {
        (void)recGblSetSevr(asub_p, COMM_ALARM, INVALID_ALARM);
        return ERROR;
    }

	char *note = (char *)asub_p->vala;

	note[0] = '\0';

	return SUCCESS;
}

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},
	{"set_hwmode", (REGISTRYFUNCTION) &asub_set_hwmode},
	{"set_hwmode_hist", (REGISTRYFUNCTION) &asub_set_hwmode_hist},
	{"note_clear", (REGISTRYFUNCTION) &asub_note_clear}
};

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

#include <epicsExport.h>

epicsExportRegistrar(rfsubRegister);