#include "../include/utilities.h"

extern "C" {

std::string paramsFile = "grazing3D_params.json";

double r_c = 3.0;

double delta_c = 1.0;

double a_c = 0.1;

double r_s = 1.0;

double u_min = -0.05;

double u_max = 0.05;

double g_min = 0.65;

double s_min = 0.1;

 

void dynamics(const double *x, const double *u, double *image)

{

// programm here your dynamics function:

// x : the input state vector

// u : the input control vector

// image : the resulting state vector


image[0] = r_c*x[1]-delta_c*x[0];

image[1] = x[0]*(a_c+r_s*x[1])*(1-x[1])-x[1]*x[2];

image[2] = u[0];
}

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

// Jacobian matrix of the dynamics. It will be used to estimate locally the Lipschitz constant

// Please fill the code of teh below function without changing it's signature

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

void jacobian(const double *x, const double *u , double ** jacob){

// programm here the jacobian matrix of your dynamics function:

// x : the input state vector

// u : the input control vector

// jacob : the resulting matrix

jacob[0][0]= -delta_c;

jacob[0][1]= r_c;

jacob[0][2]= 0.0;

jacob[1][0]= (a_c+r_s*x[1])*(1-x[1]);

jacob[1][1]= x[0]*(r_s-a_c-2*r_s*x[1])-x[2];

jacob[1][2]= - x[1];

jacob[2][0]= 0.0;

jacob[2][1]= 0.0;

jacob[2][2]= 0.0;

}

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

// Local bound of the dynamics. Each element of the result vector is defined as max(over u) F_i(x,u)

// Please fill the code of teh below function without changing it's signature

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

void localDynBounds(const double *x, double * bound){

// programm here the bound of your dynamics function, maximized over the control:

// x : the input state vector

// bound : the resulting vector

bound[0]=  r_c*x[1]-delta_c*x[0];;

bound[1]= x[0]*(a_c+r_s*x[1])*(1-x[1])-x[1]*x[2];

bound[2]=u_max;
}

 double constraintsX( const double *x )
{
   double res=((x[1]>=s_min) && (x[2]>=g_min))?1.0:PLUS_INF;

  return res;
}
}