/*
   There are a total of 4 entries in the algebraic variable array.
   There are a total of 2 entries in each of the rate and state variable arrays.
   There are a total of 3 entries in the constant variable array.
 */
/*
 * VOI is t in component main (second).
 * STATES[0] is q_1 in component main (metre).
 * STATES[1] is v_1 in component main (m_per_s).
 * ALGEBRAIC[3] is a_1 in component main (m_per_s2).
 * ALGEBRAIC[0] is u_C in component main (J_per_m).
 * ALGEBRAIC[1] is u_R in component main (J_per_m).
 * ALGEBRAIC[2] is u_L in component main (J_per_m).
 * CONSTANTS[0] is C in component main (m2_per_J).
 * CONSTANTS[1] is R in component main (Js_per_m2).
 * CONSTANTS[2] is L in component main (Js2_per_m2).
 * RATES[0] is d/dt q_1 in component main (metre).
 * RATES[1] is d/dt v_1 in component main (m_per_s).
 */
void
initConsts(double* CONSTANTS, double* RATES, double *STATES)
{
STATES[0] = 1;
STATES[1] = 0;
CONSTANTS[0] = 20;
CONSTANTS[1] = 0.1;
CONSTANTS[2] = 10;
}
void
computeRates(double VOI, double* CONSTANTS, double* RATES, double* STATES, double* ALGEBRAIC)
{
RATES[0] = STATES[1];
ALGEBRAIC[0] = STATES[0]/CONSTANTS[0];
ALGEBRAIC[1] =  STATES[1]*CONSTANTS[1];
rootfind_0(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
rootfind_1(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
RATES[1] = ALGEBRAIC[3];
}
void
computeVariables(double VOI, double* CONSTANTS, double* RATES, double* STATES, double* ALGEBRAIC)
{
ALGEBRAIC[0] = STATES[0]/CONSTANTS[0];
ALGEBRAIC[1] =  STATES[1]*CONSTANTS[1];
}

void objfunc_0(double *p, double *hx, int m, int n, void *adata)
{
  struct rootfind_info* rfi = (struct rootfind_info*)adata;
#define VOI rfi->aVOI
#define CONSTANTS rfi->aCONSTANTS
#define RATES rfi->aRATES
#define STATES rfi->aSTATES
#define ALGEBRAIC rfi->aALGEBRAIC
#define pret rfi->aPRET
  ALGEBRAIC[2] = *p;
  *hx = (ALGEBRAIC[0]) - (- ALGEBRAIC[1] - ALGEBRAIC[2]);
#undef VOI
#undef CONSTANTS
#undef RATES
#undef STATES
#undef ALGEBRAIC
#undef pret
}
void rootfind_0(double VOI, double* CONSTANTS, double* RATES, 
double* STATES, double* ALGEBRAIC, int* pret)
{
  static double val = 0.1;
  double bp, work[LM_DIF_WORKSZ(1, 1)];
  struct rootfind_info rfi;
  rfi.aVOI = VOI;
  rfi.aCONSTANTS = CONSTANTS;
  rfi.aRATES = RATES;
  rfi.aSTATES = STATES;
  rfi.aALGEBRAIC = ALGEBRAIC;
  rfi.aPRET = pret;
  do_levmar(objfunc_0, &val, &bp, work, pret, 1, &rfi);
  ALGEBRAIC[2] = val;
}

void objfunc_1(double *p, double *hx, int m, int n, void *adata)
{
  struct rootfind_info* rfi = (struct rootfind_info*)adata;
#define VOI rfi->aVOI
#define CONSTANTS rfi->aCONSTANTS
#define RATES rfi->aRATES
#define STATES rfi->aSTATES
#define ALGEBRAIC rfi->aALGEBRAIC
#define pret rfi->aPRET
  ALGEBRAIC[3] = *p;
  *hx = (ALGEBRAIC[2]) - ( ALGEBRAIC[3]*CONSTANTS[2]);
#undef VOI
#undef CONSTANTS
#undef RATES
#undef STATES
#undef ALGEBRAIC
#undef pret
}
void rootfind_1(double VOI, double* CONSTANTS, double* RATES, 
double* STATES, double* ALGEBRAIC, int* pret)
{
  static double val = 0.1;
  double bp, work[LM_DIF_WORKSZ(1, 1)];
  struct rootfind_info rfi;
  rfi.aVOI = VOI;
  rfi.aCONSTANTS = CONSTANTS;
  rfi.aRATES = RATES;
  rfi.aSTATES = STATES;
  rfi.aALGEBRAIC = ALGEBRAIC;
  rfi.aPRET = pret;
  do_levmar(objfunc_1, &val, &bp, work, pret, 1, &rfi);
  ALGEBRAIC[3] = val;
}