#include <string>
#include <iostream>
#include <iomanip>
#include <vector>

#include "JDetector/JTripod.hh"

#include "JMath/JSVD3D.hh"

#include "Jeep/JContainer.hh"
#include "Jeep/JParser.hh"
#include "Jeep/JMessage.hh"


/**
 * \file
 * Auxiliary application to determine tilt angles of seabed based on tripod positions.
 *
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;

  typedef JContainer< vector<JTripod> >      tripods_container;

  tripods_container  tripods;
  int                debug;

  try { 

    JParser<> zap("Auxiliary application to determine tilt angles of seabed based on tripod positions.");
    
    zap['T'] = make_field(tripods);
    zap['d'] = make_field(debug)   = 1;
    
    zap(argc, argv);
  }
  catch(const exception& error) {
    FATAL(error.what() << endl);
  }

  // solve z(x,y) = a*x + b*y + c

  JMatrix3D A;
  double    Y[3] = { 0.0, 0.0, 0.0 };

  for (tripods_container::const_iterator i = tripods.begin(); i != tripods.end(); ++i) {

    A.a00 += i->getX() * i->getX();  A.a01 += i->getX() * i->getY();  A.a02 += i->getX();
    A.a10 += i->getX() * i->getY();  A.a11 += i->getY() * i->getY();  A.a12 += i->getY();
    A.a20 += i->getX() ;             A.a21 += i->getY();              A.a22 += 1.0;

    Y[0]  += i->getX() * i->getZ();
    Y[1]  += i->getY() * i->getZ();
    Y[2]  += i->getZ();
  }

  DEBUG(A);
  
  JSVD3D V(A);

  try {

    A = V.invert();

    const double a  = A.a00 * Y[0] + A.a01 * Y[1] + A.a02 * Y[2];
    const double b  = A.a10 * Y[0] + A.a11 * Y[1] + A.a12 * Y[2];
    //const double c  = A.a20 * Y[0] + A.a21 * Y[1] + A.a22 * Y[2];

    //const double Rx = atan(+b);   // rotation aroud x-axis
    //const double Ry = atan(-a);   // rotation aroud y-axis
    const double Tx = a;
    const double Ty = b;

    cout << showpos << FIXED(12,6) << Tx << ' ' 
	 << showpos << FIXED(12,6) << Ty << endl;
  }
  catch(const exception& error) {
    FATAL(error.what());
  }

  return 0;
}