// -----------------------------------------------------------------------------//
//    MISALIGNMENT MATRIX CALCULATION FOR JEMX                                  //
//                                                                              //
// -- author: Pascal Favre, ISDC                                                //
//    pascal.favre@obs.unige.ch                                                 // 
//                                                                              //
// -- root help:                                                                // 
//    http://root.cern.ch/root/html/TMinuit.html                                //
//    http://root.cern.ch/root/html/ClassIndex.html                             //
//                                                                              //
// -- minimization: see doc                                                     //
//                                                                              //
const Double_t sigma_ra=0.001*(3.14159265358979323846/180.0);
const Double_t sigma_dec=0.001*(3.14159265358979323846/180.0);
const Int_t numlines=352;
//                                                                              //
// -----------------------------------------------------------------------------//

const Double_t pi = 3.14159265358979323846; 

Double_t mes[3*numlines];
Double_t cat[3*numlines];
Double_t error[3*numlines];

Double_t ra_scx[numlines]; 
Double_t dec_scx[numlines];
Double_t ra_scz[numlines]; 
Double_t dec_scz[numlines];

// ---------- Compute chi-square -----------------------------------------------//

// Approximation of Minuit Fcn: FCN(npar: num of variable params,grad: optional 
// vector of first deriv, fval: calculated function value,xval: vect of parms,
// iflag: what is to be calculated)

void chisq(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag)
{  
  Double_t chi=0.0;
  Double_t chisquare=0.0;
  Double_t stmscatx,stmscaty,stmscatz;
  TMatrixD S(3,3);

    for (Int_t ind=0;ind<numlines; ind++) {
      attitude(ra_scx[ind],dec_scx[ind],ra_scz[ind],dec_scz[ind],S);
      equa(cat[3*ind],cat[3*ind+1],cat[3*ind+2],S,stmscatx,stmscaty,stmscatz,par);
      stmscatx-=mes[3*ind];
      stmscaty-=mes[3*ind+1];
      stmscatz-=mes[3*ind+2];
      stmscatx/=error[3*ind];
      stmscaty/=error[3*ind+1];
      stmscatz/=error[3*ind+2];
      chi=stmscatx*stmscatx+stmscaty*stmscaty+stmscatz*stmscatz;
      chisquare=chi+chisquare;
    }
    f=chisquare;
}

 // ---------- Function attitude matrix ----------------------------------------//
  
void attitude(Double_t ra_scx,Double_t dec_scx,Double_t ra_scz,Double_t dec_scz,
	      TMatrixD &S)
{
  //this way: writing directly S (and not first ST and then transpose as before) 

  S(0,0)=cos(dec_scx*pi/180.0)*cos(ra_scx*pi/180.0);
  S(0,1)=cos(dec_scx*pi/180.0)*sin(ra_scx*pi/180.0);
  S(0,2)=sin(dec_scx*pi/180.0);
  S(2,0)=cos(dec_scz*pi/180.0)*cos(ra_scz*pi/180.0);
  S(2,1)=cos(dec_scz*pi/180.0)*sin(ra_scz*pi/180.0);
  S(2,2)=sin(dec_scz*pi/180.0);
  S(1,0)=S(2,1)*S(0,2)-S(2,2)*S(0,1);
  S(1,1)=S(2,2)*S(0,0)-S(2,0)*S(0,2);
  S(1,2)=S(2,0)*S(0,1)-S(2,1)*S(0,0);
}


// ---------- Function S^TMS(cat)=(mes) ----------------------------------------//

// parameters: par[0]=alpha, par[1]=beta, par[2]=gamma, in radians !!

void equa(Double_t catx,Double_t caty,Double_t catz,TMatrixD &S,
	  Double_t &stmscatx,Double_t &stmscaty,Double_t &stmscatz,
	  Double_t *par)
{
  Double_t Ca=cos(par[0]);
  Double_t Sa=sin(par[0]);
  Double_t Cb=cos(par[1]);
  Double_t Sb=sin(par[1]);
  Double_t Cg=cos(par[2]);
  Double_t Sg=sin(par[2]);
  
  TMatrixD m(3,3);

  m(0,0)=Cb*Cg;
  m(0,1)=Cg*Sa*Sb-Ca*Sg;
  m(0,2)=Ca*Cg*Sb+Sa*Sg;
  m(1,0)=Cb*Sg;
  m(1,1)=Ca*Cg+Sa*Sb*Sg;
  m(1,2)=(-Cg*Sa+Ca*Sb*Sg);
  m(2,0)=(-Sb);
  m(2,1)=Cb*Sa;
  m(2,2)=Ca*Cb;
  
  //not yet implemented...TMatrixD stms(S, TMatrixD::kAtBA, m);
  TMatrixD ST(TMatrixD::kTransposed,S);
  TMatrixD ms(3,3);
  ms.Mult(m,S);
  TMatrixD stms(3,3);
  stms.Mult(ST,ms);

  TMatrixD cata(3,1);
  cata(0,0)=catx;
  cata(1,0)=caty;
  cata(2,0)=catz;
  
  TMatrixD stmscat(3,1);
  stmscat.Mult(stms,cata);

  stmscatx=stmscat(0,0);
  stmscaty=stmscat(1,0);
  stmscatz=stmscat(2,0);
}

// ---------- Function "testing" -----------------------------------------------//

void testing(TMatrixD S_n,TMatrixD M,Double_t mesx,Double_t mesy,Double_t mesz,
	     Double_t &corx,Double_t &cory,Double_t &corz)
{ 
  TMatrixD ST_n(TMatrixD::kTransposed,S_n);
  TMatrixD MS_n=TMatrixD(3,3);
  MS_n.Mult(M,S_n);
  TMatrixD ST_nMS_n=TMatrixD(3,3);
  ST_nMS_n.Mult(ST_n,MS_n);

  TMatrixD CO(TMatrixD::kTransposed,ST_nMS_n);
  corx=CO(0,0)*mesx+CO(0,1)*mesy+CO(0,2)*mesz;
  cory=CO(1,0)*mesx+CO(1,1)*mesy+CO(1,2)*mesz;
  corz=CO(2,0)*mesx+CO(2,1)*mesy+CO(2,2)*mesz;
}

// ---------- Main function ---------------------------------------------------//

void misfit()
{
  // gROOT->Reset(); don't put this !!! reset the global variables !!!
  
  FILE *file = fopen("sources","r");
  
  Double_t ra_cat[numlines]; 
  Double_t dec_cat[numlines];
  Double_t ra_mes[numlines]; 
  Double_t dec_mes[numlines];
   
  Int_t nline=0;
  
  while (nline<numlines){
    fscanf(file,"%lf %lf %lf %lf %lf %lf %lf %lf\n",&ra_cat[nline],&dec_cat[nline],&ra_mes[nline],&dec_mes[nline],&ra_scx[nline],&dec_scx[nline],&ra_scz[nline],&dec_scz[nline]);
    nline++;
  }

  for (Int_t i=0;i<numlines;i++){
    cout << "ra cat: " << ra_cat[i] << " dec cat: " << dec_cat[i] << " ra mes: " << ra_mes[i] << " dec mes: " << dec_mes[i] << endl;
  }
  fclose(file);
  cout << "used "<< numlines << " sources"<< endl;

  // vector cat; catalog, norm is unity  

  
  for (Int_t j=0;j<numlines;j++){
    cat[3*j]=cos(dec_cat[j]*pi/180.0)*cos(ra_cat[j]*pi/180.0);
    cat[3*j+1]=cos(dec_cat[j]*pi/180.0)*sin(ra_cat[j]*pi/180.0);
    cat[3*j+2]=sin(dec_cat[j]*pi/180.0); 
  }
  
  // vector mes; measured, norm is unity 

  for (Int_t k=0;k<numlines;k++){
    mes[3*k]=cos(dec_mes[k]*pi/180.0)*cos(ra_mes[k]*pi/180.0);
    mes[3*k+1]=cos(dec_mes[k]*pi/180.0)*sin(ra_mes[k]*pi/180.0);
    mes[3*k+2]=sin(dec_mes[k]*pi/180.0);
  }
	      
  // errors on the individual components of the vectors

  for (Int_t l=0;l<numlines;l++){
    error[3*l]=sqrt(sin(dec_mes[l]*pi/180.0)*sin(dec_mes[l]*pi/180.0)*cos(ra_mes[l]*pi/180.0)*cos(ra_mes[l]*pi/180.0)*sigma_dec*sigma_dec+cos(dec_mes[l]*pi/180.0)*cos(dec_mes[l]*pi/180.0)*sin(ra_mes[l]*pi/180.0)*sin(ra_mes[l]*pi/180.0)*sigma_ra*sigma_ra);
    error[3*l+1]=sqrt(sin(dec_mes[l]*pi/180.0)*sin(dec_mes[l]*pi/180.0)*sin(ra_mes[l]*pi/180.0)*sin(ra_mes[l]*pi/180.0)*sigma_dec*sigma_dec+cos(dec_mes[l]*pi/180.0)*cos(dec_mes[l]*pi/180.0)*cos(ra_mes[l]*pi/180.0)*cos(ra_mes[l]*pi/180.0)*sigma_ra*sigma_ra);
    error[3*l+2]=sqrt(cos(dec_mes[l]*pi/180.0)*cos(dec_mes[l]*pi/180.0))*sigma_dec;
    printf("sigma on coordinate 1 %1.8lf ",error[3*l]);
    printf("sigma on coordinate 2 %1.8lf ",error[3*l+1]);
    printf("sigma on coordinate 3 %1.8lf\n",error[3*l+2]);
  }
  
  // ---------- Minimization with Minuit --------------------------------------//
  
  cout << "\n now computing S^T M S, using (cat) and (mes)\n" << endl;  

  // argument in the constructor: max number of param
  
  TMinuit *gMin = new TMinuit(3);
  
  // method used to set the address of the minimization function  
  // Warning: gMin is a pointer thus use -> instead of gMin.SetFCN()
  
  gMin->SetFCN(chisq);
  
  // mnexcm: "MiNuitEXecuteCoMmand"
  // mnexcm("command",numeric(s) arg of the command,num of arg specified,
  // error flag (0 if went fine))
  
  // SET ERRordef [up]
  
  Double_t arglist[3]; // array of size max num of param
  Int_t ierflg = 0;
  arglist[0] = 1;      // default for a chisquare
  gMin->mnexcm("SET ERR",arglist,1,ierflg);
  
  // starting values and step sizes for parameters
  
  Double_t start=0.0; // this is radian !!
  Double_t ste=0.00000001;
  
  Double_t vstart[3] = {start,start,start};
  Double_t step[3] = {ste,ste,ste};
  
  // MNPARM(param num, "param name", start val, step size, low bound, high bound, 
  // err flag). If low bound=high bound=0 then the param is considered unbounded
  
  gMin->mnparm(0,"alpha",vstart[0],step[0],0,0,ierflg);
  gMin->mnparm(1,"beta",vstart[1],step[1],0,0,ierflg);
  gMin->mnparm(2,"gamma",vstart[2],step[2],0,0,ierflg);
  
  // launch Migrad : Migrad [maxcalls] [tolerance]
  
  arglist[0] = 500; // *approximated* max number of calls, even if did not converge
  arglist[1] = 1.;  // minimization stop when EDM < 0.001*tolerance*UP; default tolerance 0.1
  gMin->mnexcm("MIGRAD",arglist,2,ierflg);
  
  // results
  // MNSTAT(fmin: best function found,edm,errdef: UP,npari: num of var params,
  // nparx,istat: has to be 3!)
  
  Double_t fmin,edm,errdef;
  Int_t nvpari,nparx,istat;
  gMin->mnstat(fmin,edm,errdef,nvpari,nparx,istat);
  gMin->mnprin(3,fmin);
  
  Double_t param[3];
  TString name;
  Double_t val;
  Double_t error;
  Double_t bnd1;
  Double_t bnd2;
  Int_t ivarbl;
  
  for (Int_t t=0;t<3;t++){
      gMin->mnpout(t,name,val,error,bnd1,bnd2,ivarbl);
      param[t]=val;
  }
  
  // Final Misalignment Matrix Obtained by Minimization
  
  TMatrixD M=TMatrixD(3,3);  
  Double_t alpha=param[0]; // still radians, thus no need *180.0/pi below
  Double_t beta=param[1];
  Double_t gamma=param[2];
  
  // line 0

  M(0,0)=cos(beta)*cos(gamma);
  M(0,1)=cos(gamma)*sin(alpha)*sin(beta)-cos(alpha)*sin(gamma);
  M(0,2)=cos(alpha)*cos(gamma)*sin(beta)+sin(alpha)*sin(gamma);
  
  // line 1
  
  M(1,0)=cos(beta)*sin(gamma);
  M(1,1)=cos(alpha)*cos(gamma)+sin(alpha)*sin(beta)*sin(gamma);
  M(1,2)=(-cos(gamma))*sin(alpha)+cos(alpha)*sin(beta)*sin(gamma);
  
  // line 2
  
  M(2,0)=(-sin(beta));
  M(2,1)=cos(beta)*sin(alpha);
  M(2,2)=cos(alpha)*cos(beta);
  
  cout << "\n Matrix M: " << endl;
  M.Print("");
  
  cout << "since diagonal elements are rounded off, here are the details...\n" << endl;
  
  for (Int_t j=0;j<3;j++){
    for (Int_t p=0;p<3;p++){
      printf("%1.12lf ",M(j,p));
    }
    printf(" \n");
  }
  
  // Testing: det=1
  
  Double_t det;
  
  det=M.Determinant();
  
  printf("\n testing orthogonality: det M = %1.12lf",det);
  
  // Testing: orthogonality M^T M=1
  
  cout << "\n testing orthogonality: M^T M = " << endl;
  
  TMatrixD MT(TMatrixD::kTransposed,M);
  TMatrixD result=TMatrixD(3,3);
  
  result.Mult(MT,M);
  result.Print("");

  // ---------- Fill the fits file --------------------------------------------// 
  
  dal_element *fitsfile=NULL;
  DALobjectOpen("inst_misalign_20030701.fits[1]",&fitsfile,0);
  double buffer[9];
  int l=0;
  for (int i=0;i<3;i++)
    {
      for (int j=0;j<3;j++)
	{
	  buffer[l]=M(i,j);
	  l++;
	} 
    }
  long nrow=5;   // jemx2
  DALtablePutColBins(fitsfile,"MATRIX",0,DAL_DOUBLE,nrow,nrow,9,buffer,0);
  DALobjectClose(fitsfile,DAL_SAVE,0);

  //---------- Testing on mes data --------------------------------------------//
  
  cout << "\n we now apply (S^TMS)^T on mes[..] to deduce the corrected \n" << endl;
  cout << "\n vectors cor, to be compared with cat \n" << endl; 
  
  Double_t cor[3*numlines];
  TMatrixD S_n(3,3);
  Double_t corx,cory,corz;
  
  for (Int_t i=0;i<numlines;i++){
    attitude(ra_scx[i],dec_scx[i],ra_scz[i],dec_scz[i],S_n);
    testing(S_n,M,mes[3*i],mes[3*i+1],mes[3*i+2],corx,cory,corz);
    cor[3*i]=corx;
    cor[3*i+1]=cory;
    cor[3*i+2]=corz;
  }    

  Double_t ra_new1[numlines];
  Double_t ra_new2[numlines];
  
  Double_t dec_new[numlines];
  Double_t ra_new[numlines];
  
  cout << "\n here are the coordinates found \n" << endl;
  
  for (Int_t j=0;j<numlines;j++){
    dec_new[j]=asin(cor[3*j+2])*180.0/pi;
    cout << "#" << j << " dec final: " << dec_new[j] << endl;
    
    //quadrant I
    if ((cor[3*j]>0) && (cor[3*j+1]>0)){
      cout<< "quadrant I"<<endl; 
      ra_new1[j]=acos(cor[3*j]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new2[j]=asin(cor[3*j+1]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new[j]=ra_new1[j];
      cout << "#" << j << " ra_new1: " << ra_new1[j] << " ra_new2: " << ra_new2[j] << " ra final: " << ra_new[j] << endl;
    }
    //quadrant II
    if ((cor[3*j]<0) && (cor[3*j+1]>0)){
      cout<< "quadrant II"<<endl; 
      ra_new1[j]=acos(cor[3*j]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new2[j]=asin(cor[3*j+1]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new[j]=ra_new1[j];
      cout << "#" << j << " ra_new1: " << ra_new1[j] << " ra_new2: " << ra_new2[j] << " ra final: " << ra_new[j] <<endl;
    }
    //quadrant III
    if ((cor[3*j]<0) && (cor[3*j+1]<0)){
      cout<< "quadrant III"<<endl;
      ra_new1[j]=acos(cor[3*j]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new2[j]=asin(cor[3*j+1]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new[j]=360.0-ra_new1[j]; 
      cout << "#" << j << " ra_new1: " << ra_new1[j] << " ra_new2: " << ra_new2[j] << " ra final: " << ra_new[j] << endl;
    }
    //quadrant IV
    if ((cor[3*j]>0) && (cor[3*j+1]<0)){
      cout<< "quadrant IV"<<endl;
      ra_new1[j]=acos(cor[3*j]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new2[j]=asin(cor[3*j+1]/cos(dec_new[j]*pi/180.0))*180.0/pi;
      ra_new[j]=360.0+ra_new2[j]; 
      cout << "#" << j << " ra_new1: " << ra_new1[j] << " ra_new2: " << ra_new2[j] << " ra final: " << ra_new[j] << endl;
    }
  }

  //---------- Display --------------------------------------------------------//

  Int_t delta=1;

  if (delta==1){  
      
      // delta ra vs delta dec; displacement of the clusters
      
      Double_t drames[numlines];
      Double_t ddecmes[numlines];
      Double_t dranew[numlines];
      Double_t ddecnew[numlines];

      // pull on each axis, (0,0) is the catalog point
  
      for (j=0;j<numlines;j++){
	  drames[j]=ra_cat[j]-ra_mes[j];
	  ddecmes[j]=dec_cat[j]-dec_mes[j];
	  dranew[j]=ra_cat[j]-ra_new[j];
	  ddecnew[j]=dec_cat[j]-dec_new[j];
      } 
  
      TGraph *mesu=new TGraph(numlines,drames,ddecmes);
      TGraph *corr=new TGraph(numlines,dranew,ddecnew);
  } else {
    TGraph *cata=new TGraph(numlines,ra_cat,dec_cat);
    TGraph *mesu=new TGraph(numlines,ra_mes,dec_mes);
    TGraph *corr=new TGraph(numlines,ra_new,dec_new);
    // insert results with dal3aux
    FILE *file9 = fopen("cygx1_7jan03_results.ascii","r"); 
    const Int_t num9=22;
    Double_t ra_mes9[num9]; 
    Double_t dec_mes9[num9];
    Int_t nline=0;
    while (nline<num9){
      fscanf(file9,"%lf %lf\n",&ra_mes9[nline],&dec_mes9[nline]);
      nline++;
    }
    fclose(file9);
    TGraph *dal=new TGraph(num9,ra_mes9,dec_mes9);
  }
  TCanvas *c1= new TCanvas("jemx2","JEMX2 misalignment correction",200,10,1000,1000);
  c1->SetGrid();
  c1->SetFillColor(kWhite);
  // multi graph 
  TMultiGraph *m1= new TMultiGraph();
  if (delta==0){cata->SetMarkerStyle(2);
  cata->SetMarkerColor(kGreen);
  m1->Add(cata);}
  mesu->SetMarkerStyle(5);
  mesu->SetMarkerColor(kBlack);
  m1->Add(mesu);
  corr->SetMarkerStyle(30);
  corr->SetMarkerColor(kRed);
  m1->Add(corr);
  if (delta==0){dal->SetMarkerStyle(5);
  dal->SetMarkerColor(kBlue);
  m1->Add(dal);}
  m1->Draw("AP");
  m1->GetXaxis()->SetLabelFont(50);
  m1->GetXaxis()->SetLabelSize(0.03);
  m1->GetYaxis()->SetLabelFont(50);
  m1->GetYaxis()->SetLabelSize(0.03);
  //m1->GetXaxis()->SetTitle("RA [deg]");   
  //m1->GetYaxis()->SetTitle("DEC [deg]");
  m1->GetXaxis()->SetTitle("delta RA [deg]");   
  m1->GetYaxis()->SetTitle("delta DEC [deg]");
  m1->GetXaxis()->CenterTitle();    
  m1->GetYaxis()->SetTitleOffset(1.5);
  m1->Draw("AP");
  // label
  TPaveLabel *label = new TPaveLabel(0,0.09,0.14,0.098,"Crab observed by JEMX-2");
  label->SetFillColor(kWhite);
  label->SetTextSize(.38);
  label->SetTextColor(kBlack);
  label->Draw();
  TText *ttt = new TText();
  ttt->SetTextFont(50);
  ttt->SetTextColor(kBlack);
  ttt->SetTextSize(0.02);
  ttt->DrawText(299.435,35.135,"positions measured with a unity matrix");
  TText *tttt = new TText();
  tttt->SetTextFont(50);
  tttt->SetTextColor(kRed);
  tttt->SetTextSize(0.02);
  tttt->DrawText(299.505,35.205,"positions corrected with the misalignment matrix");
  TText *tt = new TText();
  tt->SetTextFont(50);
  tt->SetTextColor(kBlue);
  tt->SetTextSize(0.02);
  tt->DrawText(299.508,35.195,"result of the analysis with the updated software");
  TText *tc = new TText();
  tc->SetTextFont(50);
  tc->SetTextColor(kGreen);
  tc->SetTextSize(0.02);
  tc->DrawText(299.545,35.201,"catalog position");
  TText *td = new TText();
  td->SetTextFont(50);
  td->SetTextColor(kBlack);
  td->SetTextSize(0.02);
  td->DrawText(0.1,0.001,"P. Favre, ISDC (14 MAY 2003)");
  c1->Modified();
  c1->Update();
    
}