/*
   There are a total of 10 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 7 entries in the constant variable array.
 */
/*
 * ALGEBRAIC[0] is u_in_e in component Environment (J_per_C).
 * VOI is t in component Environment (second).
 * STATES[0] is v_1_e in component Voice_coil_equations (C_per_s).
 * ALGEBRAIC[8] is a_1_e in component Voice_coil_equations (C_per_s2).
 * STATES[1] is q_C_m in component Voice_coil_equations (metre).
 * STATES[2] is v_2_m in component Voice_coil_equations (m_per_s).
 * ALGEBRAIC[9] is a_2_m in component Voice_coil_equations (m_per_s2).
 * ALGEBRAIC[2] is u_R_e in component Voice_coil_equations (J_per_C).
 * ALGEBRAIC[6] is u_L_e in component Voice_coil_equations (J_per_C).
 * ALGEBRAIC[4] is u_1_e in component Voice_coil_equations (J_per_C).
 * ALGEBRAIC[1] is u_2_m in component Voice_coil_equations (J_per_m).
 * ALGEBRAIC[3] is u_C_m in component Voice_coil_equations (J_per_m).
 * ALGEBRAIC[5] is u_R_m in component Voice_coil_equations (J_per_m).
 * ALGEBRAIC[7] is u_L_m in component Voice_coil_equations (J_per_m).
 * CONSTANTS[0] is E_1 in component Voice_coil_equations (J_per_C2).
 * CONSTANTS[1] is E_2 in component Voice_coil_equations (J_per_m2).
 * CONSTANTS[2] is R_1_e in component Voice_coil_equations (Js_per_C2).
 * CONSTANTS[3] is R_2_m in component Voice_coil_equations (Js_per_m2).
 * CONSTANTS[4] is L_1_e in component Voice_coil_equations (Js2_per_C2).
 * CONSTANTS[5] is L_2_m in component Voice_coil_equations (Js2_per_m2).
 * CONSTANTS[6] is Bl in component Voice_coil_equations (Js_per_C_m).
 * RATES[0] is d/dt v_1_e in component Voice_coil_equations (C_per_s).
 * RATES[1] is d/dt q_C_m in component Voice_coil_equations (metre).
 * RATES[2] is d/dt v_2_m in component Voice_coil_equations (m_per_s).
 */
void
initConsts(double* CONSTANTS, double* RATES, double *STATES)
{
STATES[0] = 0;
STATES[1] = 0;
STATES[2] = 0;
CONSTANTS[0] = 1;
CONSTANTS[1] = 100;
CONSTANTS[2] = 5;
CONSTANTS[3] = 0.4;
CONSTANTS[4] = 0.2;
CONSTANTS[5] = 0.01;
CONSTANTS[6] = 6;
}
void
computeRates(double VOI, double* CONSTANTS, double* RATES, double* STATES, double* ALGEBRAIC)
{
RATES[1] = STATES[2];
ALGEBRAIC[0] =  50.0000* sin( 50.0000*2.00000* 3.14159265358979*VOI);
ALGEBRAIC[2] =  CONSTANTS[2]*STATES[0];
ALGEBRAIC[4] =  CONSTANTS[6]*STATES[2];
rootfind_0(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
rootfind_1(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
RATES[0] = ALGEBRAIC[8];
ALGEBRAIC[1] =  CONSTANTS[6]*STATES[0];
ALGEBRAIC[3] =  CONSTANTS[1]*STATES[1];
ALGEBRAIC[5] =  CONSTANTS[3]*STATES[2];
rootfind_2(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
rootfind_3(VOI, CONSTANTS, RATES, STATES, ALGEBRAIC, pret);
RATES[2] = ALGEBRAIC[9];
}
void
computeVariables(double VOI, double* CONSTANTS, double* RATES, double* STATES, double* ALGEBRAIC)
{
ALGEBRAIC[0] =  50.0000* sin( 50.0000*2.00000* 3.14159265358979*VOI);
ALGEBRAIC[2] =  CONSTANTS[2]*STATES[0];
ALGEBRAIC[4] =  CONSTANTS[6]*STATES[2];
ALGEBRAIC[1] =  CONSTANTS[6]*STATES[0];
ALGEBRAIC[3] =  CONSTANTS[1]*STATES[1];
ALGEBRAIC[5] =  CONSTANTS[3]*STATES[2];
}

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

void objfunc_2(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[1]) - (ALGEBRAIC[3]+ALGEBRAIC[5]+ALGEBRAIC[7]);
#undef VOI
#undef CONSTANTS
#undef RATES
#undef STATES
#undef ALGEBRAIC
#undef pret
}
void rootfind_2(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_2, &val, &bp, work, pret, 1, &rfi);
  ALGEBRAIC[7] = val;
}

void objfunc_3(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[9] = *p;
  *hx = (ALGEBRAIC[7]) - ( CONSTANTS[5]*ALGEBRAIC[9]);
#undef VOI
#undef CONSTANTS
#undef RATES
#undef STATES
#undef ALGEBRAIC
#undef pret
}
void rootfind_3(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_3, &val, &bp, work, pret, 1, &rfi);
  ALGEBRAIC[9] = val;
}