

    //Status: Removed all unnecessary code. Builds OK.
    //        TODO: Run it and compare  results with paper on the WEB.




typedef enum {
  SRC_FUNC_NULL = -1,
  SRC_FUNC_TriangleWave,
  SRC_FUNC_PwmA,
  SRC_FUNC_PwmB,
  SRC_FUNC_PwmC,

} SRC_FUNCTION;


typedef enum {
  ODE_FUNC_NULL = -1,
  ODE_FUNC_Ia_Ib_Ic_1,
  ODE_FUNC_Ia_Ib_Ic_2,
  ODE_FUNC_Ia_Ib_Ic_3,
  ODE_FUNC_Omega,
  ODE_FUNC_Theta,
  ODE_FUNC_Probe,
  
} ODE_FUNCTION;

//(NOTE: As of 6/12/08, "pTranslationList" and "pTrajectoryList" CTRL's have been 
//       deprecated and replaced with a "group" structure. However, we still maintain
//       only one "CTRL_FUNCTION" enumeration for all groups)
typedef enum {
  CTRL_FUNC_NULL = -1,
  CTRL_FUNC_RefGen,
  CTRL_FUNC_VDqCmd,


} CTRL_FUNCTION;

typedef enum {
  COEF_FUNC_NULL = -1,
} COEF_FUNCTION;



typedef enum {
  SWITCH_FUNC_NULL = -1,

} SWITCH_FUNCTION;

typedef enum {
  SPICE_FUNC_NULL = -1,

} SPICE_FUNCTION;



#include "../Simulation/Simulation.hpp" 


     // un-comment to plot all probes and object outputs.
//#define RECORD_ALL_PROBES


//Parameters....

#define Mab  .495e-3          
#define Mbc  Mab
#define Mac  Mab

#define Lm_a  1.0e-3               
#define Lm_b  Lm_a
#define Lm_c  Lm_a

#define Ra 1.0
#define Rb Ra
#define Rc Ra

#define Ka .1  //N*m/amp
#define Kb Ka
#define Kc Ka

#define Bm   5e-3   // MEA: All tests before Neuro, this was  5e-5   //N*m*sec/rad

#define Cm  0    //.005 //N*m

#define Dm  0    //.001  //N*m

#define Jm  .002  //N*m*sec**2/rad

#define Nr  6

//MEA: Added for Neuro test. This disturbance is outside the reference model.
#define Dm_external  0   // 2.5  (use this value)


#define MIN_PWM_FREQ_SCALE    1.0
#define MAX_PWM_FREQ_SCALE    10.0

      //Value is such such that "PwmFreqScale" equals MIN_PWM_FREQ_SCALE  when "vcmd_mag" obtains a value >= "PWM_FREQ_GAIN * v_bus"
#define PWM_FREQ_GAIN  .2            // (.000010 / (PWM_CYCLE_QUANTUM_CNT * QUANTUM_PERIOD)) //*** Original test ***


// These modulus based on "QUARTER_PWM_CYCLE" time below
#define REF_GEN_QUANTUM_CNT       4   //For Neuro test, RefGen now runs at 50 uSec.
#define VDQ_CMD_QUANTUM_CNT       4
#define PWM_CYCLE_QUANTUM_CNT     4

#define QUANTUM_PERIOD .0000125   



 
    

#define QUARTER_PWM_CYCLE QUANTUM_PERIOD

     




   

#define SECTOR_ANGLE   (PI / 3.0)
#define T_Z            (QUARTER_PWM_CYCLE * (double) PWM_CYCLE_QUANTUM_CNT)
#define SQRT_3          1.7320508075688772935274463415059



#define  RT_TRAJ     .15
#define  CT_TRAJ     .15
#define  VO_TRAJ   100.0


#define NEURO_RUN_MODE




           //uncomment if triangle wave modulation or linear mode is to be used.
//#define TRIANGLE_REF_MODE
#ifdef TRIANGLE_REF_MODE
          //uncoment if linear mode....
//#define LINEAR_AMP_MODE 
         
#define PWM_GAIN    5.0
#define PWM_OFFSET  0

#else
// "Space Vector" mode....
#define PWM_GAIN    T_Z
#define PWM_OFFSET  (.5*T_Z)

#endif 
 










#define ENCODER_RESOLUTION  4000   //steps/rev   

#define DC_BUS_VOLTAGE 200.0

#define SRC_SLEW_RATE  200000000.0   //volts/sec   (At 200 VDC bus, this is 1 uSec rise and fall time typical for a 30 amp IPM)

#define PI   3.1415926535897932384626433832795

//Misc. definitions...

typedef enum {TIME_NONE,
	      TIME_T1,
	      TIME_T2,
	      TIME_T1_T2
} REFERENCE_CONSTRUCT;








// **** SrcObject Classes ********************************

// ---- TriangleWave ---------------------------------------

class TriangleWave : public SrcObject
{
public:
  virtual void SrcFunction(double t);
  virtual void OdeRValueUpdate(void);
  TriangleWave(void);
  ~TriangleWave(void);
  double t_mod;
  double t_prev;
  double PwmRampDir;
  double PwmFreqScale;



};


TriangleWave Inst_TriangleWave;


// --------------------------------------------------------

// ---- PwmA  ---------------------------------------------

class PwmA : public SrcObject
{
public:
  virtual void SrcFunction(double t);
  virtual void OdeRValueUpdate(void);
  PwmA(void);
  ~PwmA(void);
  //source for this ODE
  double TriAngWave;
  double V_xo;   //"x" is equal to "r", "y", or "b" depending on the sector of operation.

 
};



PwmA Inst_PwmA;

// -------------------------------------------------------------

// ---- PwmB  ---------------------------------------------

class PwmB : public SrcObject
{
public:
  virtual void SrcFunction(double t);
  virtual void OdeRValueUpdate(void);
  PwmB(void);
  ~PwmB(void);
  //source for this ODE
  double TriAngWave;
  double V_xo;   //"x" is equal to "r", "y", or "b" depending on the sector of operation.

};



PwmB Inst_PwmB;

// -------------------------------------------------------------

// ---- PwmC  ---------------------------------------------

class PwmC : public SrcObject
{
public:
  virtual void SrcFunction(double t);
  virtual void OdeRValueUpdate(void);
  PwmC(void);
  ~PwmC(void);
  //source for this ODE
  double TriAngWave;
  double V_xo;   //"x" is equal to "r", "y", or "b" depending on the sector of operation.

 

};



PwmC Inst_PwmC;

// -------------------------------------------------------------


SrcObject * SrcObjectList[] = {(SrcObject *) &Inst_TriangleWave,
			       (SrcObject *) &Inst_PwmA,
			       (SrcObject *) &Inst_PwmB,
			       (SrcObject *) &Inst_PwmC,
			       0};


// ***********************************************************

// **** OdeObject Classes ********************************



// ---- Ia_Ib_Ic_1 -----------------------------------------

class Ia_Ib_Ic_1 : public OdeObject
{
public:
  virtual double OdeFunction(double y, double t);
  virtual void PostOdeFunction(double t);
  virtual void OdeRValueUpdate(void);
  virtual void CtrlRValueUpdate(void);
  virtual void OdeGroupMSolve(double dydt[],  double dmdt[]);
  //we use some probes in this object to get a better understanding
  //of what is going on.
  virtual void RecordProbes(void);
  virtual void PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[]);
  Ia_Ib_Ic_1(void);
  ~Ia_Ib_Ic_1(void);
  //source for this ODE
  double vs_a;
  double pwm_sig;
  double ia;
  double omega;
  double theta;
  double v_n;
  int PositionFeedback;    //(this will ultimately to "FSFCtrl")
  int VelocityFeedback;    //(this will ultimately to "FSFCtrl")
  //storage for probes...
  vector<double> Cemf_a;   //for expression "Ka*omega*sin(Nr*theta)"
  vector<double> VBus_a;   //"vs_a" (processed DC_BUS_VOLTAGE or 0 depending on "pwm_sig")
  vector<double> VRa;      //for expression "Ra*ia"
  vector<double> Theta_enc;     //quantitized motor position based on ENCODER_RESOLUTION (rad).
  vector<double> Omega_enc;     //quantitized motor velocity based on ENCODER_RESOLUTION (rad/sec).
};

Ia_Ib_Ic_1 Inst_Ia_Ib_Ic_1;


// --------------------------------------------------------

// ---- Ia_Ib_Ic_2 -----------------------------------------

class Ia_Ib_Ic_2 : public OdeObject
{
public:
  virtual double OdeFunction(double y, double t);
  virtual void OdeRValueUpdate(void);
  virtual void CtrlRValueUpdate(void);
  virtual void RecordProbes(void);
  virtual void PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[]);
  Ia_Ib_Ic_2(void);
  ~Ia_Ib_Ic_2(void);
  //source for this ODE
  double vs_b;
  double pwm_sig;
  double ib;
  double omega;
  double theta;
  double v_n;
  //storage for probes...   
  vector<double> Cemf_b;   //for expression "Kb*omega*sin(Nr*theta - 2*PI/3)"
  vector<double> VBus_b;   //"vs_b" (processed DC_BUS_VOLTAGE or 0 depending on "pwm_sig")
  vector<double> VRb;      //for expression "Rb*ib"
};

Ia_Ib_Ic_2 Inst_Ia_Ib_Ic_2;

  


// --------------------------------------------------------


// ---- Ia_Ib_Ic_3 -----------------------------------------

class Ia_Ib_Ic_3 : public OdeObject
{
public:
  virtual double OdeFunction(double y, double t);
  virtual void OdeRValueUpdate(void);
  virtual void CtrlRValueUpdate(void);
  virtual void RecordProbes(void);
  virtual void PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[]);
  Ia_Ib_Ic_3(void);
  ~Ia_Ib_Ic_3(void);
  //source for this ODE
  double vs_c;
  double pwm_sig;
  double ic;
  double omega;
  double theta;
  double v_n;
  //storage for probes...
  vector<double> Cemf_c;   //for expression "Kc*omega*sin(Nr*theta - 4*PI/3)"
  vector<double> VBus_c;   //"vs_c" (processed DC_BUS_VOLTAGE or 0 depending on "pwm_sig")
  vector<double> VRc;      //for expression "Rc*ic"
};

Ia_Ib_Ic_3 Inst_Ia_Ib_Ic_3;


// --------------------------------------------------------


// ---- Omega ---------------------------------------------

class Omega : public OdeObject
{
public:
  virtual double OdeFunction(double y, double t);
  virtual void OdeRValueUpdate(void);
  virtual void CtrlRValueUpdate(void);
  Omega(void);
  ~Omega(void);
  //source for this ODE
  double ia;
  double ib;
  double ic;
  double theta;



};

Omega Inst_Omega;


// --------------------------------------------------------

// ---- Theta ---------------------------------------------

class Theta : public OdeObject
{
public:
  virtual double OdeFunction(double y, double t);
  virtual void OdeRValueUpdate(void);
  virtual void CtrlRValueUpdate(void);
  Theta(void);
  ~Theta(void);
  //source for this ODE
  double omega;

};

Theta Inst_Theta;

// --------------------------------------------------------

// ---- Probe ---------------------------------------------

class Probe : public OdeObject
{
public:
  //(No OdeFunction() here, we are just using this to sync.data for probes.
  //The variable "v_n" is the problem here. It is calculated in a "group solve").
  virtual void RecordProbes(void);
  virtual void PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[]);
  Probe(void);
  ~Probe(void);
  //soure for this ODE
  double ia;
  double ib;
  double ic;
  double vs_a;
  double vs_b;
  double vs_c;
  double v_n;
  double omega;
  double theta;
  //storage for probes...
  vector<double> Vcm_a;    //for expression  "vs_a - Ra*ia - Ka*omega*sin(Nr*theta) - v_n"        
  vector<double> Vcm_b;    //for expression  "vs_b - Rb*ib - Kb*omega*sin(Nr*theta - 2*PI/3) - v_n"     
  vector<double> Vcm_c;    //for expression  "vs_c - Rc*ic - Kc*omega*sin(Nr*theta - 4*PI/3) - v_n" 
  vector<double> V_n;       //for the instantanious neutral voltage "v_n"
};

Probe Inst_Probe;

// --------------------------------------------------------




OdeObject * OdeObjectList[] = {(OdeObject *) &Inst_Ia_Ib_Ic_1, //the next three are in a group and must be maintained in this order.
			       (OdeObject *) &Inst_Ia_Ib_Ic_2,
			       (OdeObject *) &Inst_Ia_Ib_Ic_3,
			       (OdeObject *) &Inst_Omega,
			       (OdeObject *) &Inst_Theta,
			       (OdeObject *) &Inst_Probe,
			       0};


// ***********************************************************



// **** CtrlObject Classes **********************************




// ---- VDqCmd  ---------------------------------------------

class VDqCmd : public CtrlObject
{
public:
  virtual void CtrlFunction(double t);
  virtual void OdeRValueUpdate(void);
  virtual void CtrlRValueUpdate(void);
  virtual void RecordProbes(void);
  virtual void PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[]);
  VDqCmd(void);
  ~VDqCmd(void);
  void SpaceVectorControl(void);
  //source for this CTRL
  double V_xo[3];
  double theta;
  double omega;    
  double alpha;    
  double vdd;
  double vqd;
     // Commands for magnitude and angle in the rotating D/Q plane. (These
     // commands ultimately come from another CTRL that is not part
     // of this simulation).
  double vcmd_mag;
  double vcmd_ang;
     // This signal is ultimate feed by a SRC or ODE that simulates the DC bus with
     // ripple voltage. For now, it will be set to a constant.
  double v_bus;
     // This table used to construct the reference commands for each phase.
  REFERENCE_CONSTRUCT SVTable[6][3];  
     // Set to FALSE if operation is SVPWM "clamped" mode (minus side).
  bool HighSpeedSVPwm;
  //intermediary varables need for debugging only.
  double V_xo_debug[3];
  double PwmFreqScale;
  //storage for probes..
  vector<double> VCmd_a;  //value of V_xo[0].
  vector<double> VCmd_b;  //value of V_xo[1].
  vector<double> VCmd_c;  //value of V_xo[2].
  vector<double> VCmd_mag; 
  vector<double> VCmd_ang; 
  vector<double> FreqScale;
};



VDqCmd Inst_VDqCmd;

// -------------------------------------------------------------




// ---- RefGen  ---------------------------------------------

class RefGen : public CtrlObject
{

public:
  virtual void CtrlFunction(double t);
  virtual void CtrlRValueUpdate(void);
  virtual void RecordProbes(void);
  virtual void PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[]);
  double sgn(double omegad);
  double impulse(double sgn_omegad);
  RefGen(void);
  ~RefGen(void);
  //source for this CTRL
  double betad;
  double alphad;
  double omegad;
  double thetad;
  double zetad;
  double idd;
  double iqd;
  double didd;
  double diqd;
  double vdd;
  double vqd;
  //storage for probes..
  vector<double> beta_d;
  vector<double> alpha_d;
  vector<double> omega_d;
  vector<double> theta_d;
  vector<double> zeta_d;
  vector<double> id_d;
  vector<double> iq_d;
  vector<double> did_d;
  vector<double> diq_d;
  vector<double> vd_d;
  vector<double> vq_d;  

  //source for this CTRL
  double omega;
  double theta;
  double id;
  double iq;
  //storage for probes..
  vector<double> omega_a;
  vector<double> theta_a;
  vector<double> id_a;
  vector<double> iq_a;

  double ia;
  double ib;
  double ic;

  //   https://github.com/codeplea/genann/blob/master/README.md
  genann * ann;

  	  //Used for training and prediction
  double ann_in[10];
  	  //Use for training only.
  double ann_train[10];
  	  //Used for prediction only.
  double const * ann_pred;
  double ann_pred_buf[10];
  	      //storage for probes...
  vector<double> theta_pred_diff_a;
  vector<double> omega_pred_diff_a;
  vector<double>iq_pred_diff_a;


  int NumOfNeurons;
};








RefGen Inst_RefGen;

// -------------------------------------------------------------





//(NOTE: As of 6/12/08, "pTranslationList" and "pTrajectoryList" CTRL's have been 
//       deprecated and replaced with a "group" structure. However, we still maintain
//       only one "CtrlObjectList[]" array for all groups, but we add a "CtrlObjGroupStatus[]"
//       array to specify the "quantum" value for each CTRL object)

CtrlObject * CtrlObjectList[] = {(CtrlObject *) &Inst_RefGen,
                                 (CtrlObject *) &Inst_VDqCmd,
				 0};
        //This array contains the value of the execution quantium for each CtrlObject listed above
        //The order of this list must be relative to the order of the "CtrlObjectList[]" above.
        //CtrlObjects with the same quantum values may be considered a "group" of CtrlObjects.
        //However, there is no requirement that these CtrlObjects be related to each other.
      
int CtrlObjectQuantum[] = { REF_GEN_QUANTUM_CNT,
                            VDQ_CMD_QUANTUM_CNT,
                          };

// **************************************************************











// **** SrcObject Functions **************************************

// ---- TriangleWave  ----------------------------------------------------

TriangleWave::TriangleWave(void)
{
 
  SrcObjItem * pCurSrcItem;


  SrcFuncName = SRC_FUNC_TriangleWave;
  LiteralName = "TriangleWave";


  //build the SRC Rvalue list.

  pSrcObjRValList = new SrcObjItem;
  pCurSrcItem = pSrcObjRValList;

  pCurSrcItem->pSrcObject = SrcObjectList[SRC_FUNC_PwmA];
  pCurSrcItem->pNextSrcItem = new SrcObjItem;
  pCurSrcItem = pCurSrcItem->pNextSrcItem;
  pCurSrcItem->pSrcObject = SrcObjectList[SRC_FUNC_PwmB];
  pCurSrcItem->pNextSrcItem = new SrcObjItem;
  pCurSrcItem = pCurSrcItem->pNextSrcItem;
  pCurSrcItem->pSrcObject = SrcObjectList[SRC_FUNC_PwmC];





  PwmRampDir = 1.0;
#ifdef RECORD_ALL_PROBES
  PlotThisOutput = TRUE;
#endif 
  Plot_Tag = "TriangleWave";
}

TriangleWave::~TriangleWave(void)
{



}

void TriangleWave::SrcFunction(double t)
{
  t_mod += (t - t_prev);
  t_prev = t;

  if(t_mod > QUARTER_PWM_CYCLE){
    t_mod -=  QUARTER_PWM_CYCLE;
    PwmRampDir *= -1.0;
  }
 
  y = PWM_GAIN*(t_mod/QUARTER_PWM_CYCLE - .5)*PwmRampDir + PWM_OFFSET;

}

void TriangleWave::OdeRValueUpdate(void)         //!!!!! Pwm's should be in SrcRValueUpdate() (should not change operation) !!!!!!1
{
  SrcObjItem * pCurSrcItem;
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurSrcItem = pSrcObjRValList;

  ((PwmA *) pCurSrcItem->pSrcObject)->TriAngWave = y;
  pCurSrcItem = pCurSrcItem->pNextSrcItem;    
  ((PwmB *) pCurSrcItem->pSrcObject)->TriAngWave = y;
  pCurSrcItem = pCurSrcItem->pNextSrcItem;    
  ((PwmC *) pCurSrcItem->pSrcObject)->TriAngWave = y;

 

}




// -----------------------------------------------------------------

// ---- PwmA ---------------------------------------------------

PwmA::PwmA(void)
{
  OdeObjItem * pCurOdeItem;

  SrcFuncName = SRC_FUNC_PwmA;
  LiteralName = "PwmA";
  
  //build the ODE Rvalue list.

  pOdeObjRValList = new OdeObjItem;
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_1];  

}

PwmA::~PwmA(void)
{



}

void PwmA::SrcFunction(double t)
{



  //For simulation, "V_xo" is space vector reference to a triangle wave
  //running between 0 and PWM_GAIN.
  y = V_xo > TriAngWave ? 1.0 : 0;




}

void PwmA::OdeRValueUpdate(void)
{


  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

#ifndef LINEAR_AMP_MODE

  ((Ia_Ib_Ic_1 *) pCurOdeItem->pOdeObject)->pwm_sig = y;

#endif 

}


// -----------------------------------------------------------------


// ---- PwmB ---------------------------------------------------


PwmB::PwmB(void)
{
  OdeObjItem * pCurOdeItem;

  SrcFuncName = SRC_FUNC_PwmB;
  LiteralName = "PwmB";
  
  //build the ODE Rvalue list.

  pOdeObjRValList = new OdeObjItem;
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_2];  

}


PwmB::~PwmB(void)
{


}


void PwmB::SrcFunction(double t)
{



  //For simulation, "V_xo" is space vector reference to a triangle wave
  //running between 0 and PWM_GAIN.
  y = V_xo > TriAngWave ? 1.0 : 0;





}

void PwmB::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

#ifndef LINEAR_AMP_MODE

  ((Ia_Ib_Ic_2 *) pCurOdeItem->pOdeObject)->pwm_sig = y;

#endif 


}

// ----------------------------------------------------------------- 


// ---- PwmC ---------------------------------------------------

PwmC::PwmC(void)
{
  OdeObjItem * pCurOdeItem;

  SrcFuncName = SRC_FUNC_PwmC;
  LiteralName = "PwmC";
  
  //build the ODE Rvalue list.

  pOdeObjRValList = new OdeObjItem;
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_3];  

}

PwmC::~PwmC(void)
{


}


void PwmC::SrcFunction(double t)
{



  //For simulation, "V_xo" is space vector reference to a triangle wave
  //running between 0 and PWM_GAIN.
  y = V_xo > TriAngWave ? 1.0 : 0;




}

void PwmC::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

#ifndef LINEAR_AMP_MODE

  ((Ia_Ib_Ic_3 *) pCurOdeItem->pOdeObject)->pwm_sig = y;

#endif 


}

// ----------------------------------------------------------------- 






// ***************************************************************


// **** OdeObject Functions **************************************


// ---- Ia_Ib_Ic_1  ----------------------------------------------------

Ia_Ib_Ic_1::Ia_Ib_Ic_1(void)
{
  OdeObjItem * pCurOdeItem;
  CtrlObjItem * pCurCtrlItem;

  OdeFuncName = ODE_FUNC_Ia_Ib_Ic_1;
  LiteralName = "Ia_Ib_Ic_1"; 
  

  //this object marks the beginning of a "group solve" of
  //three objects.
  GroupSolve = 1;
  NumberOfGrpOdes = 3;

  //build the ODE Rvalue list.
 
  pOdeObjRValList = new OdeObjItem;
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Omega];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];

  //build the CTRL Rvalue list

  pCtrlObjRValList = new CtrlObjItem;
  pCurCtrlItem = pCtrlObjRValList;

  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_RefGen];


  pwm_sig = 0;
  ia = 0;
 

//  PlotThisOutput = TRUE;
  Plot_Tag = "Ia";
  DoProbes = TRUE;
  
}

Ia_Ib_Ic_1::~Ia_Ib_Ic_1(void)
{

}

double Ia_Ib_Ic_1::OdeFunction(double y, double t)
{

  //Note: This is within a "group" ODE so we are really returning for "dm/dt + v_n"

          //"vs_a - Ra*ia + Ka*omega*sin(Nr*theta)" 
#ifdef LINEAR_AMP_MODE
     //generating the voltage command directly through CtrlObject "VDqCmd" 
  vs_a = pwm_sig + .5*DC_BUS_VOLTAGE;
#else
  vs_a = ShapeSquareWaveSource(((pwm_sig > 0) ? DC_BUS_VOLTAGE : 0), SRC_SLEW_RATE ,t);
#endif 
  //(Note: "v_n" is really part of the LValue when we are doing the calculations).
  return(vs_a - Ra*y + Ka*omega*sin(Nr*theta));
}

void Ia_Ib_Ic_1::PostOdeFunction(double t)
{
  //un-comment this to allow encoder single to generated.
  //#define GENERATE_ENCODER_FEEDBACK
#ifdef GENERATE_ENCODER_FEEDBACK

  PositionFeedback = (int) (theta/(2*PI) * ENCODER_RESOLUTION);
  VelocityFeedback = (int) (omega/(2*PI) * ENCODER_RESOLUTION);
#endif 

}


void Ia_Ib_Ic_1::RecordProbes(void)
{
//  VBus_a.push_back(vs_a);
#ifdef RECORD_ALL_PROBES
  Cemf_a.push_back(Ka*omega*sin(Nr*theta));
  //"Ra*ia"
  VRa.push_back(Ra*y); 
#ifdef GENERATE_ENCODER_FEEDBACK
  Theta_enc.push_back((double) PositionFeedback * 2 * PI / ENCODER_RESOLUTION);
  Omega_enc.push_back((double) VelocityFeedback * 2 * PI / ENCODER_RESOLUTION);
#endif 
#endif 

}

void Ia_Ib_Ic_1::PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[])
{
  int i;
  if(TagNamesToPlot[0] == ""){
    SimuPlot.plot_xy(Plot_t, Cemf_a, "Cemf_a");
    SimuPlot.plot_xy(Plot_t, VBus_a, "VBus_a");
    SimuPlot.plot_xy(Plot_t, VRa, "VRa");  
#ifdef GENERATE_ENCODER_FEEDBACK
    SimuPlot.plot_xy(Plot_t, Theta_enc, "Theta_enc");
    SimuPlot.plot_xy(Plot_t, Omega_enc, "Omega_enc");
#endif 
  }
  else{
    for(i = 0; i < 20; i++){
      if(TagNamesToPlot[i] == "Cemf_a"){
	SimuPlot.plot_xy(Plot_t, Cemf_a, "Cemf_a");
      }
      else if(TagNamesToPlot[i] == "VBus_a"){
	SimuPlot.plot_xy(Plot_t, VBus_a, "VBus_a");
      }
      else if(TagNamesToPlot[i] == "VRa"){
	SimuPlot.plot_xy(Plot_t, VRa, "VRa");
      }
#ifdef GENERATE_ENCODER_FEEDBACK
      else if(TagNamesToPlot[i] == "Theta_enc"){
	SimuPlot.plot_xy(Plot_t, Theta_enc, "Theta_enc");
      }
      else if(TagNamesToPlot[i] == "Omega_enc"){
	SimuPlot.plot_xy(Plot_t, Omega_enc, "Omega_enc");
      }
#endif 
      else if(TagNamesToPlot[i] == ""){
	break;
      }
    }

  }
}

       

void Ia_Ib_Ic_1::OdeGroupMSolve(double dydt[], double dmdt[])
{

  
#define A1  (Lm_b/Mab + Mbc/Mac) 
#define B1  (- Lm_c/Mac - Mbc/Mab)
#define A2  (Mab/Lm_a - Lm_b/Mab)
#define B2  (Mac/Lm_a + Mbc/Mab)

#define D   (B2*A1 - B1*A2)
#define F   (D*B2)
#define G   (Mab/(D*Lm_a) - Mac*A2/(F*Lm_a) + 1/D - A2/F)
#define H   (Mac/(B2*Lm_a) + 1/B2)
  
#define I   (H - B1*G)
#define J   (B2*G) 


  double dia;
  double dib;
  double dic;

  double C1;
  double C2;

  v_n = ((dmdt[2]/Mac - dmdt[1]/Mab)*J + (dmdt[0]/Lm_a + dmdt[1]/Mab)*I - dmdt[0]/Lm_a) / 
    (-J/Mab + J/Mac + I/Lm_a + I/Mab - 1/Lm_a);  

  C1 = - v_n/Mab + dmdt[1]/Mab + v_n/Mac - dmdt[2]/Mac;
  C2 = v_n/Lm_a + v_n/Mab - dmdt[0]/Lm_a - dmdt[1]/Mab;

  dib = (B2*C1 - B1*C2)/D;                
  dic = (C2 - A2*dib)/B2; 
  dia = (Mab*dib + Mac*dic - v_n + dmdt[0])/Lm_a; 

  dydt[0] = dia;
  dydt[1] = dib;
  dydt[2] = dic;

}

void Ia_Ib_Ic_1::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

  ((Omega *) pCurOdeItem->pOdeObject)->ia = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->ia = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->vs_a = vs_a;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->v_n = v_n;

  
}

void Ia_Ib_Ic_1::CtrlRValueUpdate(void)
{
  CtrlObjItem * pCurCtrlItem;

  //Update Rvalues
  pCurCtrlItem = pCtrlObjRValList;

  ((RefGen *) pCurCtrlItem->pCtrlObject)->ia = y;


}



// -----------------------------------------------------------------


// ---- Ia_QI_Ic_2  ----------------------------------------------------



Ia_Ib_Ic_2::Ia_Ib_Ic_2(void)
{
  OdeObjItem * pCurOdeItem;
  CtrlObjItem * pCurCtrlItem;

  OdeFuncName = ODE_FUNC_Ia_Ib_Ic_2;
  LiteralName = "Ia_Ib_Ic_2";  

  //build the ODE Rvalue list.
 
  pOdeObjRValList = new OdeObjItem;
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Omega];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];

  //build the CTRL Rvalue list

  pCtrlObjRValList = new CtrlObjItem;
  pCurCtrlItem = pCtrlObjRValList;

  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_RefGen];

  pwm_sig = 0;
  ib = 0;
 

//  PlotThisOutput = TRUE;
  Plot_Tag = "Ib";
  DoProbes = TRUE;
  
}



Ia_Ib_Ic_2::~Ia_Ib_Ic_2(void)
{

}



double Ia_Ib_Ic_2::OdeFunction(double y, double t)
{
 
  //Note: This is within a "group" ODE so we are really returning for "dm/dt + v_n"

          //"vs_b - Rb*ib + Kb*omega*sin(Nr*theta - 2*PI/3))" 
#ifdef LINEAR_AMP_MODE
     //generating the voltage command directly through CtrlObject "VDqCmd" 
  vs_b = pwm_sig + .5*DC_BUS_VOLTAGE;
#else
  vs_b = ShapeSquareWaveSource(((pwm_sig > 0) ? DC_BUS_VOLTAGE : 0), SRC_SLEW_RATE ,t);
#endif
  //(Note: "v_n" is really part of the LValue when we are doing the calculations).
  return(vs_b - Rb*y + Kb*omega*sin(Nr*theta - 2*PI/3));
}


void Ia_Ib_Ic_2::RecordProbes(void)
{
//  VBus_b.push_back(vs_b);
#ifdef RECORD_ALL_PROBES
  Cemf_b.push_back(Kb*omega*sin(Nr*theta - 2*PI/3));
  //"Rb*ib"
  VRb.push_back(Rb*y); 
#endif
}




void Ia_Ib_Ic_2::PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[])
{
  int i;
  if(TagNamesToPlot[0] == ""){
    SimuPlot.plot_xy(Plot_t, Cemf_b, "Cemf_b");
    SimuPlot.plot_xy(Plot_t, VBus_b, "VBus_b");
    SimuPlot.plot_xy(Plot_t, VRb, "VRb");  
  }
  else{
    for(i = 0; i < 20; i++){
      if(TagNamesToPlot[i] == "Cemf_b"){
	SimuPlot.plot_xy(Plot_t, Cemf_b, "Cemf_b");
      }
      else if(TagNamesToPlot[i] == "VBus_b"){
	SimuPlot.plot_xy(Plot_t, VBus_b, "VBus_b");
      }
      else if(TagNamesToPlot[i] == "VRb"){
	SimuPlot.plot_xy(Plot_t, VRb, "VRb");
      }
      else if(TagNamesToPlot[i] == ""){
	break;
      }
    }

  }
}




void Ia_Ib_Ic_2::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

  ((Omega *) pCurOdeItem->pOdeObject)->ib = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->ib = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->vs_b = vs_b;

  
}


void Ia_Ib_Ic_2::CtrlRValueUpdate(void)
{
  CtrlObjItem * pCurCtrlItem;

  //Update Rvalues
  pCurCtrlItem = pCtrlObjRValList;

  ((RefGen *) pCurCtrlItem->pCtrlObject)->ib = y;


}


// -----------------------------------------------------------------


// ---- Ia_Ib_Ic_3  ----------------------------------------------------


Ia_Ib_Ic_3::Ia_Ib_Ic_3(void)
{
  OdeObjItem * pCurOdeItem;
  CtrlObjItem * pCurCtrlItem;

  OdeFuncName = ODE_FUNC_Ia_Ib_Ic_3;
  LiteralName = "Ia_Ib_Ic_3";   

  //build the ODE Rvalue list.
 
  pOdeObjRValList = new OdeObjItem;
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Omega];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];

  //build the CTRL Rvalue list

  pCtrlObjRValList = new CtrlObjItem;
  pCurCtrlItem = pCtrlObjRValList;

  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_RefGen];

  pwm_sig = 0;
  ic = 0;
 

//  PlotThisOutput = TRUE;
  Plot_Tag = "Ic";
  DoProbes = TRUE;
  
}



Ia_Ib_Ic_3::~Ia_Ib_Ic_3(void)
{

}

double Ia_Ib_Ic_3::OdeFunction(double y, double t)
{
 
  //Note: This is within a "group" ODE so we are really returning for "dm/dt + v_n"

          //"vs_c - Rc*ic + Kc*omega*sin(Nr*theta - 4*PI/3))" 
#ifdef LINEAR_AMP_MODE
     //generating the voltage command directly through CtrlObject "VDqCmd" 
  vs_c = pwm_sig + .5*DC_BUS_VOLTAGE;
#else
  vs_c = ShapeSquareWaveSource(((pwm_sig > 0) ? DC_BUS_VOLTAGE : 0), SRC_SLEW_RATE ,t);
#endif 
  //(Note: "v_n" is really part of the LValue when we are doing the calculations).
  return(vs_c - Rc*y + Kc*omega*sin(Nr*theta - 4*PI/3));
}


void Ia_Ib_Ic_3::RecordProbes(void)
{
//  VBus_c.push_back(vs_c);
#ifdef RECORD_ALL_PROBES
  Cemf_c.push_back(Kc*omega*sin(Nr*theta - 4*PI/3));
  //"Rc*ic"
  VRc.push_back(Rc*y);
#endif  
}



void Ia_Ib_Ic_3::PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[])
{
  int i;
  if(TagNamesToPlot[0] == ""){
    SimuPlot.plot_xy(Plot_t, Cemf_c, "Cemf_c");
    SimuPlot.plot_xy(Plot_t, VBus_c, "VBus_c");
    SimuPlot.plot_xy(Plot_t, VRc, "VRc");  
  }
  else{
    for(i = 0; i < 20; i++){
      if(TagNamesToPlot[i] == "Cemf_c"){
	SimuPlot.plot_xy(Plot_t, Cemf_c, "Cemf_c");
      }
      else if(TagNamesToPlot[i] == "VBus_c"){
	SimuPlot.plot_xy(Plot_t, VBus_c, "VBus_c");
      }
      else if(TagNamesToPlot[i] == "VRc"){
	SimuPlot.plot_xy(Plot_t, VRc, "VRc");
      }
      else if(TagNamesToPlot[i] == ""){
	break;
      }
    }

  }
}



void Ia_Ib_Ic_3::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

  ((Omega *) pCurOdeItem->pOdeObject)->ic = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->ic = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->vs_c = vs_c;

  
}

void Ia_Ib_Ic_3::CtrlRValueUpdate(void)
{
  CtrlObjItem * pCurCtrlItem;

  //Update Rvalues
  pCurCtrlItem = pCtrlObjRValList;

  ((RefGen *) pCurCtrlItem->pCtrlObject)->ic = y;


}

// -----------------------------------------------------------------

// ---- Omega  ----------------------------------------------------

Omega::Omega(void)
{

  OdeObjItem * pCurOdeItem;
  CtrlObjItem * pCurCtrlItem;

  OdeFuncName = ODE_FUNC_Omega;
  LiteralName = "Omega";     

  //build the ODE Rvalue list.

  pOdeObjRValList = new OdeObjItem;          
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_1];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_2];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_3];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Theta];  
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];

  //build the CTRL Rvalue list

  pCtrlObjRValList = new CtrlObjItem;
  pCurCtrlItem = pCtrlObjRValList;

  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_RefGen];


  ia = 0;
  ib = 0;
  ic = 0;
  
  
//  PlotThisOutput = TRUE;
  Plot_Tag = "Omega";

}

Omega::~Omega(void)
{

}

double Omega::OdeFunction(double y, double t)
{



  //#define FORCE_CONSTANT_OMEGA   //un-comment this generate constant "Omega" to test phase characteristics
#ifdef FORCE_CONSTANT_OMEGA
  //Force "dOmega/dt" to some constant value so that probes like "Cemf_a" and "VBus_a" can be compared.
  //This gives us an Omega that starts from zero and increases in time.

  return(2*PI*100);  //rad/sec**2 (we should see a change of 2*PI*10 radians/sec every .1 sec?) 

#else
         //MEA: Add "- Dm_external*sin(2*Nr*theta)" for Neuro test.

         //(- Ka*ia*sin(Nr*theta) - Kb*ib*sin(Nr*theta - 2*PI/3) - Kc*ic*sin(Nr*theta - 4*PI/3) -
         //                        Bm*omega - Cm*sgn(omega) - Dm*sin(2*Nr*theta) - Dm_external*sin(2*Nr*theta) )/Jm

  return((- Ka*ia*sin(Nr*theta) - Kb*ib*sin(Nr*theta - 2*PI/3) - Kc*ic*sin(Nr*theta - 4*PI/3) -
	              Bm*y - Cm*(y < 0 ? -1 : (y > 0 ? 1 : 0)) - Dm*sin(2*Nr*theta) - Dm_external*sin(2*Nr*theta))/Jm);

#endif 



}

void Omega::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

  ((Ia_Ib_Ic_1 *) pCurOdeItem->pOdeObject)->omega = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Ia_Ib_Ic_2 *) pCurOdeItem->pOdeObject)->omega = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Ia_Ib_Ic_3 *) pCurOdeItem->pOdeObject)->omega = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Theta *) pCurOdeItem->pOdeObject)->omega = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->omega = y;



}

void Omega::CtrlRValueUpdate(void)
{
  CtrlObjItem * pCurCtrlItem;

  //Update Rvalues
  pCurCtrlItem = pCtrlObjRValList;

  ((RefGen *) pCurCtrlItem->pCtrlObject)->omega = y;


}

// -----------------------------------------------------------------




// ---- Theta  ----------------------------------------------------

Theta::Theta(void)
{

  OdeObjItem * pCurOdeItem;
  CtrlObjItem * pCurCtrlItem;

  OdeFuncName = ODE_FUNC_Theta;
  LiteralName = "Theta";    

  //build the ODE Rvalue list.

  pOdeObjRValList = new OdeObjItem;          
  pCurOdeItem = pOdeObjRValList;

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_1];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_2];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_3];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Omega];  
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Probe];


  //build the CTRL Rvalue list
  
  pCtrlObjRValList = new CtrlObjItem;          
  pCurCtrlItem = pCtrlObjRValList;
 
  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_VDqCmd];
  pCurCtrlItem->pNextCtrlItem = new CtrlObjItem;
  pCurCtrlItem = pCurCtrlItem->pNextCtrlItem;
  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_RefGen];

  omega = 0;
  
//#ifdef RECORD_ALL_PROBES
//  PlotThisOutput = TRUE;
//#endif
  Plot_Tag = "Theta";

}

Theta::~Theta(void)
{

}

double Theta::OdeFunction(double y, double t)
{
 


     //omega;
  return(omega); 

}

void Theta::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

  ((Ia_Ib_Ic_1 *) pCurOdeItem->pOdeObject)->theta = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Ia_Ib_Ic_2 *) pCurOdeItem->pOdeObject)->theta = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Ia_Ib_Ic_3 *) pCurOdeItem->pOdeObject)->theta = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Omega *) pCurOdeItem->pOdeObject)->theta = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((Probe *) pCurOdeItem->pOdeObject)->theta = y;
}

void Theta::CtrlRValueUpdate(void)
{
  CtrlObjItem * pCurCtrlItem;

  //Update Rvalues
  pCurCtrlItem = pCtrlObjRValList;

  ((VDqCmd *) pCurCtrlItem->pCtrlObject)->theta = y;
  pCurCtrlItem = pCurCtrlItem->pNextCtrlItem;
  ((RefGen *) pCurCtrlItem->pCtrlObject)->theta = y;
}


// -----------------------------------------------------------------


// ---- Probe  ----------------------------------------------------

Probe::Probe(void)
{

  //This is a receptical for viewing probes only. Thus, 
  //no "OdeRValueUpdate()" or "OdeFunction() functions 
  //are used.

  //(NOTE 10/5/08: A better way of doing this is to use "PostOdeFunction()" in "Ia_Ib_Ic_1"
  //               to immediately update "v_n" in "Ia_Ib_Ic_2" and "Ia_Ib_Ic_3". This way
  //               we get rid of the state delay in viewing "Vcm_x" relative to the the
  //               probes in these other ODE's.)

  OdeFuncName = ODE_FUNC_Probe;
  LiteralName = "Probe";   

  DoProbes = TRUE;

}

Probe::~Probe(void)
{

}


void Probe::RecordProbes(void)
{
#ifdef RECORD_ALL_PROBES
  Vcm_a.push_back(vs_a - Ra*ia + Ka*omega*sin(Nr*theta) - v_n);
  Vcm_b.push_back(vs_b - Rb*ib + Kb*omega*sin(Nr*theta - 2*PI/3) - v_n);
  Vcm_c.push_back(vs_c - Rc*ic + Kc*omega*sin(Nr*theta - 4*PI/3) - v_n);
  V_n.push_back(v_n);
#endif 
}



void Probe::PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[])
{
  int i;
  if(TagNamesToPlot[0] == ""){
    SimuPlot.plot_xy(Plot_t, Vcm_a, "Vcm_a");
    SimuPlot.plot_xy(Plot_t, Vcm_b, "Vcm_b");
    SimuPlot.plot_xy(Plot_t, Vcm_c, "Vcm_c");
    SimuPlot.plot_xy(Plot_t, V_n, "V_n");
  }
  else{
    for(i = 0; i < 20; i++){
      if(TagNamesToPlot[i] == "Vcm_a"){
	SimuPlot.plot_xy(Plot_t, Vcm_a, "Vcm_a");
      }
      else if(TagNamesToPlot[i] == "Vcm_b"){
	SimuPlot.plot_xy(Plot_t, Vcm_b, "Vcm_b");
      }
      else if(TagNamesToPlot[i] == "Vcm_c"){
	SimuPlot.plot_xy(Plot_t, Vcm_c, "Vcm_c");
      }
      else if(TagNamesToPlot[i] == "V_n"){
	SimuPlot.plot_xy(Plot_t, V_n, "V_n");
      }
      else if(TagNamesToPlot[i] == ""){
	break;
      }
    }

  }
}

// -----------------------------------------------------------------



// ******************************************************************

// ***** CtrlObject Functions ***************************************


// ** (Control of D/Q voltage command) **


// ---- VDqCmd  ---------------------------------------------------

VDqCmd::VDqCmd(void)
{
  SrcObjItem * pCurSrcItem;
  OdeObjItem * pCurOdeItem;

  CtrlFuncName = CTRL_FUNC_VDqCmd;
  LiteralName = "VDqCmd";  

  // Refering to Figure 11 of "SpaceVector_PWM_Inverter.pdf", we set  up the reference constructs.
  // The form here is:
  //
  //       SVTable[<sector_index>][<phase>] = <reference construct>
  //
  //                    where
  //                        <sector_index> == SectorNumber - 1
  //
  //                        <phase> == 0 -> "A", 1 -> "B", 2 -> "C"
  //
  //                        <reference construct> == TIME_T1_T2 -> "T1 + T2", TIME_T1 -> "T1", TIME_T2 -> "T2", TIME_NONE -> 0
  //                                                 (for non-clamped SVPWM, add ".5*T0" to all references.
  //
    
  // Definitions here are for minus side "clamped" SVPWM. For regular SVPWM, add ".5*TO" to all reference values.
  SVTable[0][0] = TIME_T1_T2;
  SVTable[0][1] = TIME_T2;
  SVTable[0][2] = TIME_NONE;
  SVTable[1][0] = TIME_T1; 
  SVTable[1][1] = TIME_T1_T2;
  SVTable[1][2] = TIME_NONE;
  SVTable[2][0] = TIME_NONE;
  SVTable[2][1] = TIME_T1_T2;
  SVTable[2][2] = TIME_T2;
  SVTable[3][0] = TIME_NONE;
  SVTable[3][1] = TIME_T1;
  SVTable[3][2] = TIME_T1_T2;
  SVTable[4][0] = TIME_T2;
  SVTable[4][1] = TIME_NONE;
  SVTable[4][2] = TIME_T1_T2;
  SVTable[5][0] = TIME_T1_T2;
  SVTable[5][1] = TIME_NONE;
  SVTable[5][2] = TIME_T1;

  //for now, we run strictly in SVPWM "clamped" mode.
  HighSpeedSVPwm = FALSE;
  //until we can simulate DC bus ripple voltage, we set for a constant value.
  v_bus = DC_BUS_VOLTAGE;
  
  //build the SRC Rvalue list.

  pSrcObjRValList = new SrcObjItem;
  pCurSrcItem = pSrcObjRValList;

  pCurSrcItem->pSrcObject = SrcObjectList[SRC_FUNC_PwmA];  
  pCurSrcItem->pNextSrcItem = new SrcObjItem;
  pCurSrcItem = pCurSrcItem->pNextSrcItem;
  pCurSrcItem->pSrcObject = SrcObjectList[SRC_FUNC_PwmB];
  pCurSrcItem->pNextSrcItem = new SrcObjItem;
  pCurSrcItem = pCurSrcItem->pNextSrcItem;
  pCurSrcItem->pSrcObject = SrcObjectList[SRC_FUNC_PwmC];
  pCurSrcItem->pNextSrcItem = new SrcObjItem;
  pCurSrcItem = pCurSrcItem->pNextSrcItem;
  pCurSrcItem->pSrcObject = SrcObjectList[SRC_FUNC_TriangleWave];


  //build the ODE Rvalue list.

  pOdeObjRValList = new OdeObjItem;
  pCurOdeItem = pOdeObjRValList;


#ifdef LINEAR_AMP_MODE
  //we are send the analog voltage commands directly to the "Ia_Ib_Ic_x" OdeObjects's
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_1];  
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_2];  
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_Ia_Ib_Ic_3];  

#endif 

  DoProbes = TRUE;

}

VDqCmd::~VDqCmd(void)
{



}



void VDqCmd::CtrlFunction(double t)
{
  int i;

  //(Note: "y" is not used in this CTRL. We use this CTRL to send command
  //       to the "PwmA", "PwmB" and "PwmC" CTRL's directly.



  //#define CONSTANT_VDQ_COMMAND
#ifdef CONSTANT_VDQ_COMMAND

  //For now, we provide constant commands for magnitude and angle.
  //These will come form seperate CTRL later.
  vcmd_mag = 10.0;
  vcmd_ang = PI/2;   //valid values are  +/-PI/2 <= vcmd_ang < +/-PI (pure "q" to pure "d") 
#else





  //This is a "Space Vector" command reference test....

  //VDQ command is generated by "RefGen" CtrlObject. This is a test to see
  //"theta", "omega", and "alpha" tracks "thedad", "omegad", and "alphad". Also if "id" and "iq" (generated in 
  //"FSFCtrl" tracks "idd" and "idq"

  vcmd_mag = sqrt(vdd*vdd + vqd*vqd);

  if(vqd != 0){

     if(vqd >= 0)
       vcmd_ang = PI/2.0;
     else
       vcmd_ang = - PI/2.0;

 
     vcmd_ang -= atan(vdd/vqd);

  }
  else{
    vcmd_ang = PI/2.0;
  }


#endif 




  //#define DEBUG_SPACE_VECTOR_CONTROL   //un-comment this to debug function "SpaceVectorControl()"
#ifdef DEBUG_SPACE_VECTOR_CONTROL
  //force feedback position to sweep a number of cycles so
  //that we may view "V_xo[]"
  theta = PI*t*10;   //specify "- PI*t*10" for minus test.
#endif 

  //generate the references.
  SpaceVectorControl();

#ifdef DEBUG_SPACE_VECTOR_CONTROL
  for(i = 0; i < 3; i++){
    //transfer over to debug buffers.
    V_xo_debug[i] = V_xo[i];
    //keep the actual commands zero to prevent the simulation from running away.
    V_xo[i] = 0;
  }

#endif 




}

void VDqCmd::RecordProbes(void)
{
#ifdef DEBUG_SPACE_VECTOR_CONTROL

  VCmd_a.push_back(V_xo_debug[0]);
  VCmd_b.push_back(V_xo_debug[1]);
#ifdef RECORD_ALL_PROBES
  VCmd_c.push_back(V_xo_debug[2]);
#endif

#else

//  VCmd_a.push_back(V_xo[0]);
//  VCmd_b.push_back(V_xo[1]);
#ifdef RECORD_ALL_PROBES
  VCmd_c.push_back(V_xo[2]);
#endif


#endif 

//  VCmd_mag.push_back(vcmd_mag);
//  VCmd_ang.push_back(vcmd_ang);
//  FreqScale.push_back(PwmFreqScale);

}

void VDqCmd::PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[])
{

  int i;
  if(TagNamesToPlot[0] == ""){
    SimuPlot.plot_xy(Plot_t, VCmd_a, "VCmd_a");
    SimuPlot.plot_xy(Plot_t, VCmd_b, "VCmd_b");
    SimuPlot.plot_xy(Plot_t, VCmd_c, "VCmd_c");  
    SimuPlot.plot_xy(Plot_t, VCmd_mag, "VCmd_mag");
    SimuPlot.plot_xy(Plot_t, VCmd_ang, "VCmd_ang");  
    SimuPlot.plot_xy(Plot_t, FreqScale, "FreqScale"); 
  }
  else{
    for(i = 0; i < 20; i++){
      if(TagNamesToPlot[i] == "VCmd_a"){
	SimuPlot.plot_xy(Plot_t, VCmd_a, "VCmd_a");
      }
      else if(TagNamesToPlot[i] == "VCmd_b"){
	SimuPlot.plot_xy(Plot_t, VCmd_b, "VCmd_b");
      }
      else if(TagNamesToPlot[i] == "VCmd_c"){
	SimuPlot.plot_xy(Plot_t, VCmd_c, "VCmd_c");
      }
      else if(TagNamesToPlot[i] == "VCmd_mag"){
	SimuPlot.plot_xy(Plot_t, VCmd_mag, "VCmd_mag");
      }
      else if(TagNamesToPlot[i] == "VCmd_ang"){
	SimuPlot.plot_xy(Plot_t, VCmd_ang, "VCmd_ang");
      }
      else if(TagNamesToPlot[i] == "FreqScale"){
	SimuPlot.plot_xy(Plot_t, FreqScale, "FreqScale");
      }

    }
  }


}

void  VDqCmd::CtrlRValueUpdate(void)     //!!!!! Pwm's should be in SrcRValueUpdate() (should not change operation) !!!!!!1
{
  SrcObjItem * pCurSrcItem;

  //Update Rvalues
  pCurSrcItem = pSrcObjRValList;

  ((PwmA *) pCurSrcItem->pSrcObject)->V_xo = V_xo[0];
  pCurSrcItem = pCurSrcItem->pNextSrcItem;       
  ((PwmB *) pCurSrcItem->pSrcObject)->V_xo = V_xo[1];
  pCurSrcItem = pCurSrcItem->pNextSrcItem;       
  ((PwmC *) pCurSrcItem->pSrcObject)->V_xo = V_xo[2];
  pCurSrcItem = pCurSrcItem->pNextSrcItem;       
  ((TriangleWave *) pCurSrcItem->pSrcObject)->PwmFreqScale = PwmFreqScale;


}

void VDqCmd::OdeRValueUpdate(void)
{
  OdeObjItem * pCurOdeItem;

  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;


#ifdef LINEAR_AMP_MODE

  //we are sending the analog voltage commands directly to the "Ia_Ib_Ic_x" OdeObjects's
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  ((Ia_Ib_Ic_1 *) pCurOdeItem->pOdeObject)->pwm_sig = V_xo[0];
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  ((Ia_Ib_Ic_2 *) pCurOdeItem->pOdeObject)->pwm_sig = V_xo[1];
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  ((Ia_Ib_Ic_3 *) pCurOdeItem->pOdeObject)->pwm_sig = V_xo[2];

#endif 

}


void VDqCmd::SpaceVectorControl(void) 
{

     
  int SectorNumber;  
  int SectorIndex;
  double SectorAngle;
  double CtrlAngle;
  double T0, T1, T2;
  double Amplitude;
  double T_Z_Scaled;
  int i;



#ifdef TRIANGLE_REF_MODE
  //This is a "Triangle" command reference test....

  CtrlAngle = Nr*theta;

#ifdef LINEAR_AMP_MODE
  //Here we generate analog voltage commands directly....

  Amplitude = (2.0/3.0);

  V_xo[0] = Amplitude*(vdd*cos(CtrlAngle) - vqd*sin(CtrlAngle));
  V_xo[1] = Amplitude*(vdd*cos(CtrlAngle - 2*PI/3) - vqd*sin(CtrlAngle - 2*PI/3));
  V_xo[2] = Amplitude*(vdd*cos(CtrlAngle - 4*PI/3) - vqd*sin(CtrlAngle - 4*PI/3));  

#else

  Amplitude = (2.0/3.0)*PWM_GAIN;

  V_xo[0] = Amplitude*(vdd*cos(CtrlAngle) - vqd*sin(CtrlAngle)) / v_bus;
  V_xo[1] = Amplitude*(vdd*cos(CtrlAngle - 2*PI/3) - vqd*sin(CtrlAngle - 2*PI/3)) / v_bus;
  V_xo[2] = Amplitude*(vdd*cos(CtrlAngle - 4*PI/3) - vqd*sin(CtrlAngle - 4*PI/3)) / v_bus;  

#endif 

#else

  //This is a "Space Vector" command reference test....

     //for now, only "theta" and "vcmd_ang" determine the control angle. Position correction due to "omega"
     //and "alpha" will be added later.
  CtrlAngle = theta * Nr + vcmd_ang;                  
  SectorIndex = (int) (CtrlAngle / SECTOR_ANGLE);

  //Note: "SpaceVector_PWM_Inverter.pdf" specifies the the range of "SectorAngle" between 0 and 60 degrees.
  //      This is wronge, the range should be 0 to 360 degrees.
  if(CtrlAngle >= 0){
    SectorNumber = SectorIndex % 6 + 1;
    SectorAngle = CtrlAngle - (double) (SectorIndex / 6) * SECTOR_ANGLE * 6.0; 
  }
  else{
    SectorNumber = 6 + SectorIndex % 6;
    SectorAngle = CtrlAngle + (double) (6 - SectorIndex / 6) * SECTOR_ANGLE * 6.0; 
  }
  
  T_Z_Scaled = (2.0 / 3.0) * T_Z;

  T1 = SQRT_3 * T_Z_Scaled * vcmd_mag * sin((PI * (double) SectorNumber / 3.0) - SectorAngle) / v_bus;
  T2 = SQRT_3 * T_Z_Scaled * vcmd_mag * sin(SectorAngle - (SectorNumber - 1) * PI / 3.0) / v_bus;
  T0 = T_Z_Scaled - T1 - T2;
  //assign references to each phase,
  for(i = 0; i < 3; i++){

    switch(SVTable[SectorNumber-1][i]){
    case TIME_NONE:
      V_xo[i] = (HighSpeedSVPwm ==  TRUE ? .5*T0 : 0);
      break;
    case TIME_T1:
      V_xo[i] = T1 + (HighSpeedSVPwm ==  TRUE ? .5*T0 : 0);
      break;
    case TIME_T2:
      V_xo[i] = T2 + (HighSpeedSVPwm ==  TRUE ? .5*T0 : 0);
      break;
    case TIME_T1_T2:
      V_xo[i] = T1 + T2 + (HighSpeedSVPwm ==  TRUE ? .5*T0 : 0);
      break;
    }

  }


#endif 

  




}


// ---------------------------------------------------------------



// ---- RefGen  ---------------------------------------------------

RefGen::RefGen(void)
{
  CtrlObjItem * pCurCtrlItem;

  CtrlFuncName = CTRL_FUNC_RefGen;
  LiteralName = "RefGen";  

    //We want this "RefGen" to immediately update "VDqCmd" and other selected CtrlObject's ahead of it after it's 
    //control function is executed.
  DoImmedCtrlUpdate = TRUE;

    //build the CTRL Rvalue list
  
  pCtrlObjRValList = new CtrlObjItem;          
  pCurCtrlItem = pCtrlObjRValList;

  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_VDqCmd];
  pCurCtrlItem->pNextCtrlItem = new CtrlObjItem;
  pCurCtrlItem = pCurCtrlItem->pNextCtrlItem;
  pCurCtrlItem->pCtrlObject = CtrlObjectList[CTRL_FUNC_VDqCmd];

  //!!!!!!!!!!!!!!!
  //
  //   Here is where we setup to write the following R-values to "FSFCtrl" below
  //
  //   vdd, vqd, idd, iqd, omegad, thetad, zetad
  //
  //
  //
  //!!!!!!!!!!

  DoProbes = TRUE;

  	  // https://github.com/codeplea/genann/blob/master/README.md

  srand(time(0));
  	  //Determine the number input layer neurons to  create for the test.
#define NEURON_COUNT_SCALE_FACTOR  (.01)
  NumOfNeurons = (double) (2*RT_TRAJ + CT_TRAJ) / (REF_GEN_QUANTUM_CNT * QUANTUM_PERIOD)
  ann = NeuralNetworkInitialize(NumOfNeurons, 0, 0, 1);




}

RefGen::~RefGen(void)
{



}



void RefGen::CtrlFunction(double t)
{

  double Lm = (Lm_a + Lm_b + Lm_c) / 3.0;
  double M = (Mab + Mac + Mbc) / 3.0;
  double R = (Ra + Rb + Rc) / 3.0;
  double K = (Ka + Kb + Kc) / 3.0;


#ifdef NEURO_RUN_MODE

  int i;


#define NERUO_MODE_DELAY .02
#define  TRAJ_RAMP   (.5  - NERUO_MODE_DELAY)

  //determine actual "id" and "iq" currents.

  id =  cos(Nr*theta)*ia + cos(Nr*theta - 2*PI/3.0)*ib + cos(Nr*theta - 4*PI/3.0)*ic;
  iq =  - sin(Nr*theta)*ia - sin(Nr*theta - 2*PI/3.0)*ib - sin(Nr*theta - 4*PI/3.0)*ic;


  if(t < (TRAJ_RAMP  + NERUO_MODE_DELAY))
  {


	  //Point to point, "modified sin" trajectory generator.
	  if(t < RT_TRAJ){
	    omegad = VO_TRAJ*(sin(t*PI/RT_TRAJ - PI/2.0) + 1)/2.0;
	    thetad =  VO_TRAJ*(-(RT_TRAJ/PI)*cos(t*PI/RT_TRAJ - PI/2.0) + t)/2.0;
	    alphad = VO_TRAJ*(PI/RT_TRAJ)*cos(t*PI/RT_TRAJ - PI/2.0)/2.0;
	    betad = VO_TRAJ*(-PI/RT_TRAJ)*(PI/RT_TRAJ)*sin(t*PI/RT_TRAJ - PI/2.0)/2.0;



	  }
	  else if(t < (RT_TRAJ + CT_TRAJ)){
	    omegad = VO_TRAJ;
	    thetad = VO_TRAJ*RT_TRAJ/2.0 + VO_TRAJ*(t-RT_TRAJ);
	    alphad = 0;
	    betad = 0;




	  }
	  else if(t < (2*RT_TRAJ + CT_TRAJ)){
	    omegad = VO_TRAJ*(sin((CT_TRAJ+2*RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0) + 1)/2.0;
	    thetad =  VO_TRAJ*RT_TRAJ/2.0 + VO_TRAJ*(CT_TRAJ) + VO_TRAJ*(-(RT_TRAJ/PI)*cos((CT_TRAJ+RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0) +
								      t-(RT_TRAJ+CT_TRAJ))/2.0;
	    alphad = VO_TRAJ*(-PI/RT_TRAJ)*cos((CT_TRAJ+2*RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0)/2.0;
	    betad = VO_TRAJ*(PI/RT_TRAJ)*(-PI/RT_TRAJ)*sin((CT_TRAJ+2*RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0)/2.0;

	  }
	  else{
	    omegad = 0;
	    thetad = 0;
	    alphad = 0;
	    betad = 0;

	    Inst_Ia_Ib_Ic_1,ia = 0;
	    Inst_Ia_Ib_Ic_1.v_n = 0;
	    Inst_Ia_Ib_Ic_1.vs_a = 0;
	    Inst_Ia_Ib_Ic_1.omega = 0;
	    Inst_Ia_Ib_Ic_1.theta = 0;
	    Inst_Ia_Ib_Ic_1.y = 0;

	    Inst_Ia_Ib_Ic_2,ib = 0;
	    Inst_Ia_Ib_Ic_2.v_n = 0;
	    Inst_Ia_Ib_Ic_2.vs_b = 0;
	    Inst_Ia_Ib_Ic_2.omega = 0;
	    Inst_Ia_Ib_Ic_2.theta = 0;
	    Inst_Ia_Ib_Ic_2.y = 0;

	    Inst_Ia_Ib_Ic_3,ic = 0;
	    Inst_Ia_Ib_Ic_3.v_n = 0;
	    Inst_Ia_Ib_Ic_3.vs_c = 0;
	    Inst_Ia_Ib_Ic_3.omega = 0;
	    Inst_Ia_Ib_Ic_3.theta = 0;
	    Inst_Ia_Ib_Ic_3.y = 0;

	    Inst_Omega.ia = 0;
	    Inst_Omega.ib = 0;
	    Inst_Omega.ic = 0;
	    Inst_Omega.theta = 0;
	    Inst_Omega.y = 0;

	    Inst_Theta.omega = 0;
	    Inst_Theta.y = 0;

	  }

  	  // !!!!!! TEST !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

	   if(t < .4)
	   {
		   ann_in[0] = .5;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
		   ann_train[0] = .5;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .6;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
		   ann_train[0] = .55;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .5;
		   ann_in[1] = .6;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
		   ann_train[0] = .6;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .6;
		   ann_in[1] = .6;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
		   ann_train[0] = .65;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .5;
		   ann_in[1] = .5;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
		   ann_train[0] = .7;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .6;
		   ann_in[1] = .5;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
		   ann_train[0] = .75;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .5;
		   ann_in[1] = .6;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
		   ann_train[0] = .8;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .6;
		   ann_in[1] = .6;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
		   ann_train[0] = .85;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .5;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .6;
		   ann_train[0] = .9;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);

		   ann_in[0] = .6;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .6;
		   ann_train[0] = .95;
		   NeuralNetworkTrain(ann, &ann_in[0], &ann_train[0], 5);
	   }






	  	  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  }
  else
  {

	double t_off = t - (TRAJ_RAMP  + NERUO_MODE_DELAY);
	//Point to point, "modified sin" trajectory generator.
	if(t_off < RT_TRAJ){
		omegad = VO_TRAJ*(sin(t_off*PI/RT_TRAJ - PI/2.0) + 1)/2.0;
		thetad =  VO_TRAJ*(-(RT_TRAJ/PI)*cos(t_off*PI/RT_TRAJ - PI/2.0) + t_off)/2.0;
		alphad = VO_TRAJ*(PI/RT_TRAJ)*cos(t_off*PI/RT_TRAJ - PI/2.0)/2.0;
		betad = VO_TRAJ*(-PI/RT_TRAJ)*(PI/RT_TRAJ)*sin(t_off*PI/RT_TRAJ - PI/2.0)/2.0;


	}
	else if(t_off < (RT_TRAJ + CT_TRAJ)){
		omegad = VO_TRAJ;
		thetad = VO_TRAJ*RT_TRAJ/2.0 + VO_TRAJ*(t_off-RT_TRAJ);
		alphad = 0;
		betad = 0;



	}
	else if(t_off < (2*RT_TRAJ + CT_TRAJ)){
		omegad = VO_TRAJ*(sin((CT_TRAJ+2*RT_TRAJ-t_off)*PI/RT_TRAJ - PI/2.0) + 1)/2.0;
		thetad =  VO_TRAJ*RT_TRAJ/2.0 + VO_TRAJ*(CT_TRAJ) + VO_TRAJ*(-(RT_TRAJ/PI)*cos((CT_TRAJ+RT_TRAJ-t_off)*PI/RT_TRAJ - PI/2.0) +
								  t_off-(RT_TRAJ+CT_TRAJ))/2.0;
		alphad = VO_TRAJ*(-PI/RT_TRAJ)*cos((CT_TRAJ+2*RT_TRAJ-t_off)*PI/RT_TRAJ - PI/2.0)/2.0;
		betad = VO_TRAJ*(PI/RT_TRAJ)*(-PI/RT_TRAJ)*sin((CT_TRAJ+2*RT_TRAJ-t_off)*PI/RT_TRAJ - PI/2.0)/2.0;

	}
	else{
	    omegad = 0;
	    thetad = 0;
	    alphad = 0;
	    betad = 0;

	    Inst_Ia_Ib_Ic_1,ia = 0;
	    Inst_Ia_Ib_Ic_1.v_n = 0;
	    Inst_Ia_Ib_Ic_1.vs_a = 0;
	    Inst_Ia_Ib_Ic_1.omega = 0;
	    Inst_Ia_Ib_Ic_1.theta = 0;
	    Inst_Ia_Ib_Ic_1.y = 0;

	    Inst_Ia_Ib_Ic_2,ib = 0;
	    Inst_Ia_Ib_Ic_2.v_n = 0;
	    Inst_Ia_Ib_Ic_2.vs_b = 0;
	    Inst_Ia_Ib_Ic_2.omega = 0;
	    Inst_Ia_Ib_Ic_2.theta = 0;
	    Inst_Ia_Ib_Ic_2.y = 0;

	    Inst_Ia_Ib_Ic_3,ic = 0;
	    Inst_Ia_Ib_Ic_3.v_n = 0;
	    Inst_Ia_Ib_Ic_3.vs_c = 0;
	    Inst_Ia_Ib_Ic_3.omega = 0;
	    Inst_Ia_Ib_Ic_3.theta = 0;
	    Inst_Ia_Ib_Ic_3.y = 0;

	    Inst_Omega.ia = 0;
	    Inst_Omega.ib = 0;
	    Inst_Omega.ic = 0;
	    Inst_Omega.theta = 0;
	    Inst_Omega.y = 0;

	    Inst_Theta.omega = 0;
	    Inst_Theta.y = 0;


	}

	  // !!!!!! TEST !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

	   if(t_off < .1)
	   {
		   ann_in[0] = .5;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .2)
	   {
		   ann_in[0] = .6;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .3)
	   {
		   ann_in[0] = .5;
		   ann_in[1] = .6;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .4)
	   {
		   ann_in[0] = .6;
		   ann_in[1] = .6;
		   ann_in[2] = .5;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .5)
	   {
		   ann_in[0] = .5;
		   ann_in[1] = .5;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .6)
	   {
		   ann_in[0] = .6;
		   ann_in[1] = .5;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .7)
	   {
		   ann_in[0] = .5;
		   ann_in[1] = .6;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .8)
	   {
		   ann_in[0] = .6;
		   ann_in[1] = .6;
		   ann_in[2] = .6;
		   ann_in[3] = .5;
	   }
	   else if(t_off < .9)
	   {
		   ann_in[0] = .5;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .6;
	   }
	   else
	   {
		   ann_in[0] = .6;
		   ann_in[1] = .5;
		   ann_in[2] = .5;
		   ann_in[3] = .6;
	   }









			ann_pred = NeuralNetworkRun(ann, &ann_in[0]);
			ann_pred_buf[0] = ann_pred[0];


	  	  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  }




#else


  //Point to point, "modified sin" trajectory generator.
  if(t < RT_TRAJ){
    omegad = VO_TRAJ*(sin(t*PI/RT_TRAJ - PI/2.0) + 1)/2.0;
    thetad =  VO_TRAJ*(-(RT_TRAJ/PI)*cos(t*PI/RT_TRAJ - PI/2.0) + t)/2.0;
    alphad = VO_TRAJ*(PI/RT_TRAJ)*cos(t*PI/RT_TRAJ - PI/2.0)/2.0;
    betad = VO_TRAJ*(-PI/RT_TRAJ)*(PI/RT_TRAJ)*sin(t*PI/RT_TRAJ - PI/2.0)/2.0;
    
  }
  else if(t < (RT_TRAJ + CT_TRAJ)){
    omegad = VO_TRAJ;
    thetad = VO_TRAJ*RT_TRAJ/2.0 + VO_TRAJ*(t-RT_TRAJ);
    alphad = 0;
    betad = 0;
    
  }
  else if(t < (2*RT_TRAJ + CT_TRAJ)){
    omegad = VO_TRAJ*(sin((CT_TRAJ+2*RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0) + 1)/2.0;
    thetad =  VO_TRAJ*RT_TRAJ/2.0 + VO_TRAJ*(CT_TRAJ) + VO_TRAJ*(-(RT_TRAJ/PI)*cos((CT_TRAJ+RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0) + 
							      t-(RT_TRAJ+CT_TRAJ))/2.0;
    alphad = VO_TRAJ*(-PI/RT_TRAJ)*cos((CT_TRAJ+2*RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0)/2.0;
    betad = VO_TRAJ*(PI/RT_TRAJ)*(-PI/RT_TRAJ)*sin((CT_TRAJ+2*RT_TRAJ-t)*PI/RT_TRAJ - PI/2.0)/2.0;
    
  }
  else{
    omegad = 0;
    thetad = VO_TRAJ*RT_TRAJ + VO_TRAJ*(CT_TRAJ);
    alphad = 0;
    betad = VO_TRAJ*(PI/RT_TRAJ)*(-PI/RT_TRAJ)*(-1.0/2.0);
    
  }
 
  //!!!!!!!!!!!!!!!!!!!!! NOTE !!!!!!!!!!!!!!!!!!!!!!!!1

  //"betad" being discontinuous causes small trajectory generation.

  betad = 0;
  
  //This is why exponential based ramping is ideal (no discontinuities at higher differencials)
  //Interesting howerver, if we force to "0", we can get a pretty good representation without generating
  //a "bump" on vdq.

  //In any event, this enforces the idea that the trajectory should be generated with using exponential ramping.

  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

#endif


  //generate the commands "vdd" and "vqd"


  idd = - ((Lm + M)*K*(3.0/2.0)*Nr*omegad*omegad) / (R*R + (Lm + M)*(Lm + M)*Nr*Nr*omegad*omegad);    
  iqd = (Jm*alphad  + Bm*omegad + Cm*sgn(omegad) + Dm*sin(2*Nr*thetad)) / K;                        
  didd = - (2*K*(3.0/2.0)*(Lm + M)*R*R*Nr*omegad*alphad) / (R*R + (Lm + M)*(Lm + M)*Nr*Nr*omegad*omegad);    
  diqd = (Jm*betad  + Bm*alphad  + Cm*impulse(sgn(omegad)) + Dm*2*Nr*cos(2*Nr*thetad)) / K;              

  	  //Test for  vdd set to "0".
  	  //Interesting thing is that the feed forward control is dramatically degraded when we disable field weakening (Plot 'Omega" vs "omegad").
//#define TEST_WITH_VDD_ZERO
#ifdef TEST_WITH_VDD_ZERO
  vdd = 	0;				//When we set to a constant -8.0, looks better but not as good as with the control enabled.
#else
  vdd = (Lm + M)*didd + R*idd - Nr*omegad*(Lm + M)*iqd;
#endif
                           //Like above, note the "3/2" scale factor on "K" below.
  vqd = (Lm + M)*diqd + R*iqd + Nr*omegad*(Lm + M)*idd + K*(3.0/2.0)*omegad;

  //For the full state feedback controller, we will integrate "thedad"

  zetad = zetad + thetad*(QUANTUM_PERIOD  * (double) REF_GEN_QUANTUM_CNT);

}

void RefGen::RecordProbes(void)
{

  beta_d.push_back(betad);
  alpha_d.push_back(alphad);
  theta_d.push_back(thetad);
  omega_d.push_back(omegad);



  id_d.push_back(idd);
  iq_d.push_back(iqd);

#ifdef RECORD_ALL_PROBES
  zeta_d.push_back(zetad);
  did_d.push_back(didd);
  diq_d.push_back(diqd);
#endif
//  vd_d.push_back(vdd);
//  vq_d.push_back(vqd);

  theta_a.push_back(theta);
  omega_a.push_back(omega);
  id_a.push_back(id);
  iq_a.push_back(iq);

  theta_pred_diff_a.push_back(ann_pred_buf[0]);
  omega_pred_diff_a.push_back(ann_pred_buf[1]);
  iq_pred_diff_a.push_back(ann_pred_buf[2]);

}


void RefGen::PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[])
{

  int i;
  if(TagNamesToPlot[0] == ""){
    SimuPlot.plot_xy(Plot_t, beta_d, "betad");
    SimuPlot.plot_xy(Plot_t, alpha_d, "alphad");
    SimuPlot.plot_xy(Plot_t, omega_d, "omegad"); 
    SimuPlot.plot_xy(Plot_t, theta_d, "thetad");
    SimuPlot.plot_xy(Plot_t, zeta_d, "zetad");
    SimuPlot.plot_xy(Plot_t, id_d, "idd");  
    SimuPlot.plot_xy(Plot_t, iq_d, "iqd");
    SimuPlot.plot_xy(Plot_t, did_d, "didd");
    SimuPlot.plot_xy(Plot_t, diq_d, "diqd");
    SimuPlot.plot_xy(Plot_t, vd_d, "vdd");
    SimuPlot.plot_xy(Plot_t, vq_d, "vqd");    

    SimuPlot.plot_xy(Plot_t, theta_a, "theta");
    SimuPlot.plot_xy(Plot_t, omega_a, "omega");
    SimuPlot.plot_xy(Plot_t, id_a, "id");
    SimuPlot.plot_xy(Plot_t, iq_a, "iq");

    SimuPlot.plot_xy(Plot_t, theta_pred_diff_a, "theta_pred_diff");
    SimuPlot.plot_xy(Plot_t, omega_pred_diff_a, "omega_pred_diff");
    SimuPlot.plot_xy(Plot_t, iq_pred_diff_a, "iq_pred_diff");

  }
  else{
    for(i = 0; i < 20; i++){
      if(TagNamesToPlot[i] == "betad"){
	SimuPlot.plot_xy(Plot_t, beta_d, "betad");
      }
      else if(TagNamesToPlot[i] == "alphad"){
	SimuPlot.plot_xy(Plot_t, alpha_d, "alphad");
      }
      else if(TagNamesToPlot[i] == "omegad"){
	SimuPlot.plot_xy(Plot_t, omega_d, "omegad");
      }
      else if(TagNamesToPlot[i] == "thetad"){
	SimuPlot.plot_xy(Plot_t, theta_d, "thetad");
      }
      else if(TagNamesToPlot[i] == "zetad"){
	SimuPlot.plot_xy(Plot_t, zeta_d, "zetad");
      }
      else if(TagNamesToPlot[i] == "idd"){
	SimuPlot.plot_xy(Plot_t, id_d, "idd");
      }
      else if(TagNamesToPlot[i] == "iqd"){
	SimuPlot.plot_xy(Plot_t, iq_d, "iqd");
      }
      else if(TagNamesToPlot[i] == "didd"){
	SimuPlot.plot_xy(Plot_t, did_d, "didd");
      }
      else if(TagNamesToPlot[i] == "diqd"){
	SimuPlot.plot_xy(Plot_t, diq_d, "diqd");
      }
      else if(TagNamesToPlot[i] == "vdd"){
	SimuPlot.plot_xy(Plot_t, vd_d, "vdd");
      }
      else if(TagNamesToPlot[i] == "vqd"){
	SimuPlot.plot_xy(Plot_t, vq_d, "vqd");
      }
      else if(TagNamesToPlot[i] == "thetaa"){
	SimuPlot.plot_xy(Plot_t, theta_a, "theta");
      }
      else if(TagNamesToPlot[i] == "omegaa"){
	SimuPlot.plot_xy(Plot_t, omega_a, "omega");
      }
      else if(TagNamesToPlot[i] == "ida"){
	SimuPlot.plot_xy(Plot_t, id_a, "id");
      }
      else if(TagNamesToPlot[i] == "iqa"){
	SimuPlot.plot_xy(Plot_t, iq_a, "iq");
      }


      else if(TagNamesToPlot[i] == "theta_pred_diff"){
  	SimuPlot.plot_xy(Plot_t, theta_pred_diff_a, "theta_pred_diff");
       }
      else if(TagNamesToPlot[i] == "omega_pred_diff"){
	SimuPlot.plot_xy(Plot_t, omega_pred_diff_a, "omega_pred_diff");
      }
      else if(TagNamesToPlot[i] == "iq_pred_diff"){
	SimuPlot.plot_xy(Plot_t, iq_pred_diff_a, "iq_pred_diff");
      }

    }
  }


}

void RefGen::CtrlRValueUpdate(void)
{
  CtrlObjItem * pCurCtrlItem;

  //Update Rvalues
  pCurCtrlItem = pCtrlObjRValList;

  ((VDqCmd *) pCurCtrlItem->pCtrlObject)->vdd = vdd;
  pCurCtrlItem = pCurCtrlItem->pNextCtrlItem;
  ((VDqCmd *) pCurCtrlItem->pCtrlObject)->vqd = vqd;

  //!!!!!!!!!!!!!!!
  //
  //   Here is where we write the following R-values to "FSFCtrl" below
  //
  //   vdd, vqd, idd, iqd, omegad, thetad, zetad
  //
  //
  //
  //!!!!!!!!!!




}





double RefGen::sgn(double omegad)
{
  //code needs to be written here...
  return 0;
}

double RefGen::impulse(double sgn_omegad)
{

  //code needs to be written here...

  return 0;
}





// ---------------------------------------------------------------






// *****************************************************************


  


// **** SimuObject ************************************************




SimuObject Simulation;

SimuObject::SimuObject(void)
{
  OdeObjItem * pCurOdeItem;
  SrcObjItem * pCurSrcItem;
  CtrlObjItem * pCurCtrlItem;
  CtrlObjGroup * pCurCtrlGroup; 
  int i;

#ifdef NEURO_RUN_MODE

  SimuTime = 1.0;

   MinimumPlotTime = 0;
   MaximumPlotTime = 1;

#else
  SimuTime = 1.0;        
  
  MinimumPlotTime = 0;
  MaximumPlotTime = 1;

#endif

  ExceptionTime = .05;

  //For this example the setting of "RelTol" is important. If it is set to large
  //the system will become unstable. 
  RelTol =  .0000000001;
  //Setting "AbsTol" too small will make a system with a decaying oscillation 
  //take too long to finish.
  AbsTol =  .0000000000001; 
  //typical error correction setting as per "RungeKutta_java.htm" (never set higher then "1.0")
  Safety = .98;
  h_start = .000000001; 

  h = h_start;

  //set to a value other then "0" to clamp maximum "h";
  h_max = .0000002;

  //set to store and plot "h" parameter.
 // Do_h_Plot = 1;
  
  TimeQuantum = h;
  CtrlTimeQuantum = QUANTUM_PERIOD;

  //set tp force CTRL update on first ODE update.
  CtrlTimeAccumulator = 0;

  SrcPeriodQuantum = QUANTUM_PERIOD;
  SrcPeriodAccumulator = 0;
 

  //build the SRC equation list
  pSrcEquationList = new SrcObjItem;
  pCurSrcItem = pSrcEquationList;
  i = 0;
  while(SrcObjectList[i]){
    pCurSrcItem->pSrcObject = SrcObjectList[i];
    i++;
    if(!SrcObjectList[i])
      break;
    pCurSrcItem->pNextSrcItem = new SrcObjItem;
    pCurSrcItem = pCurSrcItem->pNextSrcItem;


  }

  //build the ODE equation list.
  pOdeEquationList = new OdeObjItem;
  pCurOdeItem = pOdeEquationList;
  i = 0;
  while(1){
    pCurOdeItem->pOdeObject = OdeObjectList[i];
    i++;
    if(!OdeObjectList[i])
      break;
    pCurOdeItem->pNextOdeItem = new OdeObjItem;
    pCurOdeItem = pCurOdeItem->pNextOdeItem;
  }

  //build the CTRL equation lists.

  //(NOTE: As of 6/12/08, "pTranslationList" and "pTrajectoryList" CTRL's have been 
  //       deprecated and replaced with a "group" structure.)

  pCtrlGroupList = new CtrlObjGroup;
  pCurCtrlGroup = pCtrlGroupList;

  pCurCtrlGroup->pCtrlEquationList = new CtrlObjItem;
  pCurCtrlItem = pCurCtrlGroup->pCtrlEquationList;
  //set tp force CTRL update on first ODE update.
  pCurCtrlGroup->QuantumCount = 1;
  pCurCtrlGroup->QuantumNum = CtrlObjectQuantum[0];

  i = 0;
  while(1){
    pCurCtrlItem->pCtrlObject = CtrlObjectList[i];   
    i++;
    if(!CtrlObjectList[i])                 
      break;

    if(CtrlObjectQuantum[i] == CtrlObjectQuantum[i-1]){
      pCurCtrlItem->pNextCtrlItem = new CtrlObjItem;
      pCurCtrlItem = pCurCtrlItem->pNextCtrlItem;
    }
    else{
      //create a new group...
      pCtrlGroupList->pNextCtrlGroup = new CtrlObjGroup;
      pCurCtrlGroup = pCtrlGroupList->pNextCtrlGroup;

      pCurCtrlGroup->pCtrlEquationList = new CtrlObjItem;
      pCurCtrlItem = pCurCtrlGroup->pCtrlEquationList;
      //set tp force CTRL update on first ODE update.
      pCurCtrlGroup->QuantumCount = 1;
      pCurCtrlGroup->QuantumNum = CtrlObjectQuantum[i];  
    }
  }



 

}


SimuObject::~SimuObject(void)
{

  //(ideally we should delete all make allocation make in the constructor, here)

}



// ************************************************************





bool ExecuteSimulation(void)
{

   

  Simulation.OdeSimuType = ODE_SIMU_56;

  while(Simulation.t < Simulation.SimuTime){               

    if(!Simulation.DoOneInteration())
      return FALSE;
    
  }
  if(Simulation.GotException){
    cout << Simulation.ExceptionMessageBuffer.str().c_str();
  }

  return TRUE;
}

void PlotResults(string TagNamesToPlot[], double ScaleFactors[], double MinPlotTimes[], double MaxPlotTimes[], 
		 double PlotTimeSteps[], double PlotTimeOffset)
{
  Simulation.PlotSimuResults(TagNamesToPlot, ScaleFactors, MinPlotTimes, MaxPlotTimes, PlotTimeSteps, PlotTimeOffset);


}
