#include <math.h>
#include <memory.h>
#include <stdio.h>
#include <stdlib.h>
#include "dasutil.h"
#include "acqdef.h"
#include "ppccApSem.h"
#include "rtcdef.h"
#include "pcs_common.h"
#ifdef _SKY
#include <mathlib.h>
#include <sky/boltprotos.h>
#else
#include "mathlib_simulate.h"
#endif
#include "fortran_printf.h"

/***********************************************************************
* rtc_plasmacontrol.c --
*
* Read request voltages provided by pcs, via shared memory.
*
* History:
*  23-Mar-2004  TRG  Use waitApSem function to wait for new cmds from pcs.
*  13-Oct-2003  TRG  Comment-out the "undef" of vmax for SKYvec 5.6.0.2.
*                     This kludge was required because the "vmax" field of
*                     the struct "nstx_pcs_gen" was being confused with
*                     vmax from mathlib.h.
*  05-Aug-2003  TRG  Map pcsCommon cached.  New sub-structs psrtcc & pcsc.
*  28-Jul-2003  TRG  Convert from doublePrecision to singlePrecision.
*  03-Jun-2003  TRG  pc_seti_:  set sim->pubNstxTime.
*  06-Jul-2000  TRG  pc_seti_:  wait to set sim->u.sopeopFlag until
*                     after "published" time+data have been set.
*  16-Mar-2000  TRG  Revisions for PSC_1:  new rtc/pcs communications logic.
*  14-Mar-2000  TRG  pc_seti_: completely revised for version PSC_1.  Used
*                     only in simulation mode -- real data comes from acq.
*  09-Mar-2000  TRG  Check for npcsFlag before doing plasmacontrol_init.
*  25-Oct-1999  TRG  Move calibration factors to rtcconfig.h.
*  08-Oct-1999  TRG  pc_seti: Add tfCorrection to ip.
*  25-Jun-1999  TRG  Add waitReqTime() routine.
*  13-May-1999  TRG  Create.
*
************************************************************************/


		/*=======================================================
		 * Definitions, structures, etc ...
		 *======================================================*/
#define DUPLICATES_BEFORE_ERROR   10	/* num dups permitted before error*/
#define ONE_MSEC    1.0e-3
#define IEEE_EXPONENT_MASK  0x7F800000
#define MAX_PCS   25		/* dim of pcsInfo[] array	*/
#define MAX_SETV   20
#define MAXDUP   10
#define MAXWAIT   25
#define TEN_USECS_PER_MSEC   100	/* number of 10 usec bins per msec*/
#define TICKS_PER_MSEC    (FAST_PER_SECOND/1000)

#ifndef __ppc
#define ppc_cache_invalidate(P,N)		/* dummy for Host	*/
#define ppc_cache_flush(P,N)			/* dummy for Host	*/
#endif

struct duplicates  {
    int   reqTime;		/* req time of duplicated voltage req	*/
    int   firstTime;		/* "ntdata" of first duplication	*/
    int   lastTime;		/* "ntdata" at last duplication		*/
    int   cnt;			/* number of duplicates at "time"	*/
   };
struct tmoInfo  {
    int   ntdata;		/* current "time"			*/
    int   minReqTime;
    int   reqTime;
    int   tmoConsecutive;
   };
struct setvInfo  {
    int   setvNtdata;
    int   setvReqTime;
   };


		/*========================================================
		 * External variables ...
		 *=======================================================*/
extern struct nstx_pcs_gen  nstx_pcs_gen;
extern struct nstx_pcs_collect_real  nstx_pcs_collect_real;
extern struct nstx_pcs_collect_integer  nstx_pcs_collect_integer;
extern struct pcs_common_ftn  pcs_common_ftn;


		/*=======================================================
		 * Global variables ...
		 *======================================================*/
int   gPcsReqTime;


		/*=======================================================
		 * Static variables ...
		 *======================================================*/
static int   dupCount;
static int   errCount;
static int   gReqTime;
static int   gReqTimeB4;
static int   pcsStatus;		/* PlasmaControlSystem status		*/
static int   setvInfoCnt;
static int   pcsCnt;
static int   pcsInfoIdx;
static int   tmoCnt;		/* Number of timeouts waiting for pcs	*/
static struct duplicates  duplicates[MAXDUP];
static struct setvInfo  setvInfo[MAX_SETV];
static struct tmoInfo  tmoInfo[MAXWAIT];

static struct  pcs_common  *pcsCommon;
static struct pcsPcsCommon  *pcsc;
static struct psrtcPcsCommon  *psrtcc;
static struct pubData  *sim;

static struct pcsInfo  {
        int   reqTime;
        int   pubTime;
        int   pcsCnt;
        int   pcsDt;
       }  pcsInfo[MAX_PCS];



	/***************************************************************
	 * waitPcsRequest:
	 ***************************************************************/
static int   waitPcsRequest( int minReqTime )
#ifndef __ppc
   {
    errorExit("waitPcsRequest called on Host!  How silly.");
    return(0);			//keep compiler happy
   }
#else
   {
    int   id;
    int   idx;
    int   pcscReqTime;
    int   sts;
    long long  btTimeoutTime,btNow;
    struct tmoInfo  *tmo;
    static int   tmoConsecutiveCnt;

    ppc_readtimer(&btNow);
    if (!nstx_pcs_gen.nsubmode)
        btTimeoutTime = btNow + SEC2BT(3.0*FAST_DT);
    else
        btTimeoutTime = btNow + SEC2BT(1.0);	// 1-sec timeout (sim)

			/*===============================================
			 * Wait for request time GE minReqTime ...
			 *==============================================*/
    for ( ; btNow<btTimeoutTime ; ppc_readtimer(&btNow))
       {
        union udataApSem  udata;

        ppc_cache_invalidate((void *)pcsc,sizeof(*pcsc));
        sts = waitApSem(SEMIDX_NEW_PCSREQUEST,&udata,FAST_DT);
        if (!sts)
            continue;

        idx = udata.ival;
        if (idx != pcsc->reqIdx)
           {
            pcscReqTime = pcsc->reqTime[pcsc->reqIdx];
            if (pcscReqTime < pcsc->reqTime[idx])
                errorExit("waitPcsRequest:  Confusing request times?");
            idx = pcsc->reqIdx;
           }
        gReqTime = pcsc->reqTime[idx];
        gPcsReqTime = gReqTime;		//global copy for rtc_outdat.c info
        if (gReqTime >= minReqTime)
           {
            tmoConsecutiveCnt = 0;
            id=32; profile_update(&id,"waitPcsRequest: success",0);
            return(idx);		//-----------------------> success
           }
       }
			/*-----------------------------------------------
			 * TIMEOUT waiting for new pcsc request ...
			 *----------------------------------------------*/
    idx = pcsc->reqIdx;
    tmo = tmoInfo + tmoCnt++;
    if (tmoCnt <= MAXWAIT)
       {
        tmo->minReqTime = minReqTime;
        tmo->reqTime = gReqTime;
        tmo->tmoConsecutive = tmoConsecutiveCnt;
       }
    tmoConsecutiveCnt++;
    id=34; profile_update(&id,"waitPcsRequest: TIMEOUT",0);
    return(idx);			//----------------------> timeout
   }
#endif	/* __ppc */



	/****************************************************************
	 * plasmacontrol_init_:
	 * Initialize plasmacontrol parameters for new shot cycle
	 * Note:  called only when rtc860 is run with "-pcs" flag.
	 ****************************************************************/
int   FORTRAN_SUBROUTINE(plasmacontrol_init)()
   {
    static int   firsttime = 1;

    if (!pcs_common_ftn.npcsFlag && !pcs_common_ftn.npcsTest)
       {
        pcsmsg(0,"*WARN* plasmacontrol_init called without npcsFlag");
        return(0);
       }
		/*=======================================================
		 * Firsttime only:
		 * Check that "magnetics" buffer in pcs_common matches
		 *  size of "struct magneticsSignals".
		 * Map shared-memory segments.
		 *======================================================*/
    if (firsttime)
       {
        firsttime = 0;
        pcsCommon = mapPcsCommon_cache();
        pcsc = &pcsCommon->pcs;
        psrtcc = &pcsCommon->psrtc;
        if ((int)pcsc%CACHELINE_NBYTES || (int)psrtcc%CACHELINE_NBYTES)
            errorExit("CACHELINE_NBYTES alignment pcsc=%08X  psrtcc=%08X",
                pcsc,psrtcc);
        sim = mapSimData();
        if (!pcsCommon || !sim)
            exit(pcsmsg(0,"*EXIT* failed to map shared memory"));
       }

    gReqTimeB4 = -999;
    dupCount = 0;
    pcsCnt = 0;
    pcsInfoIdx = 0;
    errCount = 0;
    pcsStatus = 1;
    setvInfoCnt = 0;

		/*========================================================
		 * Reset times in pcs communication buffer -- this lets
		 *  the pcs know that we are about to start ...
		 *=======================================================*/
///    psrtcc->replyTime[0] = 0;
///    psrtcc->replyTime[1] = 0;

    tmoCnt = 0;

    psrtcc->nsubmode = nstx_pcs_gen.nsubmode;
    psrtcc->shotNumber = nstx_pcs_gen.nshot;
    ppc_cache_flush((void *)psrtcc,sizeof(*psrtcc));

    return(1);
   }



	/****************************************************************
	 * plasmacontrol_:
	 ****************************************************************/
int   plasmacontrol_(		/* really a FORTRAN_SUBROUTINE, but...	*/
    int   *npcstatus		/* <w> return status: 1=Success		*/
   )
   {
    int   i;
    int   id;
    int   idx;
    static int   dupFlag;
    static int   dupidx;

		/*========================================================
		 * If pcs_common is not mapped, return immediately.
		 * If the pcs has failed even once (i.e., pcsStatus==0)
		 *  then we *could* return immediately, but we execute the
		 *  function to acquire "duplicates" statistics.
		 *=======================================================*/
    if (!pcsCommon)
       {
        *npcstatus = 0;
        return(1);
       }

		/*=======================================================
		 * The pcs should be updating reqVoltage.  We check
		 *  reqTime to confirm that this is happening.
		 * If we go "too long" without an update, assume that
		 *  the pcs is having problems -- indicate via npcstatus.
		 *======================================================*/
    idx = waitPcsRequest(nstx_pcs_gen.ntdata);
			/* Wait for pcs request following ntdata	*/
    id=36;  profile_update(&id,"plasmacontrol: after waitPcsRequest",0);

    pcsCnt++;				/* count number of times ...	*/
    if (gReqTime != gReqTimeB4)
       {
        union { float fval; int ival;}  *u;

			/*----------------------------------------------
			 * Copy reqVoltage to local buffer, check for
			 *  "Not-a-Number" values ("NaN") ...
			 *---------------------------------------------*/
        u = (void *)nstx_pcs_collect_real.vprtc;
        vmov_sp(pcsc->reqVoltage[idx],1, (float *)u,1, NCOILS);
        for (i=0 ; i<NCOILS ; i++)
           {
            if ((u[i].ival & IEEE_EXPONENT_MASK) == IEEE_EXPONENT_MASK)
                nstx_pcs_collect_integer.nerracq = 1 | RTCWARN_PCS_NAN;
           }
        if (gReqTime < gReqTimeB4)
            errCount++;
        gReqTimeB4 = gReqTime;
        dupFlag = 0;
        if (gReqTime > (nstx_pcs_gen.ntdata+5))
           {
            pcsStatus = 0;		/* Set bad status	*/
            pcsmsg(0,"*WARN*  gReqTime=%d  ntdata=%d\n",
                gReqTime,nstx_pcs_gen.ntdata);
           }
       }
    else
       {
        if (dupFlag)
           {
            duplicates[dupidx].lastTime = nstx_pcs_gen.ntdata;
            duplicates[dupidx].cnt++;
            if (duplicates[dupidx].cnt > DUPLICATES_BEFORE_ERROR)
                pcsStatus = 0;		/* Set bad status	*/
           }
        else
           {
            dupFlag = 1;
            dupidx = dupCount++;
            if (dupidx >= MAXDUP)
                dupidx = MAXDUP-1;
            duplicates[dupidx].reqTime = gReqTimeB4;
            duplicates[dupidx].firstTime = nstx_pcs_gen.ntdata;
            duplicates[dupidx].lastTime = duplicates[dupidx].firstTime;
            duplicates[dupidx].cnt = 1;
           }
       }
    *npcstatus = pcsStatus;

///    if (pcsInfoIdx < MAX_PCS)
///       {
///        struct pcsInfo  *p;
///
///        p = pcsInfo + pcsInfoIdx++;
///        p->reqTime = gReqTime;
///        p->pubTime = nstx_pcs_gen.ntdata;
///       }
    return(1);
   }



	/***************************************************************
	 * pc_setv_:
	 * Set actual output voltages in pcs communication buffer.
	 ***************************************************************/
int   FORTRAN_SUBROUTINE(pc_setv)()
   {
#if 0				/* DISABLED pc_setv	*/
    int   i;
    int   idx;
    int   replyTime;
    float acosTemp[NCOILS];		/* temp buf for acos of a1	*/
    float *replyVdo,*replyVd;
    static int   ic;		/* 1st coil not locked out, for display*/
    static int   firsttime = 1;		/* flag	*/
    static float acos2vdo[NCOILS];	/* convert acos to vdo		*/

    if (1)
        return(1);		//TEMP 09-Jun-2003  TRG
    if (!pcsCommon)
        return(1);

///#undef vmax	/*..because "vmax" is a struct member (SKYvec 5.4.1 only) */
    if (firsttime)
       {			/* initialize acos2vdo array		*/
        firsttime = 0;
        for (i=0 ; i<NCOILS ; i++)
            acos2vdo[i] = nstx_pcs_gen.nplink[i] * nstx_pcs_gen.vmax[i];
        for (ic=0 ; ic<NCOILS && nstx_pcs_gen.nlock[ic] ; ic++)
            ;
        if (ic == NCOILS)
            exit(pcsmsg(0,"pc_setv: *EXIT* all coils are locked out!"));
       }

		/*========================================================
		 * Calculate actual voltage;  write to pcs_common struct
		 *=======================================================*/
    idx = psrtcc->replyIdx ? 0 : 1;
    replyVd = psrtcc->replyVd[idx];
    replyVdo = psrtcc->replyVdo[idx];
    vcos_sp(nstx_pcs_collect_real.a1,1, acosTemp,1, NCOILS);
				/* acosTemp = acos(a1)			*/

    for (i=0 ; i<NCOILS ; i++)
       {
        replyVdo[i] = (float)(acosTemp[i] * acos2vdo[i]);
        replyVd[i] = replyVdo[i] -
            (float)(nstx_pcs_collect_real.iload[i] * nstx_pcs_gen.rps[i]);
       }
    replyTime = (nstx_pcs_gen.ntdata >= pcsc->reqTime[pcsc->reqIdx]) ?
                    nstx_pcs_gen.ntdata : pcsc->reqTime[pcsc->reqIdx];
    psrtcc->replyTime[idx] = replyTime;
    psrtcc->replyIdx = idx;

    if (setvInfoCnt < MAX_SETV)
       {
        setvInfo[setvInfoCnt].setvNtdata = nstx_pcs_gen.ntdata;
        setvInfo[setvInfoCnt].setvReqTime = pcsc->reqTime[pcsc->reqIdx];
        setvInfoCnt++;
       }

    for (i=0 ; i<NCOILS ; i++)
       {
        float diff;
        float vreq;
        static int   diffCnt;

        if (fabs(nstx_pcs_collect_real.v0[i]) < 0.001)
            continue;
        vreq =(float)(nstx_pcs_collect_real.v0[i] * nstx_pcs_gen.nplink[i]);
        diff = fabs(vreq - replyVdo[i]);
        if (diff > 0.05*fabs(vreq))
           {
            if (nstx_pcs_gen.nsubmode && diffCnt++ < 20)
               {
                printf("pc_setv: circ=%d:%-5s t=%.3f  req=%8.3f  replyVdo=%8.3f  diff=%.1f%%\n",
                    i+1,circuitName(i),
                    (float)nstx_pcs_gen.ntdata*FAST_DT + nstx_pcs_gen.tsop,
                     vreq,replyVdo[i],
                    (nstx_pcs_collect_real.v0[i]==0.0) ? 100.0 :
                    diff/nstx_pcs_collect_real.v0[i]*100.0);
               }
           }
       }

    if (nstx_pcs_gen.nsubmode && !(nstx_pcs_gen.ntdata % 1000))
       {
        static int   firsttime=1;

        if (firsttime)
           {
            firsttime = 0;
            printf("\n    %s\n    %s\n",
                "ierr(n) = iref(n)-iload(n)",
                "v0(n) = ierr(n)*gain(n,1,1)+intierr(n)*gain(n,2,1)");
            printf("    gain(n,1,1)=%.3f   gain(n,2,1)=%.3f\n",
                nstx_pcs_gen.gain[0][0][0], nstx_pcs_gen.gain[0][1][0]);
            printf("\n");
           }
        printf("--> pc_setv: t=%6.3f v0[%d]=%-9.3g  iload[%d]=%-9.3g  target=%-9.3g\n",
            (float)nstx_pcs_gen.ntdata*FAST_DT + nstx_pcs_gen.tsop,
            ic,nstx_pcs_collect_real.v0[ic],
            ic,nstx_pcs_collect_real.iload[ic],
            nstx_pcs_collect_real.iref[ic]);
       }
    else if (nstx_pcs_collect_real.iref[0] != 0.0)
       {
        static int   icnt;

        if (icnt++ < 10)
           {
            printf("--> pc_setv: t=%.3f  iref[0]=%6.3f  iload[0]=%6.3f\n",
                (float)nstx_pcs_gen.ntdata*FAST_DT + nstx_pcs_gen.tsop,
                nstx_pcs_collect_real.iref[0],
                nstx_pcs_collect_real.iload[0]);
           }
       }
#endif		/* end of disabled pc_setv	*/

    return(1);
   }



	/***************************************************************
	 * pc_seti_:
	 * Set latest iload in pcs communication buffer.
	 ***************************************************************/
int   FORTRAN_SUBROUTINE(pc_seti)()
   {
    int   idx;
    float *coils;
    float *imeas;
    float nstxTime;
    static int   cnt;

#ifdef _SKY
    if (!sim || !nstx_pcs_gen.nsubmode)
        return(1);			/* return if not simulation mode */

		/*======================================================
		 * Set idx into  data arrays.
		 *=====================================================*/
    idx = sim->u.pubIdx ? 0 : 1;
    sim->u.pubTime[idx] = nstx_pcs_gen.ntdata;
    nstxTime = (float)nstx_pcs_gen.ntdata*FAST_DT + nstx_pcs_gen.tsop;
    sim->u.pubNstxTime[idx] = nstxTime;

		/*========================================================
		 * Copy iload and ipN to computeData struct
		 *=======================================================*/
    coils = sim->computeData[idx].coils;
    vmov_sp(nstx_pcs_collect_real.iload,1, coils,1, NCOILS);

		/*=======================================================
		 * Write fake magnetics data to simulated "pubData".
		 * Set fake ip1 and ip2 in "computeData" struct ...
		 *======================================================*/
    imeas = sim->calBuf[idx];
    cnt++;
    imeas[DIGIDX1_B1DMCSCU3] = (float)(cnt/100)/1.e4;
    imeas[DIGIDX1_B1DMCSCL3] = imeas[DIGIDX1_B1DMCSCU3];
    imeas[DIGIDX1_PSI_OH_M] = (float)(-cnt/100)/1.e3;
    sim->computeData[idx].ip1 = (float)(cnt % 100)*1.e3;
    sim->computeData[idx].ip2 = sim->computeData[idx].ip1;

			// place actual gis data in computeData, for debug
    gisGetCalibAdc(sim->computeData[idx].gisAdc);

			 /* iload in response to this reqTime	*/
    sim->u.pubIdx = idx;

    if (sim->u.sopeopFlag != nstx_pcs_gen.nsopeop)
        sim->u.sopeopFlag = nstx_pcs_gen.nsopeop;

#endif
    return(1);
   }



	/***************************************************************
	 * plasmacontrol_summary_:
	 * End-of-cycle summary of errors or whatever
	 ***************************************************************/
int   FORTRAN_SUBROUTINE(plasmacontrol_summary)()
   {
    int   i,k;
    char  text[256];
    static char  fmt[] =
        "%8d)  firstTime=%7.4f lastTime=%7.4f:%-6d reqTime=%-6d cnt=%d\n";

    printf("\n==> plasmacontrol_summary:\n");
    printf("\n");	//reminder: fortran_printf prefers full lines

    if (tmoCnt)
       {
        struct tmoInfo  *tmo;

        printf("  tmoInfo[]:  %13s %17s %13s  (%d total timeouts)\n",
            "minReqTime","reqTime","Consecutive",tmoCnt);
        for (i=0 ; i<tmoCnt && i<MAXWAIT ; i++)
           {
            tmo = tmoInfo+i;
            printf("       %6d:%8.4f(%6d)  %8.4f(%6d) %5d\n",i+1,
                tmo->minReqTime,tmo->minReqTime*FAST_DT + nstx_pcs_gen.tsop,
                tmo->reqTime,tmo->reqTime*FAST_DT + nstx_pcs_gen.tsop,
                tmo->tmoConsecutive);
           }
        printf("\n");
       }
///    if (setvInfoCnt)
///       {
///        printf("\n==> setvInfo:    ntdata       reqTime/replyTime   (%d)\n",
///            setvInfoCnt);
///        for (i=0 ; i<setvInfoCnt ; i++)
///            printf("   %7d(%7.3f)    %7d(%7.3f)\n",
///                setvInfo[i].setvNtdata,setvInfo[i].setvReqTime);
///       }

		/*=========================================================
		 * Display other information ...
		 *========================================================*/
    if (!dupCount && !errCount)
        return(1);

    printf("  dupCount=%d (MAXDUP=%d)   errCount=%d\n",
        dupCount,MAXDUP,errCount);
    if (dupCount)
       {
        printf("  Times where pcs reqVoltage was a duplicate\n");
        k = (dupCount < MAXDUP) ? dupCount : MAXDUP;
        for (i=0 ; i<k ; i++)
           {
            printf(fmt,i+1,
                (float)duplicates[i].firstTime*FAST_DT + nstx_pcs_gen.tsop,
                (float)duplicates[i].lastTime*FAST_DT + nstx_pcs_gen.tsop,
                duplicates[i].lastTime,
                duplicates[i].reqTime,duplicates[i].cnt);
           }
        if (dupCount > MAXDUP)
            printf("==> Note:  total of %d times\n",dupCount);
       }
    printf("\n");  fflush(stdout);

    printf("pcsInfo[]:  time of requests from pcs  (pcsInfoIdx=%d)\n",pcsInfoIdx);
    for (i=0 ; i<pcsInfoIdx ; i++)
       {
        float t;
        struct pcsInfo  *p = pcsInfo+i;

        t = p->pubTime*FAST_DT + nstx_pcs_gen.tsop;
        printf("%4d: t=%8.4f  reqTime=%-6d  pubTime=%-6d\n",
            i+1,t,p->reqTime,p->pubTime);
       }
    printf("\n");  fflush(stdout);
    return(1);
   }