/**
* Dumps a serie of points into its own GPX file.
*/
void dump_points(char * input_fname, int gpx_index, points_result rs) {
char out_f[255];
int i;
snprintf(out_f, 255, "%s_%02d.gpx", input_fname, gpx_index);
printf("dumping trace into its own file (%s) ...\n", out_f);
FILE * out_gpx = fopen(out_f, "w");
fprintf(out_gpx, "<gpx version=\"1.0\" xmlns=\"%s\" creator=\"gpx_sanitizer\">\n <trk>\n <trkseg>\n", GPX_NS);
for (i = 0; i < rs.size ; i++) {
if (i > 0) {
point p1 = rs.points[i-1], p2 = rs.points[i];
float dist_between_pts = calculate_dist(p1.lat, p1.lon, p2.lat, p2.lon);
// Cuts the segment in 2 if necessary
if (dist_between_pts > DISTANCE_THRESHOLD) {
fprintf(out_gpx, " </trkseg>\n <trkseg>\n");
}
fprintf(out_gpx, " <trkpt lat=\"%f\" lon=\"%f\" />\n", p2.lat, p2.lon);
}
}
fprintf(out_gpx, " </trkseg>\n </trk>\n</gpx>\n");
fclose(out_gpx);
return;
}
void FittedRMax::update(QUpdate update)
{
state_to_ind = VI->get_state_to_ind();
static int it = 0;
it++;
if(update.reward == 0)
{
cout << "HI" << endl;
}
//cout << "Cur It: " << it << endl;
//cout << "Cur State: " << update.state[0] << "," << update.state[1] << endl;//"," << update.state[2] << "," << update.state[3] << endl;
//cout << "Cur Action: " << update.action << endl;
for(map<QElement::State,int>::iterator state_it = state_to_ind->begin(); state_it != state_to_ind->end(); state_it++)
{
int cur_ind_update = state_it->second;
QElement::State cur_state_update = state_it->first;
//cout << "Cur Ind: " << cur_ind_update << endl;
for(unsigned int a_update = 0; a_update < m_possible_actions.size(); a_update++)
{
int cur_sa_update = cur_ind_update * m_possible_actions.size() + a_update;
double weight = calculate_dist(update.state, update.action, cur_state_update, a_update);
if(weight < .001) continue;
double alpha;
if(weight < .00001 && N_estimate[cur_sa_update] == 0) continue;
else alpha = weight/(N_estimate[cur_sa_update] + weight);
//cout << "Alpha:" << alpha << endl;
N_estimate[cur_sa_update] += weight;
R_estimate[cur_sa_update] = (1 - alpha) * R_estimate[cur_sa_update] + alpha * update.reward;
updated_states.insert(cur_ind_update);
QElement::State action_effect = action_effect_function(update.state, update.next_state, cur_state_update);
//action_effect = update.next_state;
for(unsigned int af = 0; af < action_effect.size(); af++)
{
if(action_effect[af] > m_max_ranges[af] || action_effect[af] < m_min_ranges[af])
{
action_effect = update.next_state;
break;
}
}
//cout << "Action Effect: " << action_effect[0] << "," << action_effect[1] << endl;
vector<QElement::State> coeffs_states;
map<int, double> coeffs = calculate_mlinterp(action_effect);
double sum = 0;
// for(vector<QElement::State>::iterator it = coeffs_states.begin(); it != coeffs_states.end(); it++)
// {
// double weight2 = calculate_dist(*it, 1, action_effect, 1);
// cout << weight2 << endl;
// }
double sum_offset = 0;
int cur_sa = cur_ind_update * m_possible_actions.size() + update.action;
for(map<int, double>::iterator it = coeffs.begin(); it != coeffs.end(); it++)
{
int ind = it->first;
double coeff = it->second;
if(coeff < 0.001) continue;
sum += coeff;
// for(map<QElement::State,int>::iterator blah = state_to_ind->begin(); blah != state_to_ind->end(); blah++)
// {
// if(blah->second == ind)
// {
// cout << blah->first[0] << "," << blah->first[1] << endl;
// }
// }
// cout << ind << "," << coeff << endl;
if(coeff > 1)
{
cout << "Coeff > 1" << endl;
exit(1);
}
P_estimate(cur_sa, ind) = (1 - alpha) * P_estimate(cur_sa, ind) + alpha * coeff;
double orig_val = P_estimate(cur_sa, ind);
// cout << P_estimate(cur_sa, ind) << endl;
P_estimate(cur_sa, ind) = 0.01 * round( P_estimate(cur_sa, ind) * 100.0 );
// cout << P_estimate(cur_sa, ind) << endl;
sum_offset += abs(orig_val - P_estimate(cur_sa, ind));
}
P_estimate.row(cur_sa) = P_estimate.row(cur_sa)/arma::sum(P_estimate.row(cur_sa));
if(abs(sum - 1) > 0.01)
{
cout << "MLINTERP BROKE!!!" << endl;
cout << sum << endl;
cout << "Action Effect: " << endl;
for(unsigned int i = 0; i < action_effect.size(); i++)
{
cout << action_effect[i] << endl;
}
map<int, double> coeffs = calculate_mlinterp(action_effect);
exit(1);
}
//P_estimate.print();
//cout << endl;
}
}
//VI->update_model(R_estimate, P_estimate);alpha = 0
//VI->calculate_solution();
}