// This may look like C code, but it's really -*- C++ -*-

/////////////////////////////
//      Bolos2ring.cc       //
/////////////////////////////

/// This is a processor for ArchTOIPipe using the Sophya Library.
/// An example of the use of it is in the file gph425_5.cc
/// 
/// The goal of this processor is to give Q and U rings
// computed with either 3 timelines coming from the
// two channels of 1 PSB and one of the channels of a second PSB,
// or with the differences of the two channels of each PSB.
// The first of these two cases can be used if one channel is out
// or order, the second one is supposed to be better when all channels
// work fine, since it fully uses the advantage of PSB's.
//  Both methods should give the same results when the 4 channels work ok.

// Authors CR/NP, rcecile@in2p3.fr, ponthieu@isn.in2p3.fr
//////////////////////////////////////////////////////////////////////////////////////

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
//#include <strstream>
#include "spherehealpix.h"
#include "tmatrix.h"
#include "tvector.h"
#include "vector3d.h"
#include "fitsfile.h"
#include "ctimer.h"
#include "intflapack.h"
#include "smathconst.h"
#include "ring33.h"
#include "Bolos2ring.h"

#define UNSEEN_HEALPIX (-1.6375e30)
 
// Constructor
Bolos2ring::Bolos2ring(SphereHEALPix<r_8>* ringQ, 
		       SphereHEALPix<r_8>* ringU,
		       SphereHEALPix<r_8>* ringQW,
		       SphereHEALPix<r_8>* ringUW,
		       const vector<r_8>& table_angle,
		       int_4 *bolos_ok,
		       int_4 wsz)
  : ringq(ringQ), ringu(ringU), ringqw(ringQW), ringuw(ringUW), TableFP_(table_angle), Bolos_OK(bolos_ok)
{
  SetWSize(wsz);
  totsncount_ = 0;
  
  if( ringq->NbPixels()<1) {
    cout << "Bolos2ring::Bolos2ring  : bad number of pixel in ringQ "
	 << ringq->NbPixels() << endl;
    throw ParmError("Bolos2ring::Bolos2ring  : bad number of pixel in ringQ");
  }
  if( ringu->NbPixels()<1) {
    cout << "Bolos2ring::Bolos2ring  : bad number of pixel in ringU "
	 << ringu->NbPixels()
	 << endl;
    throw ParmError("Bolos2ring::Bolos2ring  : bad number of pixel in ringU");
  }

  if( ringqw->NbPixels()<1) {
    cout << "Bolos2ring::Bolos2ring  : bad number of pixel in ringQW "
	 << ringqw->NbPixels() << endl;
    throw ParmError("Bolos2ring::Bolos2ring  : bad number of pixel in ringQW");
   }
  if( ringuw->NbPixels()<1) {
    cout << "Bolos2ring::Bolos2ring  : bad number of pixel in ringUW "
	 << ringuw->NbPixels() << endl;
    throw ParmError("Bolos2ring::Bolos2ring  : bad number of pixel in ringUW");
   }

  if( ringq->NbPixels() != ringu->NbPixels())
    throw(ParmError("Bolos2ring::Bolos2ring : rings don't have the same size!!"));

  Npix_ = ringq->NbPixels();
  int nlat = ringq->SizeIndex();
  if(nlat != ringqw->SizeIndex()) ringqw->Resize(nlat);
  if(nlat != ringuw->SizeIndex()) ringuw->Resize(nlat);
  cout << "RINGS of " <<ringu->NbPixels() << " pixels" << endl;
  
  ringq->SetPixels(0.);
  ringu->SetPixels(0.);
  ringqw->SetPixels(0);
  ringuw->SetPixels(0);
}

// Destructor
Bolos2ring::~Bolos2ring()
{
}

void Bolos2ring::PrintStatus(ostream& os)
{
  os << "____________________________________________" << endl
     << "  Bolos2ring::PrintStatus() - wsize= "<<wsize_<< endl;
  TOIProcessor::PrintStatus(os);
  os << "  ProcessedSampleCount=" << ProcessedSampleCount() << endl;
  os << "____________________________________________" << endl;
}
 
void Bolos2ring::init()
{ 
  cout << "Bolos2ring::init" << endl;

  // on branche les TOIs des 4 bolos (l'un peut etre "vide")
  int i;
  for(i=0; i < 4; i++)
    {
      char str[80];
      sprintf(str,"Bolo%d",i);
      cout << str << endl;
      declareInput(str);
    }

  // on branche le pointage des 2 PSB
  for(i=0; i < 2; i++)
    {
      char str[80];
      sprintf(str,"theta_pointing%d",i);
      cout << str << endl;
      declareInput(str);
      sprintf(str,"phi_pointing%d",i);
      cout << str << endl;
      declareInput(str);
    }
  
  name = "Bolos2ring";
}

void Bolos2ring::run()
{
  int snb = getMinIn();
  int sne = getMaxIn();

  if (!checkInputTOIIndex(0) && !checkInputTOIIndex(1)) {
    cerr << " Bolos2ring::run() - Input TOIs (in1, in2 and in3, in4) not connected! "
	 << endl;
    throw ParmError("Bolos2ring::run() Input TOIs (in1, in2 and in3, in4) not connected!");
  }

  cout << " Bolo2ring::run() SNRange=" << snb << " - " << sne << endl;

  
  for(int i=0; i<4; i++) 
    if( !checkInputTOIIndex(i) )
    {
      cerr << " Bolos2ring::run() - Input TOIs not connected! ("
	   << i << ')' << endl;
      throw ParmError("Bolos2ring::run() - Input TOIs not connected!");
    }
  
  cout << "Bolos2ring::run() - SNRange="<< snb << '-' << sne << endl;
  
  try {
    
    cout << " *** combine bolometers without computing the difference *** " << endl;    

    double v0, v1, v2, v3; // values of bolometers outputs
    double l0, b0, l1, b1; // galactic coordinates
    uint_8 vfg0, vfg1, vfg2, vfg3;
    double theta0, phi0, theta1, phi1;
    int_4 pixel0, pixel1; // pour chaque PSB
       
    //  Matrix* sts = new Matrix[Npix_]; // matrice de calcul des param Stokes
    //  Vector* stm = new Vector[Npix_]; // St * vecteur de mesures
    
    Matrix stm(Npix_, 3);
    RingOfMatrix3x3 ring(Npix_);
    cout << "****************************" << endl;
    cout << "   Ring declaration done" << endl;
    cout << "****************************" << endl;
    

    for(int i =0; i<4; i++) TableFP_[i] *= Pi/180. ;

    for(int s=snb; s<= sne; s++) {
      
      getData(0, s, v0, vfg0);
      getData(1, s, v1, vfg1);
      getData(2, s, v2, vfg2);
      getData(3, s, v3, vfg3);
      l0 = getData(4, s);
      b0 = getData(5, s);
      l1 = getData(6, s);
      b1 = getData(7, s);
      
      theta0 = (90. - b0)/180. * Pi;
      phi0 = l0/180. * Pi;
      theta1 = (90. - b1)/180. * Pi;
      phi1 = l1/180. *Pi;
      
      pixel0 = ringq->PixIndexSph(theta0, phi0); // Index of the hit pixel
      pixel1 = ringu->PixIndexSph(theta1, phi1);
      
      if ((Bolos_OK[0] == 0) || (vfg0 > 0)) v0 = 0.;
      if ((Bolos_OK[1] == 0) || (vfg1 > 0)) v1 = 0.;
      if ((Bolos_OK[2] == 0) || (vfg2 > 0)) v2 = 0.;
      if ((Bolos_OK[3] == 0) || (vfg3 > 0)) v3 = 0.;
       
      // Filling pixel0
      ring.GetElement(pixel0)(0, 0) += 0.25 * 2;  
      ring.GetElement(pixel0)(0, 1) += 0.25 *( cos(2*TableFP_[0]) + cos(2*TableFP_[1])); 
      ring.GetElement(pixel0)(0, 2) += 0.25 *( sin(2*TableFP_[0]) + sin(2*TableFP_[1])); 
      
      ring.GetElement(pixel0)(1, 0) += 0.25 *( cos(2*TableFP_[0]) + cos(2*TableFP_[1]));
      ring.GetElement(pixel0)(1, 1) += 0.125 *( 2 + cos(4*TableFP_[0]) + cos(4*TableFP_[1]));
      ring.GetElement(pixel0)(1, 2) += 0.125*( sin(4*TableFP_[0]) + sin(4*TableFP_[1]));
      
      ring.GetElement(pixel0)(2, 0) += 0.25 *( sin(2*TableFP_[0]) + sin(2*TableFP_[1]));
      ring.GetElement(pixel0)(2, 1) += 0.125*( sin(4*TableFP_[0]) + sin(4*TableFP_[1]));
      ring.GetElement(pixel0)(2, 2) += 0.125*( 2 - cos(4*TableFP_[0]) - cos(4*TableFP_[1]));
     
      stm(pixel0, 0) += 0.5*(v0 + v1);
      stm(pixel0, 1) += 0.5*( cos(2*TableFP_[0])*v0 + cos(2*TableFP_[1])*v1);
      stm(pixel0, 2) += 0.5*( sin(2*TableFP_[0])*v0 + sin(2*TableFP_[1])*v1);
           
      if((Bolos_OK[0] == 0) || (Bolos_OK[1] == 0) || (vfg0 >0) || (vfg1 >0))
      {
	ringqw->PixVal(pixel0) += 1 ; // nombre de hits dans le pixel
	ringuw->PixVal(pixel0) += 1 ; 
      }
      else 
      {
	ringqw->PixVal(pixel0) += 2 ; 
	ringuw->PixVal(pixel0) += 2 ; 
      }
      
      ring.GetElement(pixel1)(0, 0) += 0.25 * 2; 
      ring.GetElement(pixel1)(0, 1) += 0.25 *( cos(2*TableFP_[2]) + cos(2*TableFP_[3])); 
      ring.GetElement(pixel1)(0, 2) += 0.25 *( sin(2*TableFP_[2]) + sin(2*TableFP_[3])); 
      
      ring.GetElement(pixel1)(1, 0) += 0.25 *( cos(2*TableFP_[2]) + cos(2*TableFP_[3]));
      ring.GetElement(pixel1)(1, 1) += 0.125*( 2 + cos(4*TableFP_[2]) + cos(4*TableFP_[3]));
      ring.GetElement(pixel1)(1, 2) += 0.125*( sin(4*TableFP_[2]) + sin(4*TableFP_[3])); 
      
      ring.GetElement(pixel1)(2, 0) += 0.25 *( sin(2*TableFP_[2]) + sin(2*TableFP_[3]));
      ring.GetElement(pixel1)(2, 1) += 0.125*( sin(4*TableFP_[2]) + sin(4*TableFP_[3]));
      ring.GetElement(pixel1)(2, 2) += 0.125*( 2 - cos(4*TableFP_[2]) - cos(4*TableFP_[3]));
      
      stm(pixel1, 0) += 0.5*( v2 + v3);
      stm(pixel1, 1) += 0.5*( cos(2*TableFP_[2])*v2 + cos(2*TableFP_[3])*v3);
      stm(pixel1, 2) += 0.5*( sin(2*TableFP_[2])*v2 + sin(2*TableFP_[3])*v3);
      
      if((Bolos_OK[2] == 0) || (Bolos_OK[3] == 0)|| (vfg2 >0) || (vfg3 >0)) 
      {  	
	ringqw->PixVal(pixel1) += 1 ; // nombre de hits dans le pixel
	ringuw->PixVal(pixel1) += 1 ;
      }
      else 
      {
	ringqw->PixVal(pixel1) += 2 ; 
	ringuw->PixVal(pixel1) += 2 ; 
      }
      
      
      if (!(s%10000)) cout << " sample " << s << " ****** filling matrices  ****** " << endl;
      
    }// fin de la boucle sur les samplenums qui normalement a rempli
    // la matrice et le vecteur projete. Y'a plus qu'a inverser...
    
    // Ca devrait suffire de definir un ringw seulement dans ce cas la,
    // mais je garde le ringqw pour pas bugguer l'entree.
    
    cout << endl << "Start to fill rings :" << endl;
	   
    for(int pix=0 ; pix < Npix_ ; pix++) {// pour chaque pixel

      if( ringqw->PixVal(pix) < 4.0 ) 
	//pixel pas vu, 4 a cause des 2 PSB ici, a changer
	ringq->PixVal(pix) = ringu->PixVal(pix) = UNSEEN_HEALPIX; 
      
      else{ 
      
	Matrix3x3 sts;
	Matrix stsm1(3, 3);
	Vector s(3);
	Vector stokes_param(3);
	
	s(0) = stm(pix, 0);
	s(1) = stm(pix, 1);
	s(2) = stm(pix, 2);
	sts = ring.GetElement(pix); // get the sts matrix of pix
	sts = sts.Inverse();

	// voir si on peut arreter de bricoler pour que Matrix3x3 soit aussi une Matrix
	for(int r=0; r<=2; r++)
	  for(int c=0; c<=2; c++)
	    stsm1(r, c) = sts(r, c);

	stokes_param = stsm1 * s;	
  	ringq->PixVal(pix) = stokes_param(0);
  	ringu->PixVal(pix) = stokes_param(2);
	
      }
      
    }
    
  }
  
  

  catch( PThrowable& exc )
  {
    cerr << " Exception: " << exc.Msg() << endl;
  }
  
  return;
}