/*   File: simulate.cc 
 
     Simulates triangulation of an acoustic beacon utilizing 
     several hydrophones.
 
     Author: Warwick Tucker
     E-mail: warwick@math.cornell.edu
     Latest edit: Sun Dec 30 01:58:19 EST 2001
*/

//////////////////////////////////////////////////////////////////////////////

#include <iostream.h>
#include <stdlib.h>
#include <time.h>

#include "Functions.h"
#include "IntegerVector.h"
#include "LSS.h"
#include "Matrix.h"
#include "Utilities.h"
#include "Vector.h"

#undef NOISE
#define NOISE

#undef PRINT_INFO
//#define PRINT_INFO

const int    OUTPUT_PRECISION = 10;
const int    DIM              = 3;
const int    NUM_OF_HYDROS    = DIM + 2;
const double MIN_REAL         = - 100;
const double MAX_REAL         = + 100;
const double SOUND_VELOCITY   = 1031.0; 

#ifdef NOISE
const double TIME_ERROR       = 0.005;
const double HYDRO_ERROR      = 0.00;
#endif // NOISE

void immerseHydros (INTERVAL_MATRIX &hydros);
void hideBeacon    (VECTOR &beacon);
void soundBeacon   ();
void getDeltaTimes (INTERVAL_MATRIX &DT, int &minIndx,
		    const INTERVAL_MATRIX &hydros, const VECTOR &beacon);
void triangulate   (INTERVAL_VECTOR &result, const INTERVAL_MATRIX &DT, 
		    const INTERVAL_MATRIX &hydros, const int &minIndx);
void checkTheResult(const INTERVAL_VECTOR &result, const VECTOR &beacon, 
		    const INTERVAL_MATRIX &hydros);

//////////////////////////////////////////////////////////////////////////////
// Random functions.

static void   InitRandoms ();
static double RandomReal  (double min, double max);
static int    RandomInt   (int min, int max);

static void InitRandoms()
{ srand(time(NULL)); }

static double RandomReal(double min, double max)
{ return ((double) rand() / RAND_MAX) * (max - min) + min; }

static int RandomInt(int min, int max)
{ return rand() % (max - min + 1) + min; }

//////////////////////////////////////////////////////////////////////////////

int main(int argc, char * argv[]) // The arguments are not used yet.
{
  INTERVAL_MATRIX hydros(NUM_OF_HYDROS, DIM);
  INTERVAL_MATRIX DT(NUM_OF_HYDROS, NUM_OF_HYDROS);
  INTERVAL_VECTOR result(DIM);
  VECTOR          beacon(DIM);
  int             minIndx = 0;

  if ( argc > 1 )
    InitRandoms();
#ifdef NOISE
  cout << endl << "Warning: Noisy signals with relative errors " 
       << endl << "TIME_ERROR : " << TIME_ERROR 
       << endl << "HYDRO_ERROR: " << HYDRO_ERROR << endl;
#endif
  immerseHydros(hydros);
  hideBeacon(beacon); 
  soundBeacon();
  getDeltaTimes(DT, minIndx, hydros, beacon);

  // When triangulating, we only utilize 
  //    (1) the delta times, 
  //    (2) the first hydro to get hit,
  //    (3) the hydro locations.
  triangulate(result, DT, hydros, minIndx);
  checkTheResult(result, beacon, hydros);

  return 0;
}

//////////////////////////////////////////////////////////////////////////////

void immerseHydros(INTERVAL_MATRIX &hydros)
{
  for (int i = 1; i <= NUM_OF_HYDROS; i++)
    for (int j = 1; j <= DIM; j++)
      {
	hydros(i, j) = Hull(RandomReal(MIN_REAL, MAX_REAL));
#ifdef NOISE
	double error1 = RandomReal(- HYDRO_ERROR, + HYDRO_ERROR);
	double error2 = RandomReal(- HYDRO_ERROR, + HYDRO_ERROR);
	hydros(i, j) = (1 + Hull(error1, error2)) * hydros(i, j);
#endif
      }
  cout << "'immerseHydros': " << endl 
       << Mid(hydros) << endl;
#ifdef NOISE  
  if ( HYDRO_ERROR != 0.0 )
    cout << "+/-" << endl << 0.5 * Diam(hydros) << endl;
#endif
}

//////////////////////////////////////////////////////////////////////////////

void hideBeacon(VECTOR &beacon)
{
  for (int i = 1; i <= DIM; i++)
    beacon(i) = RandomReal(MIN_REAL, MAX_REAL);
#ifdef PRINT_INFO
  cout << "'hideBeacon': " << beacon << endl;
#else
  cout << endl << "'hideBeacon': Beacon hidden at the location" 
       << endl << beacon << endl;
#endif
}

//////////////////////////////////////////////////////////////////////////////

void soundBeacon()
{
#ifdef PRINT_INFO
  cout << endl << "'soundBeacon':\t PING!!!" << endl << endl;
#endif
}

//////////////////////////////////////////////////////////////////////////////

void getDeltaTimes(INTERVAL_MATRIX &DT, int &minIndx, 
		   const INTERVAL_MATRIX &hydros, const VECTOR &beacon)
{
  INTERVAL_VECTOR T(NUM_OF_HYDROS);
  double minTime = 1e100;
  for (int i = 1; i <= NUM_OF_HYDROS; i++)
    {
      T(i) = Norm(Row(hydros, i) - beacon);
#ifdef NOISE
      double error1 = RandomReal(- TIME_ERROR, + TIME_ERROR);
      double error2 = RandomReal(- TIME_ERROR, + TIME_ERROR);
      T(i) = (1 + Hull(error1, error2)) * T(i);
#endif
      if ( minTime > Inf(T(i)) )
	{
	  minTime = Inf(T(i));
	  minIndx = i;
	}
    }
  T /= SOUND_VELOCITY; 
  Clear(DT);
  for (int i = 1; i <= NUM_OF_HYDROS - 1; i++)
    for (int j = i + 1; j <= NUM_OF_HYDROS; j++) 
      {
	DT(i, j) = T(i) - T(j);
	DT(j, i) = - DT(i, j);
      }
#ifdef PRINT_INFO
  cout << "'getDeltaTimes': The times T: " << T << endl;
  cout << "'getDeltaTimes': minIndx    : " << minIndx << endl;
  cout.precision(OUTPUT_PRECISION);
  cout.setf(ios::showpos);
  cout.setf(ios::scientific); 
  cout << "'getDeltaTimes': The delta times DT: " 
       << endl << DT << endl << endl;
  cout.unsetf(ios::showpos);
  cout.unsetf(ios::scientific);
#endif
}

//////////////////////////////////////////////////////////////////////////////

void triangulate(INTERVAL_VECTOR &result, const INTERVAL_MATRIX &DT, 
		 const INTERVAL_MATRIX &hydros, const int &minIndx)
{
  INTEGER_VECTOR nu(NUM_OF_HYDROS - 1);
  INTEGER_VECTOR mu(NUM_OF_HYDROS - 1);
  INTERVAL_MATRIX DR(NUM_OF_HYDROS, NUM_OF_HYDROS); Clear(DR);
  int k = 0;
  for (int i = 1; i <= NUM_OF_HYDROS - 1; i++)
    {
      k++;
      nu(k) = i;
      mu(k) = i + 1;
      for (int j = i + 1; j <= NUM_OF_HYDROS; j++) 
	{
	  DR(i, j) = DT(i, j) * SOUND_VELOCITY;
	  DR(j, i) = - DR(i, j);
	}
    }
#ifdef PRINT_INFO
  cout << "'Triangulate': DR = " << endl << DR << endl;
  cout << "'Triangulate': nu = " << nu << endl;
  cout << "'Triangulate': mu = " << mu << endl;
#endif
  INTERVAL_MATRIX A(NUM_OF_HYDROS - 1, DIM + 1);
  INTERVAL_VECTOR c(NUM_OF_HYDROS - 1);
  INTERVAL_VECTOR d(NUM_OF_HYDROS - 1);
  for (int i = 1; i <= NUM_OF_HYDROS - 1; i++)
    {
#ifdef PRINT_INFO
      cout << "'Triangulate': (nu, mu) = " << nu(i) << ", " << mu(i) << endl;
#endif
      c(i) = Sqr(DR(nu(i), minIndx)) - Sqr(DR(mu(i), minIndx)) \
	- Sqr(Norm(Row(hydros, nu(i)))) +  Sqr(Norm(Row(hydros, mu(i))));
      d(i) = 2 * ( DR(nu(i), minIndx) - DR(mu(i), minIndx) );
      for (int j = 1; j <= DIM; j++) 
	A(i, j) = - 2 * (hydros(nu(i), j) - hydros(mu(i), j));
      A(i, DIM + 1) = - d(i);
    }
#ifdef PRINT_INFO
  cout << "'Triangulate': A = " << endl << A << endl;
  cout << "'Triangulate': d = " << d << endl;
  cout << "'Triangulate': c = " << c << endl;
#endif  
  int OK;
  INTERVAL_VECTOR preResult = ILSS(A, c, OK);
  for (int i = 1; i <= DIM; i++)
    result(i) = preResult(i);
  if ( !OK )
    cout << "'Triangulate': BAD SOLUTION!!" << endl;
#ifdef PRINT_INFO
  cout << "'Triangulate': sol = " << preResult << endl;
#else
  cout << "'Triangulate': Beacon estimated to be hidden at"
       << endl << Mid(result);
#endif
#ifdef NOISE
  cout << " +/- " << 0.5 * Diam(result) << endl;
#else
  cout << endl;
#endif
}

//////////////////////////////////////////////////////////////////////////////

void checkTheResult(const INTERVAL_VECTOR &result, const VECTOR &beacon,
		    const INTERVAL_MATRIX &hydros)
{
  INTERVAL error = Norm(result - beacon);
#ifdef PRINT_INFO
  cout << "'checkTheResult': Error norm = " << error << endl
       << "\t result = " << result << endl
       << "\t beacon = " << beacon << endl;
  INTERVAL_VECTOR r(NUM_OF_HYDROS);
  for (int i = 1; i <= NUM_OF_HYDROS; i++)
    {
      r(i) = Norm(Row(hydros, i) - beacon);
      cout << "|h(" << i << ") - b| = " 
	   <<  r(i) << endl;
    }
  for (int i = 1; i <= NUM_OF_HYDROS; i++)
    {
      for (int j = 1; j <= NUM_OF_HYDROS; j++)
	cout <<  r(i) - r(j) << " ";
      cout << endl;
   }
#endif
  cout << "'checkTheResult': Relative error = " 
       << error/Norm(beacon) << endl << endl; 
}

//////////////////////////////////////////////////////////////////////////////