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

extern "C" {

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

double a = 1.41;

double underline_sigma = 0.1;

double underline_u = -0.06;

double bar_u = 0.06;

 

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] = (1-x[0]-x[1])*pow(1-x[1],a)*x[2]-x[0]*pow(x[1],a)*(1-x[2]);

image[1] = (1-x[0]-x[1])*pow(1-x[0],a)*(1-x[2])-x[1]*pow(x[0],a)*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]= -pow(1-x[1],a)*x[2]-pow(x[1],a)*(1-x[2]);

jacob[0][1]= -pow(1-x[1],a)*x[2]-a*(1-x[0]-x[1])*pow(1-x[1],a-1)*x[2]-a*x[0]*pow(x[1],a-1)*(1-x[2]);

jacob[0][2]= (1-x[0]-x[1])*pow(1-x[1],a)+x[0]*pow(x[1],a);

jacob[1][0]= -pow(1-x[0],a)*(1-x[2])-a*(1-x[0]-x[1])*pow(1-x[0],a-1)*(1-x[2])-a*x[1]*pow(x[0],a-1)*x[2];

jacob[1][1]= -pow(1-x[0],a)*(1-x[2])-pow(x[0],a)*x[2];

jacob[1][2]= - (1-x[0]-x[1])*pow(1-x[0],a)-x[1]*pow(x[0],a);

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]= (1-x[0]-x[1])*pow(1-x[1],a)*x[2]-x[0]*pow(x[1],a)*(1-x[2]);

bound[1]= (1-x[0]-x[1])*pow(1-x[0],a)*(1-x[2])-x[1]*pow(x[0],a)*x[2];

bound[2]=fmax(underline_u,bar_u);
}

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

  return res;
}
}