/* subroutine record for calculating the temperature compensation for   */
/* device              							*/
/*									*/
/* Version History:							*/
/*	05/03/2019	    namrata					*/
/*	 Initial version 						*/

#include <epicsStdlib.h>
#include <epicsStdioRedirect.h>
#include <epicsMath.h>
#include <dbAccess.h>
#include <dbDefs.h>
#include <dbCommon.h>
#include <registryFunction.h>
#include <epicsExport.h>
#include <recSup.h>
#include <aSubRecord.h>

#define ABS(a,b) ((a<b)?(b-a):(a-b)) 

/* Initialization function for all  subroutines */
static int	pvInit(aSubRecord *pasub) {
	return(0);
}

/* Temperature compensation for undulator */
static int pvProcess(aSubRecord *precord) {

	double rtd1          =  *(double*)precord->a; /* Input: RTD1 reading*/ 
	double rtd2          =  *(double*)precord->b; /* Input: RTD2 reading*/ 
	double rtd3          =  *(double*)precord->c; /* Input: RTD3 reading*/
	double rtd4          =  *(double*)precord->d; /* Input: RTD4 reading*/
	double rtd1Inc       =  *(double*)precord->e; /* Input: RTD1 include request for compensation calculations*/
	double rtd2Inc       =  *(double*)precord->f; /* Input: RTD2 include request for compensation calculations*/
	double rtd3Inc       =  *(double*)precord->g; /* Input: RTD3 include request for compensation calculations*/
	double rtd4Inc       =  *(double*)precord->h; /* Input: RTD4 include request for compensation calculations*/
	double rtdSpread     =  *(double*)precord->i; /* Input: allowable deviations between sensor readings*/
	double tempAdjustReq =  *(double*)precord->j; /* Input: temperature compensation Request*/
	double mmfTemp       =  *(double*)precord->k; /* Input: MMF Temperature */
	double DEBUG         =  *(double*)precord->l; /* Input: Debug Menu */

    double rtd1IncStat   = 0;/* RTD1 include status for temperature compensation */ 
    double rtd2IncStat   = 0;/* RTD2 include status for temperature compensation */
    double rtd3IncStat   = 0;/* RTD3 include status for temperature compensation */
    double rtd4IncStat   = 0;/* RTD4 include status for temperature compensation */
    double meanTemp      = 0;/* Mean Temperature of Device */
    double rtdCount      = 0;/* Count of number of RTDs used for compensation */
    double tempAdjustStat= 0;/* Status of temperature compensation */
   
/* Difference between each RTD reading*/ 
    double rtd12Diff     = 0;
    double rtd13Diff     = 0;
    double rtd14Diff     = 0;
    double rtd23Diff     = 0;
    double rtd24Diff     = 0;
    double rtd34Diff     = 0;

    if (DEBUG) {
		printf("\nProcess Begins");
		printf("\nrtd1 rtd2 rtd3 rtd4: %f %f %f %f", rtd1, rtd2, rtd3, rtd4);
		printf("\nrtdSpread: %f", rtdSpread);
    }

/* If Temperature Compensation is requested, check 
 * If any RTD is included. Then select the included RTD. Calculate if any other RTD is within the spread. If yes, Change include status. Continue for other RTDs.
 * Check for count of RTD include. If less than two, mean temperature should be equivalent to MMF Temp. 
 * If Temperature Compensation is not requested, 
 * Add include status must be No, mean temperature should be MMF temperature. */


    if (tempAdjustReq) {

        /* Check for RTD1 */
        if (rtd1Inc) {
            rtd12Diff = ABS(rtd1,rtd2*rtd2Inc);
            rtd13Diff = ABS(rtd1,rtd3*rtd3Inc);
            rtd14Diff = ABS(rtd1,rtd4*rtd4Inc);
            
            if (DEBUG) {
	     	    printf("\nrtd12 rtd13 rtd14 : %f %f %f", rtd12Diff, rtd13Diff, rtd14Diff);
            }
            if ((rtd12Diff <= rtdSpread ) || (rtd13Diff <= rtdSpread) || ( rtd14Diff <= rtdSpread )) {
                rtd1IncStat = 1;
                if (DEBUG) {
	     	        printf("\nrtd1IncStat : %f", rtd1IncStat);
                }
            }/*end_if for RTD spread check */
        }/*end_if for RTD1*/
        
        /* Check for RTD2 */
        if (rtd2Inc) {
            rtd23Diff = ABS(rtd2,rtd3*rtd3Inc);
            rtd24Diff = ABS(rtd2,rtd4*rtd4Inc);
            
            if (DEBUG) {
	     	    printf("\nrtd23 rtd24 : %f %f", rtd23Diff, rtd24Diff);
            }
            
            if ((rtd12Diff <= rtdSpread )|| (rtd23Diff <= rtdSpread ) || ( rtd24Diff <= rtdSpread )) {
                rtd2IncStat = 1;
                if (DEBUG) {
	     	        printf("\nrtd2IncStat : %f", rtd2IncStat);
                }
            }/*end_if for RTD spread check */
        }/*end_if for RTD2*/

        /* Check for RTD3 */
        if (rtd3Inc) {
            rtd34Diff = ABS(rtd3,rtd4*rtd4Inc);
            
            if (DEBUG) {
	     	    printf("\nrtd34 : %f", rtd34Diff);
            }
            if ((rtd13Diff <= rtdSpread )|| (rtd23Diff <= rtdSpread ) || ( rtd34Diff <= rtdSpread )) {
                rtd3IncStat = 1;
                if (DEBUG) {
	     	        printf("\nrtd3IncStat : %f", rtd3IncStat);
                }
            }/*end_if for RTD spread check */
        }/*end_if for RTD3*/
        
        /* Check for RTD4 */
        if (rtd4Inc) {
            if ((rtd14Diff <= rtdSpread )|| (rtd24Diff <= rtdSpread ) || ( rtd34Diff <= rtdSpread )) {
                rtd4IncStat = 1;
                if (DEBUG) {
	     	        printf("\nrtd4IncStat : %f", rtd4IncStat);
                }
            }/*end_if for RTD spread check */
        }/*end_if for RTD4*/

        /* calculate the RTD`s included for count; mean temperature and status */ 
        rtdCount       = rtd1IncStat + rtd2IncStat + rtd3IncStat + rtd4IncStat ;
        meanTemp       = ( (rtd1IncStat*rtd1) + (rtd2IncStat*rtd2) + (rtd3IncStat*rtd3) + (rtd4IncStat*rtd4) ) / rtdCount ;  
        tempAdjustStat = 2;
        if (DEBUG) {
            printf("\nrtdCount, meanTemp, tempAdjustStat: %f  %f %f", rtdCount, meanTemp, tempAdjustStat);
        }
        
        /* Handle if less than 2 RTD`s included */
        if (rtdCount < 2) {
            tempAdjustStat= 0;
            meanTemp = mmfTemp;
            if (DEBUG) {
                printf("\n tempAdjustStat: %f", tempAdjustStat);
            }
        }/* end_if rtdCount <2*/
    }/*end_if tempAdjust*/

    if (!tempAdjustReq) {
        rtd1IncStat = 0;
        rtd2IncStat = 0;
        rtd3IncStat = 0;
        rtd4IncStat = 0;
        meanTemp = mmfTemp;
        rtdCount    = 0;
        tempAdjustStat= 1;
        if (DEBUG) {
            printf("\nmeanTemp, tempAdjustStat : %f  %f", meanTemp, tempAdjustStat);
        }
    }/*end_if temperature compensation not requested*/

    if (DEBUG) {
       	printf("\nProcess Ends\n");
    }

    *(double *)precord->vala = rtd1IncStat;
    *(double *)precord->valb = rtd2IncStat;
    *(double *)precord->valc = rtd3IncStat;
    *(double *)precord->vald = rtd4IncStat;
    *(double *)precord->vale = meanTemp;
    *(double *)precord->valg = rtdCount;
    *(double *)precord->valh = tempAdjustStat;

	return(0);
}

/* Temperature compensation for phase shifter */
static int pvProcessPS(aSubRecord *precord) {

    double rtd1          =  *(double*)precord->a; /* Input: RTD1 reading*/ 
    double rtd2          =  *(double*)precord->b; /* Input: RTD2 reading*/ 
    double rtdSpread     =  *(double*)precord->c; /* Input: allowable deviations between sensor readings*/
    double tempAdjustReq =  *(double*)precord->d; /* Input: temperature compensation Request*/
    double mmfTemp       =  *(double*)precord->e; /* Input: MMF Temperature */


    double meanTemp      = 0;/* Output: Average Temperature of Device */
    double tempAdjustStat= 0;/* output: Status of temperature compensation */

    /* If temperature Adjustment is NOT requested*/
    if(!tempAdjustReq) {
        meanTemp = mmfTemp;
        tempAdjustStat = 1;
    }
    /* If temperature adjustment is requested */
    else {  
        /* Check if delta temperature is in range*/
        if(ABS(rtd1,rtd2) <= rtdSpread) {
            meanTemp = (rtd1 + rtd2)/2.0;
            tempAdjustStat = 2;
        }
        /* Delta temperature is not in range*/
        else {
            meanTemp = mmfTemp;
            tempAdjustStat = 0;
        }
    }
    
    /* Set output PVs*/
    *(double *)precord->vala = meanTemp;
    *(double *)precord->valb = tempAdjustStat;

    return(0);
}

epicsRegisterFunction(pvInit);
epicsRegisterFunction(pvProcess);
epicsRegisterFunction(pvProcessPS);