#include "AnT.hpp"

#define  C1      parameters[0]
#define  C2      parameters[1]
#define  G       parameters[2]
#define  L       parameters[3]
#define  m0      parameters[4]
#define  m1      parameters[5]
#define  Bp      parameters[6]
#define mode     currentDiscreteState[0]
#define X        currentContinuousState[0]
#define Y        currentContinuousState[1]
#define Z        currentContinuousState[2]
#define Dminus   -1
#define Dnull    0
#define Dplus    1

bool chua_C (const Array<real_t>& currentContinuousState,
	     const Array<int>&    currentDiscreteState,
	     const Array<real_t>& parameters,
	     Array<real_t>& RHS)
{
  if (mode == Dminus)
    RHS[0] = (G * (Y - X) - (m0 * X + Bp * (m0 - m1)) ) / C1;
  else if (mode == Dnull)
    RHS[0] =(G * (Y - X) - m1 * X) / C1;
  else
    RHS[0] = (G * (Y - X) - (m0 * X + Bp * (m1 - m0)) ) / C1;

  RHS[1] = (G * (X - Y) + Z)/C2;
  RHS[2] = -1/L * Y;
  return true;
}

bool chua_D (const Array<real_t>& currentContinuousState,
	     const Array<int>&    currentDiscreteState,
	     const Array<real_t>& parameters,
	     Array<int>& RHS)
{
  if (X < -Bp)
    RHS[0] = Dminus;
  else if (X < Bp)
    RHS[0] = Dnull;
  else
    RHS[0] = Dplus;
  return true;
}


extern "C" {
  void connectSystem ()
  {
    HybridODE_Proxy::systemFunction = chua_C;
    HybridODE_Proxy::hybridFunction = chua_D;
  }
}