#include <iostream.h>
#include <math.h>
#include "sambainit.h"
#include "spheregorski.h"
#include "spherethetaphi.h"

#include "tod.h"
#include "fitsioserver.h"
#include "nbrandom.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.Reset(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);
      }
    }
  }

}
template <class T>
void  Project_Mol_Old(PixelMap<T> const & map, ImageR4 & img, float defval=-999.)
{
  r_8 xa, yd, teta,phi, facteur;
  int_4 i,j,k,n;
  printf("Xsize= %d   Ysize= %d  NPix= %d\n",img.XSize(),img.YSize(),img.XSize()*img.YSize() );
  n = 0;
  img.Zero();
  for(j=0; j<img.YSize(); j++) {
    yd = (r_4)(j+0.5)/(r_4)img.YSize()-0.5;
    facteur=2.*M_PI/sin(acos((double)yd*2));
    teta = (yd+0.5)*M_PI;
    //    teta = (0.5-yd)*M_PI;
    for(i=0; i<img.XSize(); i++)  {
      xa = (r_4)(i+0.5)/(r_4)img.XSize()-0.5;
      phi = xa*facteur+M_PI;
      if ( (phi <= 2*M_PI) && (phi >= 0.) ) {
        k = map.PixIndexSph(teta, phi);
        img(i,j) = map(k);
	//        if (n<20) {
	//          printf("i,j= %d %d -> %g %g -> k=%d -> val= %g \n", 
	//		 i,j,teta,phi,k,map(k)); n++; }
      }
    }
  }

}

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

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

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

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

  {
  SphereGorski<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_SphereGorski<double> fiog(&sph) ;
   nomobj = "sphg1";
   fiog.Write(s, nomobj);
   cout << "SphMap SphereGorski<double> written to POutPersist with name "  << nomobj << endl;
   }

  FitsIoServer fios;
  // On projete dans un fichier FITS
  fios.sinus_picture_projection(sph, "gsin1.fits");
  fios.save(sph, "sph1.fits");

  TMatrix<double> mtx(3*m, 6*m);
  Project_Mol(sph, mtx);
  fios.save(mtx, "mtx1.fits");

  cout << " Project_Mol_Old(sph, img) " << endl;
  ImageR4 img(6*m, 3*m);
  Project_Mol_Old(sph, img);
  img.CheckDyn();
  cout << img; 
  fios.save(img, "img1.fits");
  cout << " Apres writeFits " << endl;
  img.CheckDyn();
  cout << img; 
  /*
  ImageR4 imgr;
  fios.load(imgr, "img1.fits");
  cout << " Apres readFits " << endl;
  imgr.CheckDyn();
  cout << imgr; 
  */

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

  {
  SphereGorski<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_SphereGorski<float> fiog(&sph) ;
   nomobj = "sphg2";
   fiog.Write(s, nomobj);
   cout << "SphMap SphereGorski<float> written to POutPersist with name "  << nomobj << endl;
   }


  // On projete dans un fichier FITS
  FitsIoServer fios;
   {
   cout << "Test of Write/Read SphereGorski<float> to FITS (sphg_r4.fits) " << endl;
   fios.save(sph, "sphg_r4.fits");
 
   SphereGorski<float> sphr(4);
   fios.load(sphr, "sphg_r4.fits", 2);
   cout << " Read from file - SphereGorski<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;
   
   }


  fios.sinus_picture_projection(sph, "gsin2.fits");
  fios.save(sph, "sph2.fits");

  TMatrix<float> mtx(3*m, 6*m);
  Project_Mol(sph, mtx);
  fios.save(mtx, "mtx2.fits");

  
  cout << " Project_Mol_Old(sph, img) " << endl;
  ImageR4 img(6*m, 3*m);
  Project_Mol_Old(sph, img);
  img.CheckDyn();
  cout << img;
  fios.save(img, "img2.fits");
  cout << " Apres writeFits " << endl;
  img.CheckDyn();
  cout << img; 

  // 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;
   }
  }
  cout <<  " ===== Fin de TSPM2_Test ======== " << endl;
  return 0;
}
