#include <iostream>
#include <math.h>
#include "sopnamsp.h"
#include "sambainit.h"
#include "skymap.h"

#include "tod.h"
#include "fitstarray.h"
#include "fitsspherehealpix.h"
#include "srandgen.h"

#include "timing.h"

template <class T>
void  MeanSig(PixelMap<T> const & map, double& gmoy, double& gsig)
{
  gmoy=0.;
  gsig = 0.; 
  double valok;
  for(int k=0; k<map.NbPixels(); k++) {
    valok = map(k);
    gmoy += valok;  gsig += valok*valok; 
  }
  gmoy /= (double)map.NbPixels();
  gsig = gsig/(double)map.NbPixels() - gmoy*gmoy;
  if (gsig >= 0.) gsig = sqrt(gsig);
}

template <class T>
void  Project_Mol(PixelMap<T> const & map, TMatrix<T> & mtx, T defval=-999.)
{
  r_8 xa, yd, teta,phi, facteur;
  int_4 l,c,k;
  int_4 nl = mtx.NRows();
  int_4 nc = mtx.NCols();
  mtx = defval;   // On met tout a defval
  cout << " NRows= " << nl << "  NCols= " << nc  << endl;
  for(l=0; l<nl; l++) {
    yd = (r_8)(l+0.5)/(r_8)nl-0.5;
    facteur=2.*M_PI/sin(acos((double)yd*2));
    teta = (yd+0.5)*Pi;
    //    teta = (0.5-yd)*M_PI;
    for(c=0; c<nc; c++)  {
      xa = (r_8)(c+0.5)/(r_8)nc-0.5;
      phi = xa*facteur+M_PI;
      if ( (phi <= 2*M_PI) && (phi >= 0.) ) {
        k = map.PixIndexSph(teta, phi);
        mtx(l,c) = map(k);
      }
    }
  }

}

int main(int narg, char* arg[]) 
{
  double teta,phi;
  double gmoy=0., gsig=0.;

  SophyaInit();
  InitTim();   // Initializing the CPU timer 
  if ((narg > 1) && (strcmp(arg[1],"-h") == 0) )  {
    cout << " tspm [HEALPix_M=32] : HEALPix Spherical Map Test " << endl;
    exit(0);
    }

  int  m=32;
  if (narg >1)  m = atoi(arg[1]);
  cout <<  " ===== HEALPix Spherical Map Test M= " << m << endl;

  try {

  POutPersist s("spheres.ppf"); 
  string nomobj;

  {
  SphereHEALPix<double> sph(m);

  cout << "Filling spherical map NPixels= " << sph.NbPixels() << endl;
  for (int j=0;j<sph.NbPixels();j++) 
    {
      sph.PixThetaPhi(j,teta,phi);
      sph(j)= 0.2* cos(3.*teta)*sin(8*phi);
    }
  PrtTim("End of Fill ");

   {
   FIO_SphereHEALPix<double> fiog(&sph) ;
   nomobj = "sphg1";
   fiog.Write(s, nomobj);
   cout << "SphMap SphereHEALPix<double> written to POutPersist with name "  << nomobj << endl;
   }


  TMatrix<double> mtx(3*m, 6*m);
  cout << " Project_Mol(sph, mtx) " << endl;
  Project_Mol(sph, mtx);
  {
  cout << " Writing to FITS prjmol1.fits " << endl;  
  FITS_TArray<double> fios(mtx);
  fios.Write("prjmol1.fits");
  }

  // Computing mean and sigma on the sphere 
  MeanSig(sph, gmoy, gsig);
  cout << "SphMap Mean= " << gmoy << "  Sigma = " << gsig << endl;
  PrtTim("End of Mean-Sig ");
  }

  {
  SphereHEALPix<float> sph(m);

  cout << "Filling spherical map2 NPixels= " << sph.NbPixels() << endl;
  for (int j=0;j<sph.NbPixels();j++) 
    {
      sph.PixThetaPhi(j,teta,phi);
      if (teta < 0.3)  sph(j) = 30.;
      else if ((teta>1.4) && (teta<1.6) ) sph(j) = 20.;
      else {
        if (phi < 2.)    sph(j) = 2.;
        else if (phi > 3.) sph(j) = 5.;
        else sph(j) = 0.;
      }
    }
  PrtTim("End of Fill2 ");

   {
   FIO_SphereHEALPix<float> fiog(&sph) ;
   nomobj = "sphg2";
   fiog.Write(s, nomobj);
   cout << "SphMap SphereHEALPix<float> written to POutPersist with name "  << nomobj << endl;
   }


  // On projete dans un fichier FITS
   {
   cout << "Test of Write/Read SphereHEALPix<float> to FITS (sphg_r4.fits) " << endl;
   FITS_SphereHEALPix<r_4> fiosw(sph);
   fiosw.Write("sphg_r4.fits");
   }

   
   SphereHEALPix<float> sphr;
   {
   FITS_SphereHEALPix<r_4> fiosr("sphg_r4.fits");
   sphr = fiosr;
   cout << " Read from file - SphereHEALPix<float> NPixels= " << sphr.NbPixels() << endl;
   int ndiff = 0;
   for(int k=0; k<sphr.NbPixels(); k++) {
    if ( sphr(k) != sph(k) )   { 
      ndiff++;
      if (ndiff < 20)  cout << "!!!Diff: K= " << k << " SPHR= " << sphr(k) << " SPH= " << sph(k) << endl;
      }
    }
   cout << " ReadFrom FITS NDiff = " << ndiff << " (should be zero = 0) " << endl;
   
   }


  TMatrix<float> mtx(3*m, 6*m);
  Project_Mol(sph, mtx);
  {
  cout << " Writing to FITS prjmol2.fits " << endl;  
  FITS_TArray<double> fios(mtx);
  fios.Write("prjmol2.fits");
  }

  // Computing mean and sigma on the sphere 
  MeanSig(sph, gmoy, gsig);
  cout << "SphMap2 Mean= " << gmoy << "  Sigma = " << gsig << endl;
  PrtTim("End of Mean-Sig2 ");
  }

  {
  SphereThetaPhi< complex<float> > sphc(m*10);

  cout << "Filling spherical map3 SphereThetaPhi(complex) NPixels= " << sphc.NbPixels() << endl;
  for (int j=0;j<sphc.NbPixels();j++) 
    {
      sphc.PixThetaPhi(j,teta,phi);
      if (teta < 0.3)   sphc(j) = (30., drandpm1()*3.); 
      else if ((teta>1.4) && (teta<1.6) ) sphc(j) = (20., NorRand()); 
      else {
        if (phi < 2.)     sphc(j) = 2.; 
        else if (phi > 3.) sphc(j) = 5.;
        else sphc(j) = 0.;
      }
    }
  PrtTim("End of Fill2 ");

   {
   FIO_SphereThetaPhi< complex<float> > fio(&sphc) ;
   nomobj = "sphtp";
   fio.Write(s, nomobj);
   cout << "SphMap written to POutPersist with name "  << nomobj << endl;
   }
  }
  }
  catch (PThrowable & exc) {
    cerr << "tspm2: Catched Exception " << (string)typeid(exc).name()
         << " - Msg= " << exc.Msg() << endl;
  }
  catch (...) {
    cerr << "tspm2:  some other exception was caught ! " << endl;
  }

  cout <<  " ===== Fin de TSPM2_Test ======== " << endl;
  return 0;
}