/*
   There are a total of 13 entries in the algebraic variable array.
   There are a total of 3 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_C in component main (coulomb).
 * ALGEBRAIC[2] is v_C in component main (C_per_s).
 * ALGEBRAIC[0] is v_R in component main (C_per_s).
 * STATES[1] is v_L in component main (C_per_s).
 * ALGEBRAIC[8] is a_L in component main (C_per_s2).
 * ALGEBRAIC[4] is u_C in component main (J_per_C).
 * ALGEBRAIC[6] is u_R in component main (J_per_C).
 * ALGEBRAIC[7] is u_L in component main (J_per_C).
 * CONSTANTS[0] is C in component main (C2_per_J).
 * CONSTANTS[1] is R in component main (Js_per_C2).
 * CONSTANTS[2] is L in component main (Js2_per_C2).
 * ALGEBRAIC[9] is P_C in component main (watt).
 * ALGEBRAIC[10] is P_R in component main (watt).
 * ALGEBRAIC[11] is P_L in component main (watt).
 * ALGEBRAIC[12] is P_tot in component main (watt).
 * ALGEBRAIC[1] is E_C in component main (joule).
 * STATES[2] is E_R in component main (joule).
 * ALGEBRAIC[3] is E_L in component main (joule).
 * ALGEBRAIC[5] is E_tot in component main (joule).
 * RATES[1] is d/dt v_L in component main (C_per_s).
 * RATES[0] is d/dt q_C in component main (coulomb).
 * RATES[2] is d/dt E_R in component main (joule).
 */
void
initConsts(double* CONSTANTS, double* RATES, double *STATES)
{
STATES[0] = 1;
STATES[1] = 0;
CONSTANTS[0] = 20;
CONSTANTS[1] = 2;
CONSTANTS[2] = 10;
STATES[2] = 0;
}
void
computeRates(double VOI, double* CONSTANTS, double* RATES, double* STATES, double* ALGEBRAIC)
{
ALGEBRAIC[0] = STATES[1];
ALGEBRAIC[2] = - ALGEBRAIC[0];
RATES[0] = ALGEBRAIC[2];
ALGEBRAIC[4] = STATES[0]/CONSTANTS[0];
ALGEBRAIC[6] =  ALGEBRAIC[0]*CONSTANTS[1];
rootfind_0(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
rootfind_1(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
RATES[1] = ALGEBRAIC[8];
ALGEBRAIC[10] =  ALGEBRAIC[6]*ALGEBRAIC[0];
RATES[2] = ALGEBRAIC[10];
}
void
computeVariables(double VOI, double* CONSTANTS, double* RATES, double* STATES, double* ALGEBRAIC)
{
ALGEBRAIC[0] = STATES[1];
ALGEBRAIC[2] = - ALGEBRAIC[0];
ALGEBRAIC[4] = STATES[0]/CONSTANTS[0];
ALGEBRAIC[6] =  ALGEBRAIC[0]*CONSTANTS[1];
ALGEBRAIC[10] =  ALGEBRAIC[6]*ALGEBRAIC[0];
ALGEBRAIC[1] =  (( 0.500000*1.00000)/CONSTANTS[0])*pow(STATES[0], 2.00000);
ALGEBRAIC[3] =  0.500000*CONSTANTS[2]*pow(STATES[1], 2.00000);
ALGEBRAIC[5] = ALGEBRAIC[1]+ALGEBRAIC[3]+STATES[2];
ALGEBRAIC[9] =  ALGEBRAIC[4]*ALGEBRAIC[2];
ALGEBRAIC[11] =  ALGEBRAIC[7]*STATES[1];
ALGEBRAIC[12] = ALGEBRAIC[9]+ALGEBRAIC[10]+ALGEBRAIC[11];
}

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[7] = *p;
  *hx = (ALGEBRAIC[4]) - (ALGEBRAIC[6]+ALGEBRAIC[7]);
#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[7] = 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[8] = *p;
  *hx = (ALGEBRAIC[7]) - ( ALGEBRAIC[8]*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[8] = val;
}