void test_max(){
x1_len = 3;
x2_len = 3;
w_len = 3;
float V[3][3][3] = {{{10,10,10},{20,20,20},{30,30,30}},{{40,40,40},{50,50,50},{60,60,60}},{{70,70,70},{80,80,80},{90,90,90}}};
float V_old[3][3][3] = {{{30,10000,10},{20,20,20},{30,30,30}},{{40,40,40},{50,50,50},{60,60,60}},{{70,70,70},{80,80,80},{90,90,90}}};
float ans;
ans = max_of_V(V, V_old);
printf("ans %f \n ",ans);
}
int value_iteration(){
//init stuff
float stopping_criterion = 0.01;
float *x1_space, *x2_space, ***V, ***V_old, *temp_w, *temp_u;
int i,j,k;
int x1_len = (x_max[0]-x_min[0])/step_size[0] + 1;
int x2_len = (x_max[1]-x_min[1])/step_size[1] + 1;
int w_len = sizeof(w_space)/sizeof(float);
int u_len = sizeof(u_space)/sizeof(float);
float x_new[] = {0,0};
float g = 1;
x1_space = (float *)(malloc(x1_len*sizeof(float)));
for(i=0;i<x1_len;i++){
x1_space[i] = x_min[0] + step_size[0]*i;
//printf("%f ",x1_space[i]);
}
printf("\n");
x2_space = (float *)(malloc(x2_len*sizeof(float)));
for(i=0;i<x2_len;i++){
x2_space[i] = x_min[1] + step_size[1]*i;
//printf("%f ",x2_space[i]);
}
temp_w = (float *)(malloc(w_len*sizeof(float)));
temp_u = (float *)(malloc(u_len*sizeof(float)));
V = (float ***)(malloc(x1_len*sizeof(float *)));
V_old = (float ***)(malloc(x1_len*sizeof(float *)));
for(i=0;i<x1_len;i++){
V[i] = (float **)(malloc(x2_len*sizeof(float *)));
V_old[i] = (float **)(malloc(x2_len*sizeof(float *)));
for(j=0;j<x2_len;j++){
V[i][j]=(float *)(malloc(w_len*sizeof(float)));
V_old[i][j]=(float *)(malloc(w_len*sizeof(float)));
for(k=0;k<w_len;k++){
V[i][j][k] = 0;
V_old[i][j][k] = 1;
}
}
}
float a[] = {0,0};
float x1=0.1,x2=0.1,w=0.1;
pendulum_nonlinearmodel_ss(&a[0], 1, 0, del_t, &x_new[0]);
printf("array gotten %f,%f\n",x_new[0],x_new[1]);
float temp;
temp= interpol(x1_space, x2_space, w_space, V, x1, x2, w, x1_len, x2_len, w_len);
printf("d %f",temp);
//V_old = memcpy(&V[0][0][0], &V_old[0][0][0], x1_len*x2_len*w_len*sizeof(float));
i = 0, j=0, k=0;
int c = 0, d=0;
float x_old[] = {0,0};
float max_temp_u = 0;
int iteration = 0;
while(max_of_V(V, V_old, x1_len, x2_len, w_len) > stopping_criterion){
deepcopy(V, V_old, x1_len, x2_len, w_len);
//printf("cpy %f %f \n",V[1][1][1],V_old[1][1][1]);
for(i=0;i<x1_len;i++){
for(j=0;j<x2_len;j++){
for(k=0;k<w_len;k++){
for(c=0;c<u_len;c++){
x_old[0] = x1_space[i];
x_old[1] = x2_space[j];
pendulum_nonlinearmodel_ss(&x_old[0], u_space[c], w_space[k], del_t, &x_new[0]);
//printf("non linear %f %f \n",x_new[0],x_new[1]);
temp_u[c] = g;
for(d=0;d<w_len;d++){
temp_w[d] = interpol(x1_space, x2_space, w_space, V, x_new[0], x_new[1], w_space[d], x1_len, x2_len, w_len);
temp_u[c] = temp_u[c] + temp_w[d]*prob_matrix[k][d];
//printf("temp w[d] %f\n",temp_w[d]);
}
if(temp_u[c]>max_temp_u){
max_temp_u = temp_u[c];
}
}
V[i][j][k] = max_temp_u;
}
}
}
printf("iteration %d value %f \n",iteration, max_of_V(V, V_old, x1_len, x2_len, w_len));
iteration += 1;
}
printf("Value iteration Done\n");
FILE *outfile;
char *outfilename = "/Users/Rounak/Desktop/Summer project/DCOC/DCOC_in_C/DCOC_in_C/abc.csv";
printf("%s",outfilename);
if ((outfile = fopen(outfilename,"w")) == NULL)
{
printf("Error opening output file.\n");
return -1;
}
else{
printf("file opened\n");
}
for(i=0;i<x1_len;i++)
{
for(j=0;j<x2_len;j++)
{
for(k=0;k<w_len;k++){
fprintf(outfile,"%f,",V[i][j][k]);
}
fprintf(outfile,"::::::");
}
fprintf(outfile,"\n");
}
fclose(outfile);
return 0;
}
int value_iteration(){
//float lol = prob_matrix1[0][0];
//printf("%f",lol);
//init stuff
static
int i,j,k;
float V[x1_len][x2_len][w_len];
float V_old[x1_len][x2_len][w_len];
float x_new[] = {0,0};
float x_old[] = {1,0};
float g = 1;
float x1_space[x1_len];
float x2_space[x2_len];
float temp_w[w_len];
float temp_u[u_len];
int c = 0, d=0;
int max_temp_c = 0;
int iteration = 0;
for(i=0;i<x1_len;i++){
x1_space[i] = x_min[0] + step_size[0]*i;
//printf("%f ",x1_space[i]);
}
for(i=0;i<x2_len;i++){
x2_space[i] = x_min[1] + step_size[1]*i;
}
for(i=0;i<x1_len;i++){
for(j=0;j<x2_len;j++){
for(k=0;k<w_len;k++){
V[i][j][k] = 1.5;
V_old[i][j][k] = 1;
}
}
}
//DEBUG AREA
//printf("%d x1_len \n",x1_len);
//printf("%f probmatrix\n",prob_matrix[0][0]);
//printf("%f sc\n",stopping_criterion);
//printf("%f x\n",x_new[1]);
//pendulum_nonlinearmodel_ss(x_old, u_space[c], w_space[k],x_new);
//printf("%f x\n",x_new[1]);
float debug;
int deb;
deb = find_nearest_neighbor(-0.1, 0.1, 0, 1);
//printf("%d deb\n",deb);
debug = interpol_3D(x1_space, x2_space, w_space, V, -0.2456,-0.8015, 0.1);//interpol might be off or V update
printf("%f debug\n",debug);
debug = max_of_V(V, V_old);
while(max_of_V(V, V_old) > stopping_criterion){
deepcopy(V, V_old);
//printf("cpy %f %f \n",V[1][1][1],V_old[1][1][1]);
for(i=0;i<x1_len;i++){
for(j=0;j<x2_len;j++){
for(k=0;k<w_len;k++){
max_temp_c = 0;
for(c=0;c<u_len;c++){
x_old[0] = x1_space[i];
x_old[1] = x2_space[j];
//printf("1 %f 2 %f 3 %f 4 %f\n", x1_space[i], x2_space[j],u_space[c], w_space[k]);
pendulum_nonlinearmodel_ss1(x_old, u_space[c], w_space[k],x_new);
//printf("non linear %f %f \n",x_new[0],x_new[1]);
temp_u[c] = g;
for(d=0;d<w_len;d++){
temp_w[d] = interpol_3D(x1_space, x2_space, w_space, V, x_new[0], x_new[1], w_space[d]);
//printf("%f temp_w[d]\n",temp_w[d]);
temp_u[c] = temp_u[c] + temp_w[d]*prob_matrix[k][d];
}
//printf("temp u[c] %f\n",temp_u[c]);
if(temp_u[c]>temp_u[max_temp_c]){
max_temp_c = c;
}
}
V[i][j][k] = temp_u[max_temp_c];
//printf("V[i][j][k] %f \n",V[i][j][k]);
//printf("V_old[i][j][k] %f \n",V_old[i][j][k]);
//printf("================\n");
}
}
}
printf("iteration %d value %f \n",iteration, max_of_V(V, V_old));
iteration += 1;
}
printf("Value iteration Done\n Writing to file...\n");
int status;
status = writeToFile(V);
printf("Done\n");
return(0);
}
