

//    Here we present the design of current/velocity observer for an AC brushless motor controlled by a 
//    space vector PWM switch algorythm.
//
//
//    Refer to "App_CMChoke.hpp" and "App_DualCMChoke.hpp" as references used to construct this application
//    (App_Observer.hpp).
//
//    Also, refer the artical "Jones on Stepping Motor Physics.htm" for information on modeling
//    of the detect torque.
//
//    In addition, refer to "High-Speed Parameter Estimation of Stepper Motors" (should have this
//    in scanned PDF form in this directory as well), for information on the modeling of
//    viscous friction and Coulomb friction ("stick-tion").
//
//    Also, refer to the document (in this directory) titled "AcMotorNotes.pdf" for information on
//    constructing the model of an AC brushless motor.
//
//    In addition, two documents titled "GetTRDoc[1].pdf" and "SpaceVector_PWM_Inverter.pdf" in directory
//    "./SpaceVectorPWM" should be reviewed.
//
//
//    The algorythm used in this simulation will be what is refered to in the industry as the "conventional" form
//    of Space Vector PWM (SVPWM). A good review for this can be obtained from "SpaceVector_PWM_Inverter.pdf".
//            
//    There also exists what is known as hybrid approches to SVPWM, where five or more variants of SVPWM
//    are applied to the motor depending on it's current operating point.
//
//    This form of control is beyond the context of this simulation.
//
//
//
//    Here, we model the AC motor.
//
//                                                                              
//
//    The following assumptions are made.
//
//                - The motor windings are placed in a perfectly sinusoidal distribution and
//                  are balanced.
//                - Detent torque is modeled as a sinusoidal disturbance that varies as a
//                  function of "2*Np*theta", independent of induced stator flux.
//                  (To interpert the exact model for the detent torque is a project in  
//                   itself).
//                  
//
//        Lm_a*dia/dt - Mab*dib/dt - Mac*dic/dt = vs_a - Ra*ia + Ka*omega*sin(Nr*theta) - v_n                   (1)
//      
//      - Mab*dia/dt + Lm_b*dib/dt - Mbc*dic/dt = vs_b - Rb*ib + Kb*omega*sin(Nr*theta - 2*PI/3) - v_n          (2)
//
//      - Mac*dia/dt - Mbc*dib/dt + Lm_c*dic/dt = vs_c - Rc*ic + Kc*omega*sin(Nr*theta - 4*PI/3) - v_n          (3)
//
//        Jm*domega/dt = - 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)                                       (4)
//
//        dtheta/dt = omega                                                                                     (5)
//
//
//                      where: 
//                             Bm - viscous friction coefficient
//                             Cm - coulomb friction coefficient
//                             Dm - detent torque coefficient (approx. representation)
//        
//                          
//      Before proceeding, it should be noted that equations (1), (2), (3) and (4) do not represent
//      the AC motor correctly (in a physical sence) even though the equations refect a valid model of
//      the motor.
//
//      For instance the "Kx*omega*sin(..." terms in (1), (2), and (3) should have a leading "-" sign.
//
//      Additionally, the "Kx*ix*sin(..." terms in (4) should have a leading "+" sign.
//
//      We will stay with the documented representation for (1), (2), (3), and (4) presented by Marc and John
//      because it refects the analysis already established.
//
//      However, for the purposes of clearity, we should fix these equations in the future.
//
//      Back to the analysis at hand...
//
//      In the actual implementation of the power stage, a single IPM (inteligent power module)
//      with a single shunt resistor attached to the minus bus rail will be controlled directly
//      by one FPGA that reference the low side control circuit of the IPM.
//
//      Notice the term "v_n" in Equations 1,2 and 3 above. Because the phase voltages "vs_a",
//      "vs_b", and "vs_c" are generated by a switching circuit (IPM), the assumption of the neutral
//      remaining at a constant value of zero voltage is invalid.
//
//
//      FIGURE 1
//
//     + Bus ------------------------+--------
//                                   |        |(Bus voltage sence)
//                  -----------------|        |
//                 |          --------------  |   
//                 |         |              | | vs_a             ----------
//                 |         |              |-------------------|          |           ----------
//               <Cap>       |              | | vs_b            |   AC     |          |          |
//                 |         |   IPM        |-------------------|  Motor   |----------| Encoder  |
//                 |         |              | | vs_c            |          |          |          |
//                 |    ---->|              |-------------------|          |           ----------
//                 |   |     |              | |                  ----------                 |
//                 |   |      --------------  |                                             |
//                  -----------------|        |     -----------            -----------      |
//                     |             |         --->|           |          |           |     |
//                     |             |             |           |          |           |<----
//                     |             +------------>|           |          |           |
//                     |           <Res>           |  FPGA     |          |  FPGA     |
//                     |             +------------>| (Power    | Opto.Iso.| (Control  |
//                     |             |             |  Side)    |<========>|  Side)    |
//      - Bus -----------------------              |           |          |           |
//                     |                           |           |          |           | 
//                     |         IPM Ctrl.         |           |          |           |
//                      ---------------------------|           |          |           |
//                                                 |           |          |           |
//                                                  -----------            -----------
//                                                                              |
//                                                                              |
//                                                                         -----------
//                                                      -----------       |   1394    |       -----------
//                          (From prev. drive) ======<>|Fibre Optic|<---->| Physical  |<---->|Fibre Optic|<>=====...(To next drive)
//                                                      -----------       |  Layer    |       -----------
//                                                                         -----------
//
//
//      In this simulation, the IPM switchs (IGBT's) are simulated as slew rate controlled voltage sources. 
//      This does not provide a true representation of the IPM switching characteristics. However, by the
//      fact that these sources are slew rate controlled can provide insight into the operation of the observer
//      algorythm for phase transitions of "vs_a", "vs_b", and "vs_c" that are very close to one another.
//
//      It must be keep in mind that for switch states of "vs_a" AND "vs_b" AND "vs_c" connected to the
//      + Bus or - Bus rail, no current flows through "<Res>". Any current flowing in "vs_a", "vs_b", and "vs_c"
//      during these states, circulates between the motor phases.
//
//
//      In the real implementation, capacitor <Cap> must be present to keep the IPM from picking up stray
//      transient voltages on it's control signals that are fed by the power side FPGA. The capacitor must
//      be large enough to control the transcients entering the "level shifting" logic of the IPM but
//      be small enough to not effect the current flow in <Res>.
//
//      The observer, (which will be implemented in the power side FPGA, uses the motor parameters
//      together with the current flow senced in <Res>, the bus voltage senced at "+ Bus", and the switching
//      states of "IPM Ctrl" to reconstruct the actual currents flowing in "vs_a", "vs_b" and "vs_c". Thus,
//      the sampling of the current through <Res> is valid only when "vs_a", "vs_b" and "vs_c" are connected
//      to the opposing bus rails in any combination.
//
//      For instance, if "vs_a" is connected to the + Bus rail and "vs_b" and "vs_c" are connected to
//      - Bus rail, the current through <Res> is equal to the current in "vs_a". At this point "vs_b"
//      and "vs_c" may be circulating current through one another, but the value of this cannot be determined
//      until "vs_b" and "vs_c" inturn are connected to the +/- bus rail.
//
//      The chalenge here is to reconstruct the phase current "ia", "ib", and "ic" from the sampled current
//      in <Res> in order to correct the observer model.
//
//
//      In addition, the observer will reconstruct the "w" of the motor using the position feedback
//      provided by the encoder passed through the control side FPGA and "Opto. Iso" (opto isolation) interface
//      connecting both FPGA's.
//
//      
//      The control of the IPM switches requires some special work. Under all operating conditions we must
//      be able to recontruct two (any two) out of three phase currents so that the observer can be updated
//      on a regular basis.
//
//
//      For the discussion that follows, refer the documents "GetTRDoc[1].pdf" and "SpaceVector_PWM_Inverter.pdf"
//
//      
//      "SpaceVector_PWM_Inverter.pdf" desribes the "classical" form of Space Vector PWM (SVPWM) where as document
//      "GetTRDoc[1].pdf" go a step further and describes variant forms of SVPWM that make improvements both on
//      switching efficiency and the reduction of harmonics.
//
//
//      Both aggree as far as the analytical derivation of the basic SVPWM algorythm.
//
//
//      Refering to Figure 8 of "SpaceVector_PWM_Inverter.pdf" and Figure 4 of "GetTRDoc[1].pdf", we first 
//      analyze the operation in Sector 1 in regards to the requirement that two out of three phase currents
//      must be sampled on periodic basis so as to correct the observer algorythm.
//
//
//      We must use a variant of the switch topology described in these documents to be able to measure 
//      two phase current under all operating conditions.
//
//      The problem here is two-fold. First, we have the problem that as the value of the Vref approachs
//      zero, so does the time durations of the non-zero vectors (all vectors except "111" and "000").
//      This is a point at which the time duration of the current flow through the shunt (sence) resistor 
//      is so small that even a fast sampling A/D cannot respond (sample) fast enought to get an accurate 
//      reading.
//
//      Secondly, as the angle "alpha" approachs the boundaries that seperate sectors 1,2,3,4,5 and 6, only
//      one motor phase current can be detected, no matter the value of Vref. This is true for the basic SVPWM
//      and it's variants described in these documents.
//
//      The second problem can be addressed by refering to Figure 8 of "SpaceVector_PWM_Inverter.pdf".
//      For example, the boundary at sector 1 and 6 (defined by V1), if one were to apply the SVPWM algorythm 
//      to vectors V2-V6 with the restriction that "alpha" ranges in the span of +/- 30 degrees instead of
//      vectors V1-V2 or vector V6-V1 where "alpha" spans the range of 0 to +59.999 degress and 320.0 to
//      359.999 degress respectively, then two phase currents (in this case phase b and c) can be sensed 
//      by the shunt for a total range of 60.0 degress. (That is, 60 degrees between the range of -30 to 
//      +30 degress).
//
//      A sequence that satisfy this requirement is as follows
//
//      TABLE 1 
//
//         330.0 to 29.99    30.0 to 89.99     90.0 to 149.99   150.0 to 209.99   210.0 to 269.99   270.0 to 329.99   
//         |0-6-2|2-6-0| ... |0-1-3|3-1-0| ... |0-2-4|4-2-0| ... |0-3-5|5-3-0| ... |0-4-6|6-4-0| ... |0-5-1|1-5-0|
//
//      Notice that only the "0" vector is used as the zero voltage state. The reason for this is two fold.
//
//      First, the low to mid range IPM's are designed to use "charge pump" supplies for the upper three
//      IGBT's. This is because these IPM's also employ high voltage level shifting technology so that
//      all six IGBT's can be controlled from the minus side (low side) reference to the IPM. (This can
//      be seen in the circuit diagram in FIGURE 1 above, where in a simplied sence, the first of the two FPGA's
//      are driving the control signals to the IPM directly).
//
//      Secondly, since charge pump supplies are being used, the low side IGBT's MUST drive for some
//      minimum time to keep the pumps charged. In a servo application where one of the many operating points
//      of the motor are low power at a stationary position, applied the "7" vector for an extended period
//      of time will drain the pumps, requiring them to enter (abeit for a short time), a recharge state.
//
//      In high performance servo applications, jumping in and out of this recharge state can cause position
//      loop instability. In addition viewed from a "common mode" aspect, capacitive coupling between the motor
//      frame and motor windings as well as capacitive coupling between motor power leads and the shielding
//      in the motor cable, can cause excessive trancients can be generated. In tradition sine wave generated
//      PWM, there are periods with in the switching cycle where ALL three sets of IGBT switchs are in a
//      high impedance state (known as the "dead time" regions).
//
//      This along with the common mode coupling can cause excessive RFI interference in the circuit.
//
//      The sequence FIGURE 1 is not optimum however. The first, third, and fifth series mandates that two switch
//      transistions occure simultanously, which effective double the switch frequencey.
//      This creates potentally, more EMI/RFI than the typical "clamped"  SVPWM sequences described in document 
//      "GetTRDoc[1].pdf".
//
//
//      Further, the maximum per-unit "circle" range of Vref now is now only .5 that of
//      of the Vref that can be generated by the traditional SVPWM sequence, where only vectors that are
//      ajacent to each other are switched.
//
//      To get the best of both worlds, we need to incorporate both switch topolgies to be able to acheive
//      maximum capability, efficiency, and effectiveness. To do this, dynamic switching between the
//      the two modes must be done at stratigic points of operation, dictated by both the state of the
//      motor and the state of the controller, together. Thus, we need to be able to switch between the series
//      in TABLE 1 above, and one of the two series dipicted in TABLE 2 below depending on the value of "alpha".
//      and the command voltage Vref. This, in order to produce more voltage on the stator windings of the motor.
//
//
//     
//      TABLE 2
//
//        330.0 to 359.99    30.0 to 59.99     90.0 to 119.99   150.0 to 179.99   210.0 to 239.99   270.0 to 299.99   
//         |0-1-6|6-1-0| ... |0-1-2|2-1-0| ... |0-3-2|2-3-0| ... |0-3-4|4-3-0| ... |0-5-4|4-5-0| ... |0-5-6|6-5-0|
//
//         0.0 to 29.99      60.0 to 89.99    120.0 to 149.99   180.0 to 209.99   240.0 to 269.99   300.0 to 329.99
//         |0-1-2|2-1-0| ... |0-3-2|2-3-0| ... |0-3-4|4-3-0| ... |0-5-4|4-5-0| ... |0-5-6|6-5-0| ... |0-1-6|6-1-0|
//
//      Note, that unlike the series in TABLE 1, the two sets of series's in TABLE 2 exhibit the contraint of only 
//      two switching states per cycle, while still maintaining the low side of the bus (the "0" vector) as a 
//      reference.
//
//      Further, the two switching states do not happen together, which is in the true spirit of the classical form of
//      the "low side clamped" SVPWM sequence, described in the two ".pdf" documents cited above.
//
//      As cited in document "GetTRDoc[1].pdf", as the power requirment for the motor increases, the "clamped" froms
//      of SVPWM (vector "0" and vector "7" based), begin to generate excessive ripple current in the stator
//      windings of the motor.
//  
//      So as described in this document, it would be prudent to switch to a third sequence of SVPWM which incorporates
//      both the "0" and the "7" vectors in each cycle, as illistrated in Table 3.
//
//      TABLE 3
//
//          330.0 to 359.99        30.0 to 59.99         90.0 to 119.99       150.0 to 179.99       210.0 to 239.99       270.0 to 299.99   
//         |0-1-6-7|7-6-1-0| ... |0-1-2-7|7-2-1-0| ... [0-3-2-7|7-2-3-0| ... [0-3-4-7|7-4-3-0| ... |0-5-4-7|7-4-5-0| ... |0-5-6-7|7-6-5-0|
//
//          0.0 to 29.99           60.0 to 89.99        120.0 to 149.99       180.0 to 209.99       240.0 to 269.99       300.0 to 329.99
//         |0-1-2-7|7-2-1-0| ... [0-3-2-7|7-2-3-0| ... [0-3-4-7|7-4-3-0| ... |0-5-4-7|7-4-5-0| ... |0-5-6-7|7-6-5-0| ... |0-1-6-7|7-6-1-0|
//
//      Note, like the series in TABLE 2, the two sets of series's in TABLE 3 exhibit the contraint of only 
//      two switch states per cycle. However, the capability of referencing the low side DC bus on a constant basis is
//      lost. Operating in this series usually indicates a "rapid traverse" operating condition for the motor which usually
//      does not require precise position tracking.
//
//      Now, back to the first problem mentioned above. How to handle the case when the magnitude of
//      Vref becomes so small that sampling of the shunt resistor becomes physically impossible, no matter
//      the values of time "T1" and "T2".
//
//      The initial approach to this problem will be based soley on logic, and may not be the ultimate
//      solution to this problem.
//
//      First, it must be stressed that the observer that is running on the first of the two FPGA's 
//      mentioned above (the FPGA that controls the IPM directly and sences the current the shunt
//      resistor) will be designed such that it is "corrected" not only by the current feedback
//      senced on the shunt, but also the position feedback (that is passed through by the second
//      FPGA described above).
//
//      Under these conditions, the observer will never loose position update information, and hence
//      will never loose knowledge of the "torque" and "reactive" components of the rotating D/Q axis.
//      Now, this along with the fact that we control Vref, T1 and T2 using vectors V1 through
//      V6 and we have an accurate meaure of DC bus voltage at all times, a knowledge of
//      output power is never lost at anytime. (This of course assumes the model
//      of the motor is accurate and no short circuit conditions exist in the system).
//
//      So, the logic is simple. Under the conditions of a low power mode to the motor, when it is determined
//      that both T1 and/or T2 do not have sufficient duration in which to sample the shunt resistor,
//      we stop sampling the shunt resistor (we stop sampling current), and rely soly on the position 
//      feedback, the DC Bus voltage value, and the commanded value of Vref, to update the observer.
//
//      If the design of the A/D circuitry sampling the shunt is done correctly, the minumum points for 
//      for which T1 and T2 are determined to be two small for sampling, will indeed be very small.
//
//      If in fact we assume that the switching frequency is 20 KHz and say we design the observer
//      to run at say 200 KHz, we can allow ourself a 10 X oversampling of the shunt resistor current.
//
//      Next, assume that we allow 50 uSec of delay (1 PWM cycle period) from the point of current
//      sample to the update of the observer.
//
//      In this case, we can allow a simple FIR filter to run between this delay increasing our
//      bit resolution of the sampled current by more then three bits. Then using a standand 12 bit A/D
//      sample and hold A/D (say Analog Devices AD9220), we can increase the current feedback resolution
//      to 15 bits, and easily require that T1 and T2 be of a minumum duration of 1 uSec. (Note: The
//      1 uSec duration is not a limitation of the A/D but a limitation based on the settling time
//      of the current in the shunt resistor,... a detailed discussion in itself, and out of the context
//      of this document).
//
//      Of course there is other tricks that can be done to expand the duration of T1 and T2 at low
//      values of Vref. For instance, the switch frequency can be lowered forcing T1 and T2 to
//      increase for a given value of Vref.
//
//      For general applications, this actually is a good approach since the benifit of reduced 
//      switching loss is gained.
//
//      However, for high performance servo motor applications, the problems assocated with changing
//      frequencies and the generation of additional sub-harmonics (which could be very low in
//      frequency), may cause more harm then good as far as maintaining stability of the servo control
//      portion of the algorygthm.
//
//      For this reason, (in dealing with high performance applications), I elect the former approach
//      described above.
//
//      Now, assume that his idea of stopping the sampling of current if T1 and T2 are determined too
//      small for sampling while depending soly on the DC Bus voltage feedback and position feedback
//      to correct the observer.
//
//      If this is true, can we devise a similar stratagy if say the angle "alpha" stops or slowly traverses  
//      one of the six vectors (V1 through V6), and we can only sample one phase (e.g., only T1 or T2
//      is present).
//      
//      If a stratagy exists, we can drop the need to generate the series (switching sequence) defined in
//      TABLE 1 above and depend soly on the two series define in TABLE 2 and TABLE 3.
//
//      For clarity, we will rewrite these sequences based on 60 degree sectors starting at 0.
//
//      TABLE 4
//
//          - Operation at low power (low speed) - 
//
//         0.0 to 59.99      60.0 to 119.99   120.0 to 179.99   180.0 to 239.99   240.0 to 299.99   300.0 to 359.99
//         |0-1-2|2-1-0| ... |0-3-2|2-3-0| ... |0-3-4|4-3-0| ... |0-5-4|4-5-0| ... |0-5-6|6-5-0| ... |0-1-6|6-1-0|
//
//          - Operation at high power (high speed) -
//
//          0.0 to 59.99           60.0 to 119.99       120.0 to 179.99       180.0 to 229.99       240.0 to 299.99       300.0 to 359.99
//         |0-1-2-7|7-2-1-0| ... [0-3-2-7|7-2-3-0| ... [0-3-4-7|7-4-3-0| ... |0-5-4-7|7-4-5-0| ... |0-5-6-7|7-6-5-0| ... |0-1-6-7|7-6-1-0|
//
//      We will go with this stratagy:
//
//
//           1)  If both T1 and T2 are too small to take a current sample, we depend soly on the DC bus voltage feedback
//               and position feedback to update the observer, as was descussed above.
//
//           2)  If it is determined that we cannot sample at T1 or T2 AND "dalpha/dt" (omega) is ABOVE a certain 
//               threshold, we do exactly as described in condition "1)" above. 
//
//           3)  If it is determined that we cannot sample at T1 or T2 AND "dalpha/dt" (omega) is BELOW a certain 
//               threshold, we determine which phase is conducting (connected between the + DC rail and - DC rail)
//               at T1 or T2 and take the sample. We then assume that because we are running at low speed,
//               the other two phase currents (that are not visible) must be equal to each other in direction and 
//               magnitude and must be flowing in the opposite direction at 1/2 the value of the phase 
//               that was just sampled. We then update all three currents in the observer algorythm.
//
//           4)  Otherwise, sample the current flowing at T1 and T2, determine which of the two phases that are
//               conducting at these times, and add them together to determine the current of the third phase
//               which is not visible. Then update all three currents in the observer algorythm.
//
//      It is not evident in this discussion, but should be made clear that the observer (which is running at least 
//      a 200 KHz update), must also have a knowledge (e.g., must sample) which ever of the two sequences 
//      of TABLE 4 that are operating at the given point in time. These sequences, along with the value of the DC
//      bus voltage provide an accurate representation of the applied voltages to the three motor phases.         
//
//      Also, the actual sampling of the current(s) are done at the end of T1 and T2 to minimize noise in the
//      readings due to the "shoot through" current transcients caused commutation IGBT's and "flyback" diodes
//      within the IPM.
//
//      Further, it should be assumed that condition "1), 2) and 3)" above occure only during low power (low speed)
//      operation defined by the first of the two sequences of TABLE 4.
//
//
//
//
//



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_FUNC_ISample,
  
} 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.hpp" 

//#define SPICE_TEST_MODE    //Only defined when making a verification test relative to SPICE file "App_Observer.cir" 

     // 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-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


#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       8
#define VDQ_CMD_QUANTUM_CNT       4
#define PWM_CYCLE_QUANTUM_CNT     4

#define QUANTUM_PERIOD .0000125   



     //Un-comment to allow PWM time period to be scaled dynamically.
//#define CTRL_PWM_SCALE

#ifdef CTRL_PWM_SCALE

           //NOTE: "PwmFreqScale" is a local variable defined in CtrlObject "VDqCmd" and SrcObject "TrangleWave"
           //      In this context, it is combined into definitions QUARTER_PWM_CYCLE, T_Z, PWM_GAIN and PWM_OFFSET.
           //      There is sourced by "VDqCmd" and use as an R-Value in "TriangleWave"
#define QUARTER_PWM_CYCLE (QUANTUM_PERIOD*PwmFreqScale) 
 
#else

    
     //Un-comment for low frequency test.                  
//#define LOW_FREQ_PWM_TEST

#ifdef LOW_FREQ_PWM_TEST
#define QUARTER_PWM_CYCLE (QUANTUM_PERIOD*10.0)   
#else
#define QUARTER_PWM_CYCLE QUANTUM_PERIOD
#endif 
     

#endif 


   

#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     .2      
#define  CT_TRAJ     .5
#define  VO_TRAJ   100.0





#ifdef SPICE_TEST_MODE
#define PWM_GAIN    5.0
#define PWM_OFFSET  0
#else

           //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 

// ---- MEA: This was added for the paper that will go into the MEA-Consulting.org WEB site. ----
//uncomment if we want to generate a 3-Level equivalent of the triangle wave reference mode.
#define TRIANGLE_REF_3_LEVEL_MODE


#ifdef TRIANGLE_REF_3_LEVEL_MODE

#define PWM_GAIN    10.0
#define PWM_OFFSET  (.5*PWM_GAIN)

#else

#define PWM_GAIN    5.0
#define PWM_OFFSET  0

#endif



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

	// ---- MEA: This was added for the paper that will go into the MEA-Consulting.org WEB site. ----
    //
	//        NOTE: Refer to "C:\Simulation-Development\Space-Vector-PWM-Docs-(Including_3_Level)" document "sprabs6-(Three-Level-Space-Vector).pdf" for a quick backround on how
    //                     to derive the algorithm for a three-level design. We will not implement this for the MEA-Consulting.org paper because of the difficultly involved.
    //
	//uncomment if we want to generate a 3-Level equivalent of the minus side clamped version of the "Space-Vector" MODE. Here the clamp is at the "zero" level of the 3-Level amplifier.
//#define SPACE_VECTOR_3_LEVEL_MODE

#endif 
 


#endif 


#define MIN_BUS_SAMPLE_TIME_CHANGE  .000001   //we select 1 uSec as are minimum bus current sample interval.  

#define OMEGA_SAMPLE_LIMIT  0
#define ALPHA_SAMPLE_LIMIT  0

#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;


typedef enum {BSS_000,
	      BSS_100,
	      BSS_010,
	      BSS_001,
	      BSS_110,
	      BSS_011,
	      BSS_101,
	      BSS_111
} BUS_SAMPLE_STATE;

           //SAMPLE STATES:
           //BSA_<A_cur><B_cur><C_cur>_<A_prev><B_prev><C_prev>, 
           //BSA_<A_cur><B_cur><C_cur>_ZERO (where ZERO == 111 or 000), BSA_NONE (hold current values), BSA_ILLEGAL (error trap)
           
typedef enum {BSA_NONE,          //no state change and no current measured.
	      BSA_000_100,
	      BSA_000_010,
	      BSA_000_001,
	      BSA_000_110,
	      BSA_000_011,
	      BSA_000_101,

	      BSA_111_100,
	      BSA_111_010,
	      BSA_111_001,
	      BSA_111_110,
	      BSA_111_011,
	      BSA_111_101,




	      BSA_100_ZERO,      //state change but no current measured
	      BSA_100_110,
	      BSA_100_101,
	      BSA_010_ZERO,      //state change but no current measured
	      BSA_010_110,
	      BSA_010_011,
	      BSA_001_ZERO,      //state change but no current measured
	      BSA_001_011,
	      BSA_001_101,
	      BSA_110_ZERO,      //state change but no current measured
	      BSA_110_100,
	      BSA_110_010,
	      BSA_011_ZERO,      //state change but no current measured
	      BSA_011_010,
	      BSA_011_001,
	      BSA_101_ZERO,      //state change but no current measured
	      BSA_101_100,
	      BSA_101_001,
	      BSA_ILLEGAL        //illegal state
	      
} BUS_SAMPLE_ACTION;





// **** 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.
                 //In "SPICE_TEST_MODE", this is simply the reference on the triangle wave
                 //carrier. (see document "GetTRDoc[1].pdf", Figure 11 and Figure 12).
 
};



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.
                 //In "SPICE_TEST_MODE", this is simply the reference on the triangle wave
                 //carrier. (see document "GetTRDoc[1].pdf", Figure 11 and Figure 12).

};



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.
                 //In "SPICE_TEST_MODE", this is simply the reference on the triangle wave
                 //carrier. (see document "GetTRDoc[1].pdf", Figure 11 and Figure 12). 
 

};



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 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 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 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);
  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;

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

// ---- ISample -------------------------------------------

class ISample : public OdeObject
{
public:
  //The PostOdeFunction() is used to simulate the process of sampling the bus current
  //to determine descrete phase currents "ia_bus", "ib_bus", and "ic_bus". The OdeFunction()
  //is not used in this ODE.
  virtual void PostOdeFunction(double t);
  virtual void CtrlRValueUpdate(void);
  virtual void RecordProbes(void);
  virtual void PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[]);
  ISample(void);
  ~ISample(void);
  //source for this ODE
  double ia;
  double ib;
  double ic;
  double TriAngWave;
  double V_xo[3];  
  double omega;
  double alpha;
  BUS_SAMPLE_STATE PrevBusSampleState;
  BUS_SAMPLE_ACTION PrevBusSampleAction; 
  double PrevBusSampleTime;
  double ia_bus;
  double ib_bus;
  double ic_bus;
  double ia_bus_internal;
  double ib_bus_internal;
  double ic_bus_internal;
  //storage for probes...
  vector<double> Ia_bus;
  vector<double> Ib_bus;
  vector<double> Ic_bus;

};

ISample Inst_ISample;

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



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,
			       (OdeObject *) &Inst_ISample,
			       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 "minus clamped" mode (minus side).
  bool PlusClampedSVPwm;
  //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;  




};








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;
  OdeObjItem * pCurOdeItem;

  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];

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

  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_ISample];



  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;


  //Update Rvalues
  pCurOdeItem = pOdeObjRValList;

  ((ISample *) pCurOdeItem->pOdeObject)->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)
{

#ifdef SPICE_TEST_MODE

  //For SPICE verification test, "V_xo" is simply a reference to a triangle wave
  //running between +/- PWM_GAIN*.5
  y = V_xo > TriAngWave ? 1.0 : 0;

#else

#ifdef  TRIANGLE_REF_3_LEVEL_MODE

  if(V_xo > 0)
  {
	  y = V_xo > TriAngWave ? 1.0 : 0;

  }
  else if(V_xo < 0)
  {
	  y = V_xo < (-TriAngWave) ? -1.0 : 0;

  }
  else
  {
	  y = 0;
  }

#else

  //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;

#endif

#endif 


}

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)
{

#ifdef SPICE_TEST_MODE

  //For SPICE verification test, "V_xo" is simply a reference to a triangle wave
  //running between +/- PWM_GAIN*.5
  y = V_xo > TriAngWave ? 1.0 : 0;

#else

#ifdef  TRIANGLE_REF_3_LEVEL_MODE

  if(V_xo > 0)
  {
	  y = V_xo > TriAngWave ? 1.0 : 0;

  }
  else if(V_xo < 0)
  {
	  y = V_xo < (-TriAngWave)  ? -1.0 : 0;

  }
  else
  {
	  y = 0;
  }

#else

   //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;

#endif

#endif 



}

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)
{

#ifdef SPICE_TEST_MODE

  //For SPICE verification test, "V_xo" is simply a reference to a triangle wave
  //running between +/- PWM_GAIN*.5
  y = V_xo > TriAngWave ? 1.0 : 0;

#else

#ifdef  TRIANGLE_REF_3_LEVEL_MODE

  if(V_xo > 0)
  {
	  y = V_xo > TriAngWave ? 1.0 : 0;

  }
  else if(V_xo < 0)
  {
	  y = V_xo < (-TriAngWave)  ? -1.0 : 0;

  }
  else
  {
	  y = 0;
  }

#else

  //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;

#endif

#endif 


}

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;

  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];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_ISample];

  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
#ifdef  TRIANGLE_REF_3_LEVEL_MODE
  if(pwm_sig > 0)
  {
	  vs_a = ShapeSquareWaveSource((.5 *DC_BUS_VOLTAGE) , SRC_SLEW_RATE ,t);
  }
  else if(pwm_sig < 0)
  {
	  vs_a = ShapeSquareWaveSource((-.5 *DC_BUS_VOLTAGE) , SRC_SLEW_RATE ,t);
  }
  else
  {
	  vs_a = ShapeSquareWaveSource(0, SRC_SLEW_RATE ,t);
  }
#else
  vs_a = ShapeSquareWaveSource(((pwm_sig > 0) ? DC_BUS_VOLTAGE : 0), SRC_SLEW_RATE ,t);
#endif
#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[])
{
  //Note: This instance is the first of the group. As such, this
  //      is where the "group solve" of the individual 
  //      "dy/dt's" is performed.

  //
  //   We must solve for dia/dt, dib/dt, dic/t and v_n in (1), (2), and (3)
  //   where the right side of these equations is passed to us, the values
  //   in "dmdt[]". The "dydt[]" array hold the results. The value "v_n"
  //   is an intermediate value need to solve for dia/dt, dib/dt, and dic/d.
  //
  //
  //   Let
  //         dia = dia/dt, dib = dib/dt and dic = dic/dt
  //         
  //
  //       Lm_a*dia - Mab*dib - Mac*dic + v_n = dmdt[0]       (a)
  //
  //       - Mab*dia + Lm_b*dib - Mbc*dic + v_n = dmdt[1]     (b)
  //
  //       - Mac*dia - Mbc*dib + Lm_c*dic + v_n = dmdt[2]     (c)
  //
  //
  //
  //   Solve for dia in (b) and (c) and subtract (c) from (b)
  //
  //
  //          
  //       dia = (Lm_b*dib - Mbc*dic + v_n - dmdt[1])/Mab            
  //
  //       dia = (- Mbc*dib + Lm_c*dic + v_n - dmdt[2])/Mac          
  //
  //       (Lm_b*dib - Mbc*dic + v_n - dmdt[1])/Mab = (- Mbc*dib + Lm_c*dic + v_n - dmdt[2])/Mac 
  //
  //       (Lm_b/Mab + Mbc/Mac)*dib + (- Lm_c/Mac - Mbc/Mab)*dic = - v_n/Mab + dmdt[1]/Mab + v_n/Mac - dmdt[2]/Mac       (d)
  //
  //
  //
  //
  //
  //   Solve for dia in (a) and (b) and subtract (b) from (a)
  //
  //
  //       dia = (Mab*dib + Mac*dic - v_n + dmdt[0])/Lm_a
  //
  //       dia = (Lm_b*dib - Mbc*dic + v_n - dmdt[1])/Mab  
  //     
  //       (Mab*dib + Mac*dic - v_n + dmdt[0])/Lm_a = (Lm_b*dib - Mbc*dic + v_n - dmdt[1])/Mab 
  //
  //       (Mab/Lm_a - Lm_b/Mab)*dib + (Mac/Lm_a + Mbc/Mab)*dic = v_n/Lm_a + v_n/Mab - dmdt[0]/Lm_a - dmdt[1]/Mab       (e)
  //
  //
  //
  //
  //       Simplify (d) and (e)   
  //
  //       A1*dib + B1*dic = C1                 (f)           
  //
  //       A2*dib + B2*dic = C2                 (g)
  //
  //                 Where...
  //
  //       A1 = (Lm_b/Mab + Mbc/Mac)       
  //       B1 = (- Lm_c/Mac - Mbc/Mab)
  //       C1 = - v_n/Mab + dmdt[1]/Mab + v_n/Mac - dmdt[2]/Mac
  //       A2 = (Mab/Lm_a - Lm_b/Mab)
  //       B2 = (Mac/Lm_a + Mbc/Mab)
  //       C2 = v_n/Lm_a + v_n/Mab - dmdt[0]/Lm_a - dmdt[1]/Mab
  //
  //
  //
  //
  //       Solve for dib and dic, (g) into (f) 
  //
  //       A1*dib + B1*(C2 - A2*dib)/B2 = C1
  //
  //       A1*dib - B1*A2*dib/B2 = C1 - B1*C2/B2
  //
  //       dib = (C1 - B1*C2/B2)/(A1 - B1*A2/B2) = (B2*C1 - B1*C2)/(B2*A1 - B1*A2)     (h)
  //           
  //       Note that (h) can be simplified further (see below)
  //
  //       And (h) into (g)
  //
  //       A2*(B2*C1 - B1*C2)/(B2*A1 - B1*A2) + B2*dic = C2
  //
  //       dic = (C2 - A2*(B2*C1 - B1*C2)/(B2*A1 - B1*A2))/B2 = (C2 - A2*dib)/B2       (i)
  //
  //       (h) and (i) into (a) and solve for dia
  //
  //       dia = (Mab*dib + Mac*dic - v_n + dmdt[0])/Lm_a                              (j)
  //
  //
  //
  //
  //       Now, using the identity,
  //
  //           dia + dib + dic = 0   and (h), (i), (j)
  //
  //       Solve for v_n
  //
  //       (Mab*(B2*C1 - B1*C2)/(B2*A1 - B1*A2) + Mac*(C2 - A2*(B2*C1 - B1*C2)/(B2*A1 - B1*A2))/B2 - v_n + dmdt[0])/Lm_a + 
  //       (B2*C1 - B1*C2)/(B2*A1 - B1*A2) + 
  //       (C2 - A2*(B2*C1 - B1*C2)/(B2*A1 - B1*A2))/B2 
  //       = 0                                                                         
  //
  //       Simplify 
  //
  //       (Mab*E/D + Mac*(C2 - A2*E/D)/B2 - v_n + dmdt[0])/Lm_a + 
  //       E/D + 
  //       (C2 - A2*E/D)/B2 
  //       = 0  
  //
  //                Where...
  //                    
  //                       D = (B2*A1 - B1*A2)                                  (k)                        
  //                       E = (B2*C1 - B1*C2)
  //      
  //       Expand
  //
  //       Mab*E/(D*Lm_a) + Mac*C2/(B2*Lm_a) - Mac*A2*E/(D*B2*Lm_a) - v_n/Lm_a + dmdt[0]/Lm_a + E/D + C2/B2 - A2*E/(D*B2) = 0
  //
  //       And
  //
  //       Mab*E/(D*Lm_a) + Mac*C2/(B2*Lm_a) - Mac*A2*E/(F*Lm_a) - v_n/Lm_a + dmdt[0]/Lm_a + E/D + C2/B2 - A2*E/F = 0 
  //
  //                Where...
  //              
  //                       F = (D*B2)
  //
  //       Group C2 and E expressions to the left
  //        
  //
  //       Mab*E/(D*Lm_a) - Mac*A2*E/(F*Lm_a) + E/D - A2*E/F + Mac*C2/(B2*Lm_a) + C2/B2  = v_n/Lm_a - dmdt[0]/Lm_a 
  //
  //       E*(Mab/(D*Lm_a) - Mac*A2/(F*Lm_a) + 1/D - A2/F) + C2*(Mac/(B2*Lm_a) + 1/B2)  = v_n/Lm_a - dmdt[0]/Lm_a 
  //
  //       And
  //
  //       E*G + C2*H = v_n/Lm_a - dmdt[0]/Lm_a 
  //
  //               Where...
  //
  //                       G = (Mab/(D*Lm_a) - Mac*A2/(F*Lm_a) + 1/D - A2/F)
  //                       H = (Mac/(B2*Lm_a) + 1/B2)
  //
  //
  //       Expand E and group terms
  //
  //       (B2*C1 - B1*C2)*G + C2*H = v_n/Lm_a - dmdt[0]/Lm_a 
  //   
  //       C1*B2*G + C2*(H - B1*G) = v_n/Lm_a - dmdt[0]/Lm_a  
  //
  //
  //       Expand C1 and C2 and group terms
  //
  //       (- v_n/Mab + dmdt[1]/Mab + v_n/Mac - dmdt[2]/Mac)*B2*G + (v_n/Lm_a + v_n/Mab - dmdt[0]/Lm_a - dmdt[1]/Mab)*(H - B1*G) = 
  //                  v_n/Lm_a - dmdt[0]/Lm_a                         
  //
  //       v_n*(-B2*G/Mab + B2*G/Mac + (H - B1*G)/Lm_a + (H - B1*G)/Mab - 1/Lm_a) = (dmdt[2]/Mac - dmdt[1]/Mab)*B2*G +
  //                                                       (dmdt[0]/Lm_a + dmdt[1]/Mab)*(H - B1*G) - dmdt[0]/Lm_a 
  //
  //
  //       Finally,
  //
  //
  //       v_n = ((dmdt[2]/Mac - dmdt[1]/Mab)*B2*G + (dmdt[0]/Lm_a + dmdt[1]/Mab)*(H - B1*G) - dmdt[0]/Lm_a)/ 
  //                     (-B2*G/Mab + B2*G/Mac + (H - B1*G)/Lm_a + (H - B1*G)/Mab - 1/Lm_a)                     (l)
  //
  //
  //       Note that (h) and be further simplified by (k).
  //
  //       dib = (B2*C1 - B1*C2)/D
  //
  //       And (l) can be reduced further
  //
  //       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)   
  //
  //
  //           Where...
  //
  //                   I = (H - B1*G)
  //                   J = (B2*G)
  //
  //

  
#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;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((ISample *) pCurOdeItem->pOdeObject)->ia = y;
  
}

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


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



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

  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];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_ISample];

  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
#ifdef  TRIANGLE_REF_3_LEVEL_MODE
  if(pwm_sig > 0)
  {
	  vs_b = ShapeSquareWaveSource((.5 *DC_BUS_VOLTAGE) , SRC_SLEW_RATE ,t);
  }
  else if(pwm_sig < 0)
  {
	  vs_b = ShapeSquareWaveSource((-.5 *DC_BUS_VOLTAGE) , SRC_SLEW_RATE ,t);
  }
  else
  {
	  vs_b = ShapeSquareWaveSource(0, SRC_SLEW_RATE ,t);
  }
#else
  vs_b = ShapeSquareWaveSource(((pwm_sig > 0) ? DC_BUS_VOLTAGE : 0), SRC_SLEW_RATE ,t);
#endif
#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;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((ISample *) pCurOdeItem->pOdeObject)->ib = y;
  
}





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


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


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

  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];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_ISample];



  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
#ifdef  TRIANGLE_REF_3_LEVEL_MODE
  if(pwm_sig > 0)
  {
	  vs_c = ShapeSquareWaveSource((.5 *DC_BUS_VOLTAGE) , SRC_SLEW_RATE ,t);
  }
  else if(pwm_sig < 0)
  {
	  vs_c = ShapeSquareWaveSource((-.5 *DC_BUS_VOLTAGE) , SRC_SLEW_RATE ,t);
  }
  else
  {
	  vs_c = ShapeSquareWaveSource(0, SRC_SLEW_RATE ,t);
  }
#else
  vs_c = ShapeSquareWaveSource(((pwm_sig > 0) ? DC_BUS_VOLTAGE : 0), SRC_SLEW_RATE ,t);
#endif
#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;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((ISample *) pCurOdeItem->pOdeObject)->ic = y;
  
}

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

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

Omega::Omega(void)
{

  OdeObjItem * pCurOdeItem;

  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];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_ISample];
  pCurOdeItem->pNextOdeItem = new OdeObjItem;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  pCurOdeItem->pOdeObject = OdeObjectList[ODE_FUNC_ISample];

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

}

Omega::~Omega(void)
{

}

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

#ifdef SPICE_TEST_MODE

  return(0);

#else

  //#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

         //(- 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))/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))/Jm); 

#endif 

#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;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((ISample *) pCurOdeItem->pOdeObject)->omega = y;
  pCurOdeItem = pCurOdeItem->pNextOdeItem;       
  ((ISample *) pCurOdeItem->pOdeObject)->alpha = dydt;


}


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




// ---- 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];


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

}

Theta::~Theta(void)
{

}

double Theta::OdeFunction(double y, double t)
{
 
#ifdef SPICE_TEST_MODE 
  return(0);
#else 

     //omega;
  return(omega); 
#endif 
}

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;


}


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


// ---- 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;
      }
    }

  }
}

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



// ---- ISample  ----------------------------------------------------

ISample::ISample(void)
{
  //  CtrlObjItem * pCurCtrlItem;  //!!!! will ultimately point to the "Observer" CTRL !!!!
 
  PrevBusSampleState = BSS_000;
  PrevBusSampleAction = BSA_NONE;
  PrevBusSampleTime = 0;

  OdeFuncName = ODE_FUNC_ISample;
  LiteralName = "ISample";    

  DoProbes = TRUE;

}
 

ISample::~ISample(void)
{

}

void ISample::PostOdeFunction(double t)
{
  bool IaSampleValid = FALSE;
  bool IbSampleValid = FALSE;
  bool IcSampleValid = FALSE;
  bool AllowSectorAveraging = FALSE;
  BUS_SAMPLE_STATE BusSampleState;
  BUS_SAMPLE_ACTION BusSampleAction;
  double t_prev;


#define  ENABLE_BUS_SAMPLING   //un-comment to enable bus current sampling.

#ifdef ENABLE_BUS_SAMPLING

  //un-comment to enable determination of "non-observable" phase from present and previous samples.
#define ESTIMATE_PHASE
  
  //un-comment to show only the part of the algorythm that estimates the "non-observable" phase from the present and previous samples.
              //("ESTIMATE_PHASE must be defined above to use this option.)
  //#define ONLY_ESTIMATE_PHASE    

  if(V_xo[0] > TriAngWave){
    IaSampleValid = TRUE;
  }
  if(V_xo[1] > TriAngWave){
    IbSampleValid = TRUE;
  }
  if(V_xo[2] > TriAngWave){
    IcSampleValid = TRUE;
  }




  //forms are BSS_<A><B><C>
  if(IaSampleValid && IbSampleValid && IcSampleValid){
    //We have a "0" vector (all low) 
    BusSampleState = BSS_111;
  } 
  else if(!IaSampleValid && !IbSampleValid && !IcSampleValid){
    //We have a "0" vector (all high) 
    BusSampleState = BSS_000;
  }
  else if(IaSampleValid && IbSampleValid){
    BusSampleState = BSS_110;
  }
  else if(IbSampleValid && IcSampleValid){
    BusSampleState = BSS_011;
  } 
  else if(IaSampleValid && IcSampleValid){
    BusSampleState = BSS_101;
  } 
  else if(IaSampleValid){
    BusSampleState = BSS_100;
  }    
  else if(IbSampleValid){
    BusSampleState = BSS_010;
  } 
  else{
    BusSampleState = BSS_001;
  } 
  



  if(BusSampleState == PrevBusSampleState){
    return;
  }
  else{




    t_prev = PrevBusSampleTime;
   
    if((t - t_prev) <  MIN_BUS_SAMPLE_TIME_CHANGE){
      return;
    }
    PrevBusSampleTime = t;
  }


           //SAMPLE STATES:
           //BSA_<A_cur><B_cur><C_cur>_<A_prev><B_prev><C_prev>, 
           //BSA_<A_cur><B_cur><C_cur>_ZERO (where ZERO == 111 or 000), BSA_NONE (hold current values), BSA_ILLEGAL (error trap)


  switch(BusSampleState){
  case BSS_000:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_100:
      BusSampleAction = BSA_000_100;
      break;
    case BSS_010:
      BusSampleAction = BSA_000_010;
      break;
    case BSS_001:
      BusSampleAction = BSA_000_001;
      break;
    case BSS_110:
      BusSampleAction = BSA_000_110;
      break;
    case BSS_011:
      BusSampleAction = BSA_000_011;
      break;
    case BSS_101:
      BusSampleAction = BSA_000_101;
      break;
    case BSS_111:
      BusSampleAction = BSA_NONE;
      break;
    }
    break;
  case BSS_100:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_100_ZERO;
      break;
    case BSS_100:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_010:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_001:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_110:
      BusSampleAction = BSA_100_110;
      break;
    case BSS_011:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_101:
      BusSampleAction = BSA_100_101;
      break;
    case BSS_111:
      BusSampleAction = BSA_100_ZERO;
      break;
    }
    break;
  case BSS_010:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_010_ZERO;
      break;
    case BSS_100:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_010:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_001:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_110:
      BusSampleAction = BSA_010_110;
      break;
    case BSS_011:
      BusSampleAction = BSA_010_011;
      break;
    case BSS_101:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_111:
      BusSampleAction = BSA_010_ZERO;
      break;
    }
    break;
  case BSS_001:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_001_ZERO;
      break;
    case BSS_100:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_010:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_001:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_110:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_011:
      BusSampleAction = BSA_001_011;
      break;
    case BSS_101:
      BusSampleAction = BSA_001_101;
      break;
    case BSS_111:
      BusSampleAction = BSA_001_ZERO;
      break;
    }                                      
    break;
  case BSS_110:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_110_ZERO;
      break;
    case BSS_100:
      BusSampleAction = BSA_110_100;
      break;
    case BSS_010:
      BusSampleAction = BSA_110_010;
      break;
    case BSS_001:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_110:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_011:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_101:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_111:
      BusSampleAction = BSA_110_ZERO;
      break;
    }
    break;
  case BSS_011:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_011_ZERO;
      break;
    case BSS_100:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_010:
      BusSampleAction = BSA_011_010;
      break;
    case BSS_001:
      BusSampleAction = BSA_011_001;
      break;
    case BSS_110:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_011:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_101:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_111:
      BusSampleAction = BSA_011_ZERO;
      break;
    }
    break;
  case BSS_101:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_101_ZERO;
      break;
    case BSS_100:
      BusSampleAction = BSA_101_100;
      break;
    case BSS_010:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_001:
      BusSampleAction = BSA_101_001;
      break;
    case BSS_110:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_011:
      BusSampleAction = BSA_ILLEGAL;
      break;
    case BSS_101:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_111:
      BusSampleAction = BSA_101_ZERO;
      break;
    }
    break;
  case BSS_111:
    switch(PrevBusSampleState){
    case BSS_000:
      BusSampleAction = BSA_NONE;
      break;
    case BSS_100:
      BusSampleAction = BSA_111_100;
      break;
    case BSS_010:
      BusSampleAction = BSA_111_010;
      break;
    case BSS_001:
      BusSampleAction = BSA_111_001;
      break;
    case BSS_110:
      BusSampleAction = BSA_111_110;
      break;
    case BSS_011:
      BusSampleAction = BSA_111_011;
      break;
    case BSS_101:
      BusSampleAction = BSA_111_101;
      break;
    case BSS_111:
      BusSampleAction = BSA_NONE;
      break;
    }
    break;
  }
  //record present state for next sample.
  PrevBusSampleState = BusSampleState;




#if ((defined ESTIMATE_PHASE) && (!defined ONLY_ESTIMATE_PHASE ))
  if((omega > - OMEGA_SAMPLE_LIMIT) && (omega <  OMEGA_SAMPLE_LIMIT) &&
     (alpha > - ALPHA_SAMPLE_LIMIT) && (alpha <  ALPHA_SAMPLE_LIMIT)){
    AllowSectorAveraging = TRUE;
  }
#endif 

  switch(BusSampleAction){
  case BSA_000_100:
  case BSA_111_011:

  case BSA_010_011:
  case BSA_001_011:
  case BSA_110_100:
  case BSA_101_100:
#ifdef ONLY_ESTIMATE_PHASE
    ia_bus_internal = ia;
#else
    ia_bus = ia;
#endif 
#ifdef ESTIMATE_PHASE
    switch(PrevBusSampleAction){

    case BSA_000_010:
    case BSA_111_010:

    case BSA_000_101:
    case BSA_111_101:


    case BSA_100_101:
    case BSA_001_101:
    case BSA_110_010:
    case BSA_011_010:
#ifdef ONLY_ESTIMATE_PHASE
      ic_bus = - (ib_bus_internal + ia_bus_internal);      
#else
      ic_bus = - (ib_bus + ia_bus);
#endif 
    break;

    case BSA_000_001:
    case BSA_111_001:

    case BSA_000_110:
    case BSA_111_110:


    case BSA_100_110:
    case BSA_010_110:
    case BSA_011_001:
    case BSA_101_001:
#ifdef ONLY_ESTIMATE_PHASE
      ib_bus = - (ic_bus_internal + ia_bus_internal);
#else
      ib_bus = - (ic_bus + ia_bus);
#endif 
      break;
    }
#endif 
    break;

  case BSA_000_010:
  case BSA_111_101:

  case BSA_100_101:
  case BSA_001_101:
  case BSA_110_010:
  case BSA_011_010:
#ifdef ONLY_ESTIMATE_PHASE
    ib_bus_internal = ib;
#else
    ib_bus = ib;
#endif 
#ifdef ESTIMATE_PHASE
    switch(PrevBusSampleAction){

    case BSA_000_100:
    case BSA_111_100:   

    case BSA_000_011:
    case BSA_111_011:  

    case BSA_010_011:
    case BSA_001_011:
    case BSA_110_100:
    case BSA_101_100:
#ifdef ONLY_ESTIMATE_PHASE
      ic_bus = - (ia_bus_internal + ib_bus_internal);
#else
      ic_bus = - (ia_bus + ib_bus);
#endif 
      break;

    case BSA_000_001:
    case BSA_111_001:

    case BSA_111_110:
    case BSA_000_110:
      
    case BSA_100_110:
    case BSA_010_110:
    case BSA_011_001:
    case BSA_101_001:
#ifdef ONLY_ESTIMATE_PHASE
      ia_bus = - (ic_bus_internal + ib_bus_internal);
#else
      ia_bus = - (ic_bus + ib_bus);
#endif 
      break;
    }
#endif 
    break;


  case BSA_000_001:
  case BSA_111_110:

  case BSA_100_110:
  case BSA_010_110:
  case BSA_011_001:
  case BSA_101_001:
#ifdef ONLY_ESTIMATE_PHASE
    ic_bus_internal = ic;
#else
    ic_bus = ic;
#endif 
#ifdef ESTIMATE_PHASE
    switch(PrevBusSampleAction){


    case BSA_000_100: 
    case BSA_111_100: 

    case BSA_000_011: 
    case BSA_111_011:

    case BSA_010_011:
    case BSA_001_011:
    case BSA_110_100:
    case BSA_101_100:
#ifdef ONLY_ESTIMATE_PHASE
      ib_bus = - (ia_bus_internal + ic_bus_internal);
#else
      ib_bus = - (ia_bus + ic_bus);
#endif 
      break;



    case BSA_000_010:
    case BSA_111_010:

    case BSA_000_101:
    case BSA_111_101:

    case BSA_100_101:
    case BSA_001_101:
    case BSA_110_010:
    case BSA_011_010:
#ifdef ONLY_ESTIMATE_PHASE
      ia_bus = - (ib_bus_internal + ic_bus_internal);
#else
      ia_bus = - (ib_bus + ic_bus);
#endif 
      break;
    }
#endif 
    break;

  case BSA_000_110:
  case BSA_111_001:
#ifdef ONLY_ESTIMATE_PHASE
    ic_bus_internal = ic;
#else
    ic_bus = ic;
#endif 
    if(AllowSectorAveraging){
      //half the sample, negate and assign to the other two.
      ia_bus = ib_bus = -.5*ic;
    }
    break;
  case BSA_000_011:
  case BSA_111_100:
#ifdef ONLY_ESTIMATE_PHASE
    ia_bus_internal = ia;
#else
    ia_bus = ia;
#endif
    if(AllowSectorAveraging){     
      //half the sample, negate and assign to the other two.
      ib_bus = ic_bus = -.5*ia;
    }
    break;
  case BSA_000_101:
  case BSA_111_010:
#ifdef ONLY_ESTIMATE_PHASE
    ib_bus_internal = ib;
#else
    ib_bus = ib;
#endif 
    if(AllowSectorAveraging){     
      //half the sample,negate and assign to the other two.
      ia_bus = ic_bus = -.5*ib;
    }
    break;      
  case BSA_ILLEGAL:
    //(Under certain conditions, we are getting a trap. We will fix this later).
    //    Exception("Illegal Space Vector sequence detected.");
    break;
  }
  //record present action for next sample.
  PrevBusSampleAction = BusSampleAction;



#endif 








}


void ISample::RecordProbes(void)
{
#ifdef ENABLE_BUS_SAMPLING

  Ia_bus.push_back(ia_bus);
  Ib_bus.push_back(ib_bus);
  Ic_bus.push_back(ic_bus);

#endif 

}

void ISample::PlotProbes(Gnuplot & SimuPlot, vector<double> & Plot_t, string TagNamesToPlot[])
{
  int i;

#ifdef ENABLE_BUS_SAMPLING
  if(TagNamesToPlot[0] == ""){
    SimuPlot.plot_xy(Plot_t, Ia_bus, "Ia_bus");
    SimuPlot.plot_xy(Plot_t, Ib_bus, "Ib_bus");
    SimuPlot.plot_xy(Plot_t, Ic_bus, "Ic_bus");
  }
  else{
    for(i = 0; i < 20; i++){
      if(TagNamesToPlot[i] == "Ia_bus"){
	SimuPlot.plot_xy(Plot_t,Ia_bus, "Ia_bus");
      }
      else if(TagNamesToPlot[i] == "Ib_bus"){
	SimuPlot.plot_xy(Plot_t, Ib_bus, "Ib_bus");
      }
      else if(TagNamesToPlot[i] == "Ic_bus"){
	SimuPlot.plot_xy(Plot_t, Ic_bus, "Ic_bus");
      }
      else if(TagNamesToPlot[i] == ""){
	break;
      }
    }

  }

#endif 


}

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

  //Update Rvalues
  pCurCtrlItem = pCtrlObjRValList;

  //!!!! need to build the RValue list for "ia_bus", "ib_bus" and ic_bus" for the Observer CTRL !!!

}

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

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

// ***** 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 "minus clamped" mode.
  PlusClampedSVPwm = 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;

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

#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.

#ifdef SPICE_TEST_MODE

  //For SPICE verification test, we simply send a  reference to be compared
  //to a triangle wave running between +/- PWM_GAIN*.5. 

  V_xo[0] = .23;
  V_xo[1] = 0;
  V_xo[2] = 0;
    

#else
  //For simulation mode, we generate space vector PWM references.

  //#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

  //#define FSF_CTRL_VDQ_COMMAND
#ifdef FSF_CTRL_VDQ_COMMAND

  //"vcmd_mag" and "vcmd_ang" will be generated from signals "vd" and "vq", the command voltages
  //originating form CtrlObject 'FSFCtrl".

#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 

#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 



#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;

  ((ISample *) pCurOdeItem->pOdeObject)->V_xo[0] = V_xo[0];
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  ((ISample *) pCurOdeItem->pOdeObject)->V_xo[1] = V_xo[1];
  pCurOdeItem = pCurOdeItem->pNextOdeItem;
  ((ISample *) pCurOdeItem->pOdeObject)->V_xo[2] = V_xo[2];

#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) 
{
  // Support function for generating Space Vector PWM sequences.
  //
  // For references, refer to:
  //
  //   - Figure 11 and Figure 12 of "GetTRDoc[1].pdf" or Figure 11 of "SpaceVector_PWM_Inverter.pdf"
  //   - Figure 4 of "GetTRDoc[1].pdf" or Figure 8 of "SpaceVector_PWM_Inverter.pdf"
  //   - Section 2.2.1 and 2.2.2 (with Figures 9 and 10) of "SpaceVector_PWM_Inverter.pdf" or 
  //         Equation (1) of "GetTRDoc[1].pdf"
  //
  //  
  // From Section 2.2.2 of "SpaceVector_PWM_Inverter.pdf", the equations for determining T1, T2, and T0
  // for any sector are as follows:
  //
  //
  // Parameter definitions:
  //
  //      "theta", "omega", "alpha" are the angular position, velocity, and acceleration of the
  //      motor shaft. omega and alpha will be compensation values for correcting the position of the motor shaft
  //      at the sample points when the motor is moving and or accelerating/decelerating.
  //
  //      However, during initial development (and testing of the motor model), theta will be the direct measured value 
  //      of the motor shaft and omega_obs and alpha will be set to zero. When actually added in, omega and alpha
  //      will be analyticly derived from the observer. We will use the the "centered" or 1/2 step 
  //      compensation approach to add the correction terms to the position.
  //      
  //      "v_bus" is the measured DC bus voltage. Initially, this will be set to a constant value. Later on
  //      we may add more ODE (or minupulate a SRC object) to simulate the effect of DC bus ripple on this
  //      value.
  //
  //      "vcmd_mag" and "vcmd_ang" are the control variables to of this function. vcmd_mag (which is |Vref|
  //      in the refernce literature) is the voltage command in the rotating D/Q plane. vcmd_ang is the 
  //      control angle in the D/Q plane. Setting vcmd_ang equal to 0 radians represents a pure "Q" vector,
  //      a setting of - PI/2 represents a pure "D" vector. vcmd_ang is added to theta to determine vector
  //      angle.
  //     
  //      "V_xo[0]", V_xo[1]", and "V_xo[2]" are the outputs of this function and represent the PWM command
  //      references of the current sector.
  //      
  //      Note, the coding style here (and in every CTRL function written) is in the "flavor" of the actual
  //      code that would be implemented in the controller algorythm (e.g., no high level functions like
  //      "modf()", "fmod()", no divisions except by constants,... etc). However, this is not to say that this 
  //      is the "exact" code that will be used. The code below may be close to the actual implementation if and
  //      only if we have double precision math capability. If not, we will have to work in machine steps with a
  //      modulus of the one complete revolution, then convert to radians.
  //
  //      
  //      Here is a summary of some of the test results.
  //
  //      With "TRIANGLE_REF_MODE" AND "LINEAR_AMP_MODE" defined, we simulate as though we are driving the motor
  //      with an ideal linear amplifier.
  //
  //      "Omega" tracks "omegad" almost precisely.
  //
  //      With only "TRIANGLE_REF_MODE" defined, we simulate with a standard "sine" wave modulated PWM amplifier
  //      running at 20 KHz. "SRC_SLEW_RATE" is specified to provide some switching rise an fall times.
  //      As expected (may have to do with "SRC_SLEW_RATE" and current ripple?), we do not track as accurately as
  //      as the linear simulation stated above.
  //
  //      With both "TRIANGLE_REF_MODE" AND "LINEAR_AMP_MODE" undefined, we simulate with  "space vector" modulated
  //      PWM amplifier running at 20 KHz. There are two tests mode for this case. With "HighSpeedSVPwm" set to FALSE,
  //      we run in minus bus clamped mode. Here, we get even slightly more inaccurate tracking.
  //
  //      However, with "HighSpeedSVPwm" set to TRUE (standard unclamped SVPWM mode) we get a tracking plot that is
  //      very similar to the standard "sine" wave modulated PWM simulation stated above.
  //
  //      ** Now here is the modivation for looking into adding a control that changes the switching frequency (lowers
  //         the frequence) when motor current is at a low value (narrow PWM pulse widths). **
  //
  //         If I change QUARTER_PWM_CYCLE  form .0000125 to .00005 the "clamped" SVPWM mode now begins to track almost
  //         as good as the linear amplifier mode!
  //


     
  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] = (PlusClampedSVPwm ==  TRUE ? (3.0/2.0)*T0 : 0);
      break;
    case TIME_T1:
      V_xo[i] = T1 + (PlusClampedSVPwm ==  TRUE ? (3.0/2.0)*T0 : 0);
      break;
    case TIME_T2:
      V_xo[i] = T2 + (PlusClampedSVPwm ==  TRUE ? (3.0/2.0)*T0 : 0);
      break;
    case TIME_T1_T2:
      V_xo[i] = T1 + T2 + (PlusClampedSVPwm ==  TRUE ? (3.0/2.0)*T0 : 0);
      break;
    }

  }


#endif 

  


#ifdef CTRL_PWM_SCALE
  

  PwmFreqScale = PWM_FREQ_GAIN  * v_bus / vcmd_mag;

  

  if(PwmFreqScale < MIN_PWM_FREQ_SCALE){
    PwmFreqScale = MIN_PWM_FREQ_SCALE;
  }
  else if(PwmFreqScale > MAX_PWM_FREQ_SCALE){
    PwmFreqScale = MAX_PWM_FREQ_SCALE;
  }

#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;


}

RefGen::~RefGen(void)
{



}



void RefGen::CtrlFunction(double t)
{
  //Here we generate trajectory and feedforward copensation.
  //Here, the design is based on work presented in Section 3.1 "Reference Trajectory" of document 
  //"High Performance Non-Linear Control of Perminant Magnet stepper Motor". A PDF file was generated
  //to highlight this section (see "ReferenceTrajectory.pdf").

  //The system described in the documents above must be translated for use on a AC Brushless Motor
  //(ref "Marc's Original, NOT John's)
  //Refer to documents "AcMotorNotes.pdf" for the model of a AC Brushless Motor, and document 
  //"MotorEquations.txt" for a varification of the model. Note that "MotorEquations.txt" MUST be 
  //viewed using Microsoft "WordPad". Also refer to document "LagrangeMultipliers.txt" which must
  //be viewed using "emacs".

  //  Equations (13), (14), (15), and (16) of "ReferenceTrajectory.pdf" translated to the equivilent
  //  AC Brushless motor model are as follows (here, variables are stated as "desired" as opposed to "actual").
  //
  //     didd = (vdd - Ridd + omegad*(Lm + M)*iqd)/(Lm + M)                                       (a)
  //
  //     diqd = (vqd - Riqd - omegad*(Lm + M)*idd - 3*K*omegad/2)/(Lm + M)                        (b)
  //
  //     domegad = (K*iqd - Bm*omegad - Cm*sgn(omegad) - Dm*sin(2*Nr*thetad))/Jm                  (c)
  //
  //     dthetad = omegad                                                                         (d)
  //
  //                   where:
  //                          didd => didd/dt
  //                          diqd => diqd/dt
  //                          domegad => domegad/dt
  //                          dthetad => dthetad/dt
  //
  //  In the D/Q form, the following conditions are assumed:
  //
  //      Lm = (Lm_a + Lm_b + Lm_c) / 3
  //      M = (Mab + Mac + Mbc) / 3
  //      R = (Ra + Rb + Rc) / 3
  //      K = (Ka + Kb + Kc) / 3
  //
  //  Similar to  (17), (18), (19), and (20) of "ReferenceTrajectory.pdf",
  //
  //      idd = - ((Lm + M)*K*(3/2)*Nr*omegad**2) / (R**2 + ((Lm + M)*Nr*omegad)**2)               (e)
  //
  //      iqd = (Jm*alphad  + Bm*omegad + Cm*sgn(omegad) + Dm*sin(2*Nr*thetad)) / K                (f)
  //
  //      didd = - (2*K*(3/2)*(Lm + M)*R**2*Nr*omegad*alphad) / (R**2 + (Nr*omegad*(Lm + M))**2)   (g)
  //
  //      diqd = (Jm*betad  + Bm*alphad  + Cm*sgn(omegad) + Dm*sin(2*Nr*thetad)) / K               (h)
  //
  //                    where:
  //                           alphad => domagad
  //                           betad => dalphad
  //
  //
  //                    Note: The equation for "idd" (equation "(e)" above) is an approximation that assumes
  //                          that "alphad" and it's higher order derivitives are are zero. It derivation is 
  //                          provided in document "LagrangeMultipliers.txt"
  //
  //                          Also note the "3/2" scale factor added "K" in equation (e) and (g) that differeciates the AC  
  //                          Brushless motor expression for "idd" from the Stepping motor expression for "idd".
  //                          
  //
  //
  //  The values for "dtheda", "domega", "dalpha", and "dbeta" are derived from the trajectory generator 
  //  which is explained below. These values are used to determine "idd", "iqd", "didd" and "diqd" in
  //  equations (e) through (h) above.
  //
  //  In turn, these values are used to determine "desired" voltage command "vdd" and "vqd" described
  //  in equations (a) and (b) above.
  //
  //      vdd = (Lm + M)*didd + R*idd - Nr*omegad*(Lm + M)*iqd                                     (i)                   
  //
  //      vqd = (Lm + M)*idq + R*iqd + Nr*omegad*(Lm + M)*idd + K*(3/2)*omegad                     (j)
  //
  //
  //                  (Again, note the "3/2" scale factor applied to "K" in equation (j))
  //
  //  
  //  Equations (i) and (j) provide the feed forward command voltages "vdd" and "vqd" that are used to
  //  calculate "vcmd_mag" and "vcmd_ang" in CtrlObject "VDqCmd".
  //
  //
  //


  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;


 

  //#define EXPONENTIAL_RAMPING
#ifdef EXPONENTIAL_RAMPING

  //  To do:
  //
  //   Implement exponential ramping (with linear ramping within accel/decel segments) 
  // 
  //     Something like this....
  //
  //     - Generate a velocity profile out of seven (7) segments.
  //     - For t0 <= t < t1, v = t**k
  //     - For t1 <= t < t2, v = t1**k + k*(t - t1)**k-1
  //     - For t2 <= t < t3, v = t1**k + k*(t2 - t1)**k-1 + (t1**k - ((t1 - t0) - (t - t2))**k
  //     - For t3 <= t < t4, v = v_const = t1**k + k*(t2 - t1)**k-1 + (t1**k - ((t1 - t0) - (t3 - t2))**k
  //     - (Similar calculations for ramping down...)
  //
  //
  //     - Now, integrate the final equation above for t between t0 and t7 and equate it to the desired position.
  //
  //     - Given t0, t1, t2, k, v_const and position with t7 - t6 == t1 - t0, t6 - t5 == t2 - t1, and t5 - t4 == t3 - t2
  //       and t3 - t2 == t1 - t0, solve for t3, t4, t5, t6 and t7.
  //   
  //     - Execute each equation based on the current value of t within it's time segment.
  //
  //


#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;              


  vdd = (Lm + M)*didd + R*idd - Nr*omegad*(Lm + M)*iqd;
                           //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)
{
#ifdef RECORD_ALL_PROBES
  beta_d.push_back(betad);
  alpha_d.push_back(alphad);
#endif  
  omega_d.push_back(omegad);
#ifdef RECORD_ALL_PROBES
  theta_d.push_back(thetad);
  zeta_d.push_back(zetad);
  id_d.push_back(idd);
  iq_d.push_back(iqd);
  did_d.push_back(didd);
  diq_d.push_back(diqd);
#endif
  vd_d.push_back(vdd);
  vq_d.push_back(vqd); 

}


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");    
  }
  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");
      }
    }
  }


}

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;
}





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


// ---- FSFCtrl  ---------------------------------------------------


// Some backround regarding the development of this control object...
//
//
// In book "Linear System Theory and Design" (Chi-TSong Chen), review starting at Section 2-6, page 33 and continue through
// end of Section 2-7 page 56. 
//
// Contained in these sections is a discussion on the "Cayley-Hamilton" theorm on page 49.
//
// Next, completely review document "AckermannsFormula.rtf" in sub-directory ".\Simulation". Concentrate specificly on Section
// 10.6.6 "Ackermann's Fomula".
// 
// Both of the references sited above will be used to evaluate Section 3.3 "Full State Feedback Controller Development (SFC)" 
// of document "CtrlofPermMagMotor.pdf" in sub-directory ".\Simulation", which is the body of this control object.
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//























// !!!! "Full State Feedback Controller" (ref Section 3.3 of "High Performance Non-Linear Control of Perminant Magnet stepper Motor") !!!!
         // (This is also where we put the D/Q transformation of "ia", "ib", "ic" (or "ia_bus", "ib_bus", "ic_bus") into "id" and "iq"
         // (For bus current sampling mode, here we also substitute "id" and "iq" with "idd" and "iqd" when bus currents 
         //  "ia_bus", "ib_bus" and/or "ic_bus" cannot not be measured.)
         // (Ref. document "AckermannsFormula.rtf" using Microsoft word, for a backround in state feedback control theory).


//void FSFCtrl::CtrlFunction(double t)
//{

  //Similar to equations (a), (b), (c), and (d) of CtrlObject "RefGen" above, the model of the motor in the
  //D/Q plane representing the "actual" states is as follows:

  //     did = (vd - Rid + omega*(Lm + M)*iq)/(Lm + M)                                          (a)
  //
  //     diq = (vq - Riq - omega*(Lm + M)*id - 3*K*omega/2)/(Lm + M)                            (b)
  //
  //     domega = (K*iq - Bm*omega - Cm*sgn(omega) - Dm*sin(2*Nr*theta))/Jm                     (c)
  //
  //     dtheta = omega                                                                         (d)
  //
  //                   where:
  //                          did => did/dt
  //                          diq => diq/dt
  //                          domega => domega/dt
  //                          dtheta => dtheta/dt
  //
  //  Again, in the D/Q form, the following conditions are assumed:
  //
  //      Lm = (Lm_a + Lm_b + Lm_c) / 3
  //      M = (Mab + Mac + Mbc) / 3
  //      R = (Ra + Rb + Rc) / 3
  //      K = (Ka + Kb + Kc) / 3

  //Based on the information provided in "CtrlOfPermMagMotor.pdf", we .........

  // ***** (...and so forth and so on....) *****************




//}











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

// ---- VelObs  ---------------------------------------------------

// !!!! "Velocity Observer" (ref Section 4 of "High Performance Non-Linear Control of Perminant Magnet stepper Motor") !!!!


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



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


  


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




SimuObject Simulation;

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

#ifdef SPICE_TEST_MODE
  //The simulation run time.
  SimuTime = .01;        
  
  //set the plot time range.
  MinimumPlotTime = .004;
  MaximumPlotTime = .005;
#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);


}
