/* Modified extensively 1986-1989 Timothy L. Davis
 * $Source: /mit/hst/src/sim/RCS/eqns.c,v $
 * $Log:	eqns.c,v $
 * Revision 2.2  90/08/28  14:44:37  tldavis
 * Added function UpdateTiming() to calculate the portion of the cardiac
 * cycle and store it in an unsigned int.
 * 
 * Revision 1.1  90/08/19  16:25:52  tldavis
 * Initial revision
 * 
 * Revision 2.1  90/08/18  18:39:51  tldavis
 * New location of CVDefs in this directory.
 * 
 * Revision 2.0  90/08/06  18:02:38  tldavis
 * Final X11R3 version.
 * 
 * Revision 1.6  90/08/06  17:03:31  tldavis
 * Added a few new dependent variables: atrial pressures and instantaneous
 * ventricular capacitances.
 * 
 * Revision 1.5  90/05/31  16:25:16  tldavis
 * Pre-ACIS checkin.
 * 
 * Revision 1.4  90/02/20  00:01:55  tldavis
 * Fixed ventricular constant volume bug.
 * 
 * Revision 1.3  89/10/12  22:17:01  tldavis
 * Updated Fall, 1989.
 * 
 * Revision 1.2  89/08/21  15:23:03  tldavis
 * Works with baroreflex control in a single process.
 * 
 * Tweaked X11R3 version, for cpoon, with control system features.
 * 
 * Equations within the circulatory system model including:
 *    -	differential KCL equations at each node
 *    -	heart capacitance values as a function of time via a look-up table
 *       (without interpolation).  NOTE: this assumes the diastolic capacitance
 *	is constant, which is only valid at normal ventricular volumes.
 *
 */
#include "CVDefs.h"
#include <stdio.h>
#include <math.h>
#include "../event/Event.h" /* for UpdateParamWindow() */

extern double sigHR, sigR, sigV, sigCl, sigCr; /* from reflex.c */
extern void calcReflexes(); /* reflex.c */

/* 
 * 1/3 of systemic arterial capacitance is in the thorax: A_TH_FRAC
 */
#define A_TH_FRAC 0.333

/* nonstatic variables below are used by reflex.c exclusively */
static double Vl0;	/* left heart volume at 0 pressure */
static double Vr0;	/* right heart volume at 0 pressure */
static double Va0;	/* peripheral arterial volume at 0 pressure */
double Vv0;	/* peripheral venous volume at 0 pressure */
static double VPa0;	/* pulmonary arterial volume at 0 pressure */
static double VPv0;	/* pulmonary venous volume at 0 pressure */
static double bv;	/* total blood volume */
double Ra;	/* peripheral arterial resistance */
static double Rv;	/* peripheral venous resistance */
static double RPv;	/* pulmonary vascular resistance */
static double Rli;	/* left ventricular inflow resistance */
static double Rlo;	/* left ventricular outflow resistance */
static double Rro;	/* right ventricular outflow resistance */
double Ca;	/* peripheral arterial capacitance */
static double Cv;	/* peripheral venous capacitance */
static double CPa;	/* pulmonary arterial capacitance */
static double CPv;	/* pulmonary venous capacitance */
static double Cldias;	/* left heart diastolic capacitance */
double Clsys;	/* left heart systolic capacitance */
static double Crdias;	/* right heart diastolic capacitance */
double Crsys;	/* right heart systolic capacitance */
static double pth;	/* intrathoracic pressure */
double hr;	/* heart rate operating point in beats per minute */
static int DLI;		/* left heart inflow diode */
static int DLO;		/* left heart outflow diode */
static int DRI;		/* right heart inflow diode */
static int DRO;		/* right heart outflow diode */

/* The above variables are the signals used for calculations involving
 * hr, Ra, Vv0, Clsys, and Crsys.  The nominal values of hr, Ra, etc.
 * are used as setpoints for the linear control system if reflexes are
 * on.  If reflexes are off, hr etc. are copied directly into sigHR etc. 
 */

/*PUBLIC*/
void initeqnvars(p)
SIMULATION *p;
{
  Vl0 = p->Vl0;  Vr0 = p->Vr0;  Va0 = p->Va0;  Vv0 = p->Vv0;  
  VPa0 = p->VPa0;  VPv0 = p->VPv0;  bv = p->bv;
  Ra = p->Ra;Rv = p->Rv;RPv = p->RPv;Rli = p->Rli; Rlo = p->Rlo; Rro = p->Rro;
  Ca = p->Ca;  Cv = p->Cv; CPa = p->CPa; CPv = p->CPv;
  Cldias = p->Cldias; Clsys = p->Clsys; Crdias = p->Crdias; Crsys = p->Crsys;
  pth = p->pth;  hr = p->hr;
}

/*PUBLIC*/
SIMULATION *presentsim(p)
SIMULATION *p;
{
  p->Vl0 = Vl0;  p->Vr0 = Vr0;  p->Va0 = Va0;  p->Vv0 = Vv0;  p->VPa0 = VPa0;
  p->VPv0 = VPv0;  p->bv = bv;  p->Ra = Ra;  p->Rv = Rv;  p->RPv = RPv;
  p->Rli = Rli;  p->Rlo = Rlo;  p->Rro = Rro;
  p->Ca = Ca;  p->Cv = Cv;  p->CPa = CPa;  p->CPv = CPv;
  p->Cldias = Cldias;  p->Clsys = Clsys;  p->Crdias = Crdias; p->Crsys = Crsys;
  p->pth = pth;  p->hr = hr;
  return(p);
}

/*PUBLIC*/
void
eqns(dxdt, i, x) /* takes x[] pressures, and time index in systole
		  * then return changes in dxdt[]
		  */
double x[], dxdt[];
int i;
{
	double Cl(), Cr(), dCldt(), dCrdt(), Cinc();
	double q0, q1, q2, q3, q4, q5; /* These are FLOWs */

	q0 = DLI ? (xpPv - xpl) /Rli  : 0.;
	q1 = DLO ? (xpl - xpa)  /Rlo  : 0.;
	q2 =       (xpa - xpv)  /sigR;
	q3 = DRI ? (xpv - xpr)  /Rv   : 0.;
	q4 = DRO ? (xpr - xpPa) /Rro  : 0.;
	q5 =       (xpPa - xpPv)/RPv;
	dpldt = ((pth - xpl)*dCldt(i) + q0 - q1)/Cl(i);
	dpadt = (q1 - q2)/Ca;
	dpvdt = (q2 - q3)/Cv;
	dprdt = ((pth - xpr)*dCrdt(i) + q3 - q4)/Cr(i);
	dpPadt = (q4 - q5)/CPa;
	dpPvdt = (q5 - q0)/CPv;
}

static	struct	ctable         /* Table of heart contraction capacitance */
{
	double	fctr; /* Maximum capacitance for scaling purposes. Min == 0 */
	unsigned long val[TBLSEGSPLUS2];
} ctbl = {
#include "tblc"
};

static	struct	dctable        /* table of heart contraction cap. derivatives*/
{
	double	fctr; /* Not used. Calculated from ctbl.fctr * ds/dt */
	long val[TBLSEGSPLUS2];
} dctbl = {
#include "tbldcdt"
};

static double 
  k2oTsys,     /* time multiplier for Sys. table lookup*/
  kr1,         /* capacitance multiplier for RH systole*/         
  kl1,         /* capacitance multiplier for LH systole*/
  kr2,         /*     "          "        "  RH systole dC/dt */
  kl2;         /*     "          "        "  LH    "      "   */

/*PUBLIC*/
double Cinc(dt)
     double dt;
{
  return (dt * k2oTsys);
}

/*PUBLIC*/
double Cr(i)
     int i;
{
  if (i >= TBLSEGS)  return (Crdias);
  else  return (sigCr + ctbl.val[i] * kr1); /* ctbl.val is LONG! */
}

/*PUBLIC*/
double	Cl(i)
     int i;
{
  if (i >= TBLSEGS) return (Cldias);
  else return (sigCl + ctbl.val[i] * kl1);
}

/*PUBLIC*/
double	dCrdt(i)
     int i;
{
  if (i >= TBLSEGS) return (0.);
  else return (dctbl.val[i] * kr2);
}

/*PUBLIC*/
double dCldt(i)
     int i;
{
  if (i >= TBLSEGS) return (0.);
  else return (dctbl.val[i] * kl2);
}

/*PUBLIC*/
double Tsystole(hrate)
     double hrate;
{
  return(0.30*sqrt(60./hrate));
}

/* 
 * lookup tables for heart capacitance used from 0 <= t <= x1Tsys.
 * k2oTsys is the number of lookup table items per time in seconds.
 * kr1 is scaling factor for right heart capacitance.
 * kr2 is scaling factor for right heart d(capacitance)/dt in cap. per sec.
 * similar for kl1 and kl2.  
 */
/*PUBLIC*/
void btconst() 
{
  double Tsys = Tsystole(sigHR); /* time for contraction (seconds) */; 
  k2oTsys = SYSTOLESEGS/Tsys;  /* number of data file segs per time*/
  kl1 = (Cldias - sigCl)/ctbl.fctr; /* scaling factor for left heart */
  kl2 = kl1 * k2oTsys;  /* Scaling factor for dcdt.  Table is in
			 * dc/ds (per segment).  Convert dc/dt to cap 
			 * per time by chain rule 
			 */
  kr1 = (Crdias - sigCr)/ctbl.fctr; /* scaling factor for right heart */
  kr2 = kr1 * k2oTsys;  /* scaling factor for dcdt on right */
}


/*PUBLIC*/
void initvalves(x)
double x[];
{
	DLI = (xpPv >= xpl);
	DLO = (xpl >= xpa);
	DRI = (xpv >= xpr);
	DRO = (xpr >= xpPa);
}

/*PUBLIC*/
void fixvolume(x)
double x[];
{
      /* find deviation from total blood vol, 
       * correct at systemic venous reservoir 
       */
  double diff = (bv - (Vl0+Va0+sigV+Vr0+VPa0+VPv0) -
		 (xCl*(xpl-pth) + Ca*(xpa-A_TH_FRAC*pth) + Cv*xpv +
		  xCr*(xpr-pth) + CPa*(xpPa-pth) + CPv*(xpPv-pth)))/Cv;
#ifdef DEBUG
  if (fabs(diff) > 1.0)
    printf("Volume error:  %g ml   ",diff);
#endif
  xpv += diff;
}

/*PUBLIC*/
double VenVol(x)
     double x[];
{
  return(xpv * Cv);
}

/* Returns TRUE for valves state within given period if pd1 & pd2 are different
 * Returns TRUE at leading edge of pd1 if pd2 == 0.
 * Sets bit INPERIOD_POS on entrance, clears on exit.
 * FALSE otherwise.
 * BIT CODE:  0, 1: DLI, DLO beginning desired period, immediately following
	      2, 3: the previous DLI, DLO to bits 0, 1.
	      4, 5: DLI, DLO immediately following the end of the periods, with
	      6, 7: DLI, DLO the last valve state in the period.
	      8:    Flag set in this routine 1: in period, 0: not in.
	      9, 10: The current flag settings saved here from last time.
	      11:   1 if averages are being plotted instead of direct data
	      12:   1 at the start of each beat (beginning of diastole)
	      13:   1 during diastole
              14-15: reserved
 * This uses twice as many bits as necessary for the number of states,
 * but it is the best way to do it directly from pressures alone.
 */

/*PUBLIC*/
unsigned int UpdatePeriod(x, flagPtr)
double x[];
unsigned int *flagPtr;
{
#define INPERIOD_POS 0x8
#define PREV_POS 0x9
  unsigned int start = *flagPtr & 0x0F;
  unsigned int end = *flagPtr>>4 & 0x0F;
  unsigned int wasin = *flagPtr & 1<<INPERIOD_POS;
  unsigned int current = (*flagPtr & 3<<PREV_POS) >> (PREV_POS-2);
  
  initvalves(x);
  if (DLI && !(*flagPtr&0x2000)) *flagPtr |= 0x1000; /* Set start cycle flag */
  else *flagPtr &= 0xEFFF; /* Clear start cycle flag */
  if (DLI) *flagPtr |= 0x2000; /* Set (old ) diastole flag */
  else *flagPtr &= ~0x2000;

  if (!start) return TRUE; /* No starting point means all the time */
  current |= DLI | DLO<<1;
  *flagPtr = *flagPtr&0xF9FF | (current&0x3)<<PREV_POS; /* Update flags */
  if (current == start)
    *flagPtr |= 1<<INPERIOD_POS; /* Set the INPERIOD flag */
  else if (current == end)
    *flagPtr &= ~((unsigned int)(1<<INPERIOD_POS)); /* Clear INPERIOD flag */
  if (!end) /* Only return TRUE on up edge */
    return (!wasin && ((*flagPtr)>>INPERIOD_POS & 1)? TRUE : FALSE); 
  else
    return (1 & *flagPtr>>INPERIOD_POS ? TRUE : FALSE); 
}

/*PUBLIC*/
double getval(x, valplace, valtype) /*only returns error (huge number)
				      if totally unreasonable */
     double x[];
     int valplace, valtype;
{
  switch (valtype) {
  case (TIME): return (xt);
  case (PRESSURE): 
     switch (valplace) {
     case (SYSTEM): return(pth);
     case (PC): return (xpPv);
     case (SC): return (xpv);
     default:   return (x[valplace]);
     }
  case (VOLUME): switch (valplace) {
     case (LV): return (Vl0+(xpl-pth)*xCl);
     case (SA): return (Va0+(xpa-A_TH_FRAC*pth)*Ca);
     case (SV): return (sigV+xpv*Cv);
     case (RV): return (Vr0+(xpr-pth)*xCr);
     case (PA): return (VPa0+(xpPa-pth)*CPa);
     case (PV): return (VPv0+(xpPv-pth)*CPv);
     case (PC): return (0.0);
     case (SC): return (0.0);
     case (SYSTEM): return(bv);
     default: return(0.0);
     }
  case (FLOW): switch (valplace) {
     case (LV): 
     case (LVl):
     case (SA): initvalves(x); return (DLO*(xpl-xpa)/Rlo);
     case (SCl):
     case (SC): return ((xpa-xpv)/sigR);
     case (SVl):
     case (SV): initvalves(x); return (DRI*(xpv-xpr)/Rv);
     case (RVl):
     case (RV): 
     case (PA): initvalves(x); return (DRO*(xpr-xpPa)/Rro);
     case (PCl):
     case (PC): return ((xpPa-xpPv)/RPv);
     case (PVl):
     case (PV): initvalves(x); return (DLI*(xpPv-xpl)/Rli);
     case (SYSTEM): return((xpa-xpv)/sigR);
     default: break;
     }
  case (CAPACITANCE): switch(valplace) {
     case (LV): return (xCl);
     case (SA): return(Ca);
     case (SC): return(0.0);
     case (SV): return(Cv);
     case (RV): return (xCr);
     case (PA): return(CPa);
     case (PC): return(0.0);
     case (PV): return(CPv);
     case (SYSTEM): break;
     default: break;
     }   
  case (CAPACITANCE_SYS): switch(valplace) {
     case (LV): return(sigCl);
     case (RV): return(sigCr);
     default: return(getval(x, valplace, CAPACITANCE));
     }
  case (CAPACITANCE_DIAS): switch(valplace) {
     case (LV): return(Cldias);
     case (RV): return(Crdias);
     case (SV): initvalves(x); return(DRI ? xpr : xpv); /* ||| HORRIBLE HACK! Using this Capacitance_dias code for ATRIAL (Inflow ) PRESSURE! */
     case (PV): initvalves(x); return(DLI ? xpl : xpPv);
     default: return(getval(x, valplace, CAPACITANCE));
     }
  case (ZPVOLUME): switch(valplace) {
     case (LV): return(Vl0);
     case (SA): return(Va0);
     case (SC): return(0.0);
     case (SV): return(sigV);
     case (RV): return(Vr0);
     case (PA): return(VPa0);
     case (PC): return(0.0);
     case (PV): return(VPv0);
     case (SYSTEM): return(bv);
     default: return(0.0);
     }
  case (RESISTANCE): switch(valplace) {
     case (LV): return(Rlo);
     case (SA): return(0.0);
     case (SC): return(sigR);
     case (SV): return(Rv);
     case (RV): return(Rro);
     case (PA): return(0.0);
     case (PC): return(RPv);
     case (PV): return(Rli);
     case (SYSTEM): break;
     default: return(0.0);
     }
  case (HEARTRATE): return(sigHR);
  }
  return((double) 0x0ffffffff);
}

static double SetPlaceTypeValue(double x[], int valplace, int valtype, double value);

/*PUBLIC*/
double setval(x, valplace, valtype, value) /* returns value set or 0xffffffff */
     double x[];
     int valtype, valplace;
     double value;
{
  double SetPlaceTypeValue();
  double ret;
/*  if (!var_in_bounds(valtype, valplace, value)) doesn't work yet!
*    return((double)0x0ffffffff);
*/

  SimFlush(); /* in Event.c, to clear data queue of old simulated data. */
  ret = SetPlaceTypeValue(x, valplace, valtype, value);
  UpdateParamWindow();
  return (ret);
}

static double SetPlaceTypeValue(double x[], int valplace, int valtype, double value)
{
  double tmp;
  switch (valtype) {
  case (TIME):
  case (FLOW):
    break;
  case (PRESSURE): switch(valplace) {
    case (SYSTEM): 
      xpl += value - pth;
      xpr += value - pth;
      xpPa += value - pth;
      xpPv += value - pth;
      /* ||| Pa has 1/3 transthoracic pressure on it! */
      xpa += A_TH_FRAC*(value - pth);
      pth = value;
      return(pth);
    default:
      break;
    }
    break;
  case (VOLUME): switch (valplace) {
    case (SYSTEM): 
      bv = value;
      fixvolume(x);
      return(bv);
    default:
      break;
    }
    break;
  case (CAPACITANCE): switch(valplace) {
     case (SA): 
       xpa *= Ca/value;
       return(Ca = value);
     case (SV): 
       xpv *= Cv/value;
       return(Cv = value);
     case (PA): 
       xpPa *= CPa/value;
       return(CPa = value);
     case (PV): 
       xpPv *= CPv/value;
       return(CPv = value);
     case (LV):
     case (RV):
     case (SC):
     case (PC):
     case (SYSTEM): 
       break;
     }   
    break;
   case (CAPACITANCE_SYS): switch(valplace) {
     case (LV): 
       Clsys = value; calcReflexes(); btconst(); 
       tmp = Cl((int) x[9]);
       xpl *= xCl/tmp; xCl = tmp; return(Clsys);
     case (RV): 
       Crsys = value; calcReflexes(); btconst(); 
       tmp = Cr((int) x[9]);
       xpr *= xCr/tmp; xCr = tmp; return(Crsys);
     default: break;
     }
    break;
   case (CAPACITANCE_DIAS): switch(valplace) {
     case (LV): 
       Cldias = value; calcReflexes(); btconst(); 
       tmp = Cl((int) x[9]);
       xpl *= xCl/tmp; xCl = tmp; return(Cldias);
     case (RV): 
       Crdias = value; calcReflexes(); btconst(); 
       tmp = Cr((int) x[9]);
       xpr *= xCr/tmp; xCr = tmp; return(Crdias);
     default: break;
     }
    break;
   case (ZPVOLUME): switch(valplace) {
     case (LV): 
       xpl += (Vl0 - value)/xCl;
       return(Vl0 = value);
     case (SA): 
       xpa += (Va0 - value)/Ca;
       return(Va0 = value);
     case (SV): 
       tmp = sigV; Vv0 = value; calcReflexes();
       xpv += (tmp - sigV)/Cv;
       return(Vv0 = value);
     case (RV): 
       xpr += (Vr0 - value)/xCr;
       return(Vr0 = value);
     case (PA): 
       xpPa = (VPa0 - value)/CPa;
       return(VPa0= value);
     case (PV): 
       xpPv = (VPv0 - value)/CPv;
       return(VPv0 = value);
     case (SC): 
     case (PC):
     case (SYSTEM):
       break;
     }
    break;
   case (RESISTANCE): switch(valplace) {
     case (LV): return(Rlo = value);
     case (SC): 
       Ra = value; 
       calcReflexes();
       return(Ra);
     case (SV): return(Rv = value);
     case (RV): return(Rro = value);
     case (PC): return(RPv = value);
     case (PV): return(Rli = value);
     case (SA):
     case (PA):
     case (SYSTEM): 
       break;
     }
    break;
   case (HEARTRATE): hr = value; return(hr); 
  }
  return((double) 0x0ffffffff); /* error: bad value or location */
  }

