// Questo programma determina la traiettoria piu` breve per partire 
// da xi e arrivare a xf passando per due punti intermedi x1 e x2 (variabili).
// Le coordinate dei punti intermedi x1 e x2 vengono lette dai file points1.dat 
// e points2.dat

#include <iostream>
#include <fstream>
#include <math.h>
using namespace std;

#define NDim 3
#define NIntermedi 10

int main() {

  double xi[NDim] = { 2., 2., 2.};   // Coordinate punto iniziale
  double xf[NDim] = {10.,10.,10.};   // Coordinate punto finale
  double x1[NDim], x2[NDim];
  
  double dist_i1, dist_12, dist_2f, dist_tot;
  double minimo = 1.e32;   // Inizializzazione variabile per ricerca del minimo
  int    imin1, imin2;
  
  fstream f, g;
  
  f.open("points1.dat",ios::in);

  //  Calcolo della distanza totale per tutti i possibili percorsi
  
  for (int i = 0; i < NIntermedi; i++) {
    
    dist_i1 = 0.;
    for (int k = 0; k < NDim; k++) {
      f >> x1[k];
      dist_i1 += pow( x1[k] - xi[k], 2. );
    }
    dist_i1 = sqrt( dist_i1 );
    
    g.open("points2.dat",ios::in);
    
    for (int j = 0; j < NIntermedi; j++) {
      
      dist_12 = 0.;
      dist_2f = 0.;
      for (int k = 0; k < NDim; k++) {
        g >> x2[k];
        dist_12 += pow( x2[k] - x1[k], 2. );
        dist_2f += pow( xf[k] - x2[k], 2. );
      }
      dist_12 = sqrt( dist_12 );
      dist_2f = sqrt( dist_2f );
    
      dist_tot = dist_i1 + dist_12 + dist_2f;   // Distanza totale
      
      // Ricerca del minimo
    
      if ( dist_tot < minimo ) {
        minimo = dist_tot;
        imin1  = i;
        imin2  = j;
      }
      
    }
    
    g.clear(); g.close();

  }
  
  f.clear(); f.close();
  
  cout << "Distanza minima: " << minimo << endl;
  cout << "Percorso ideale: i --> " << imin1 << " --> " << imin2 << " --> f" << endl;

  return 0;
  
}