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

extern "C" {

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

double r_c = 1.0;

double delta_c = 1.0;

double a_c = 0.3;

double r_s = 3;

double s_star = 1.0;

double alpha_ws = 0.5;

double u_min = -0.02;

double u_max = 0.02;

double g_min = 0.0;

double s_min = 0.1;

double s_max = 0.18;
 

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] = u[0];

image[1] = r_c/delta_c*x[1]*(a_c+r_s*x[1])*(1-x[1]/s_star-alpha_ws)-x[1]*x[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]= 0.0;

jacob[0][1]= 0.0;

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

jacob[1][1]= r_c/delta_c*a_c*(1-alpha_ws)-x[0]+2*r_c/delta_c*(r_s*(1-alpha_ws)-a_c/s_star)*x[1]-3*r_c/delta_c*r_s/s_star*x[1]*x[1];

}

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

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

bound[1]=r_c/delta_c*x[1]*(a_c+r_s*x[1])*(1-x[1]/s_star-alpha_ws)-x[1]*x[0];

}

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

  return res;
}
}