#include <fstream>
#include <sstream>
#include <gamalib/adj/adj.h>
#include <gamalib/xml/dataobject.h>

void write(std::ostream& cout, const GaMaLib::SparseMatrix<>* sgm)
{
  cout << endl;
  for (unsigned long k=1; k<=sgm->rows(); k++)
    {
      cout << k << " : ";
      double* n = sgm->begin(k);
      double* e = sgm->end  (k);
      for(size_t* i=sgm->ibegin(k) ; n!=e; n++, i++)
        {
          cout << *n << " [" << *i << "]  ";
        }
      cout << endl;
    }
}


GaMaLib::Mat Matrix(const GaMaLib::SparseMatrix<>* sgm)
{
  using namespace GaMaLib;
  Mat M(sgm->rows(), sgm->columns());
  M.set_zero();

  for (unsigned long k=1; k<=sgm->rows(); k++)
    {
      double* n = sgm->begin(k);
      double* e = sgm->end  (k);
      for(size_t* i=sgm->ibegin(k) ; n!=e; n++, i++)  M(k,*i) = *n; 
     }

  return M;
}


int main(int argc, char* argv[])
{
  using namespace GaMaLib;
  using namespace std;

  AdjInputData *data = new AdjInputData;

  {
    char* file_name;
    if (argc == 1)
      file_name = "geodet-pc-218.adj.input.data";
    else if (argc == 2)
      file_name = argv[1];
    else
      {
        cout << "\nusage: " << argv[0] 
             << " [ project_equations__old_format ] \n\n";
        return 1;
      }
      
    ifstream inp(file_name);
    if (!inp)
      {
        cout << "\n" << argv[0]
             << " : cannot open file \"" << file_name << "\"\n\n";
        return 2;
      }
    data->read_gama_local_old_format(inp);
  }

  if (bool write_input_data = false) 
    {
      write(cout, data->mat());
      cout << endl;
      for (const double *b=data->cov()->begin(1), *e=data->cov()->end(1); 
           b!=e; b++)
        {
          cout << *b << ' ';
        }
      cout << endl;
      cout << '\n' << trans(data->rhs());
    }

  //### if (bool write_vector_of_adjusted_unknowns = true) 
    {
      using namespace GaMaLib;
      Mat A = Matrix(data->mat());
      Mat C(data->mat()->rows(), data->mat()->rows());
      C.set_zero();  
      long i=1;
      for (const double *b=data->cov()->begin(1), *e=data->cov()->end(1); 
           b!=e; b++, i++)
        {
          C(i,i) = *b;
        }
      Mat P = inv(C);
      Mat N = trans(A)*P*A;
      Vec n = trans(A)*P*data->rhs();
      Vec x = inv(N)*n;
      cout << '\n' << trans(x);
    }

  // ..................................................................

  stringstream str;
  str << DataObject::xml_begin();
  data->write_xml(str);
  str << DataObject::xml_end();
  delete data;

  AdjInputData *sdata = new AdjInputData;
  sdata->read_xml(str);
  
  Adj adj;
  adj.set(sdata);
  
  cout << trans( adj.x() );

  cout << "\n-----------------------------------------------------\n\n";

  cout << DataObject::xml_begin();
  sdata->write_xml(cout);
  cout << DataObject::xml_end();

  return 0;
}
