/**
* DESC: Generates the nearest neighbor tour based on a random city.
*
* city : city to start with
* num_cities : number of cities there are
* cities : the master array of city objects
*/
tour_t* create_tour_nn(city_t* city, int num_cities, tour_t* cities) {
// Set up the cities_visited array; 0 for not visited, 1 for visited.
char cities_visited[MAX_CITIES]; /* keeps track of which cities we've
visited so far */
tour_t* tour; // The tour to be returned.
city_t* next_city; // The next city to place in the tour.
int i; // loop control
memset(cities_visited, 0, MAX_CITIES * sizeof(char)); // set all to false
tour = malloc( sizeof(tour_t) ); // instantiate tour
// Init to be the city passed into the function
next_city = city;
// The first city is city passed.
tour->city[0] = city;
cities_visited[city->id] = 1;
// Iterate through the cities, adding new ones and marking them off.
for (i=1;i<num_cities;i++)
{
next_city = find_nearest_neighbor(next_city, num_cities, cities, cities_visited);
tour->city[i] = next_city;
cities_visited[next_city->id] = 1;
}
// Before returning, set the tour's size.
tour->size = num_cities;
return tour;
}
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);
}
