/**
 * SaaaatInc
 * author  Malakhov V.
 * version 1.0 - 05/02/2007
 */

#include "InclinationInfo.h" 
 
#include <mcmd/McmdEvent.h>
#include <mcmd/McmdRecord.h>
#include <EventHeader.h>
#include <PscuHeader.h>
#include "RunHeaderEvent.h"
#include <TTree.h>
#include "sgp4.h"
#include "TH2F.h"

#include "TFrame.h"
#include "TGraph.h"
#include "TCanvas.h"

#include <TDatime.h>
#include <TFile.h>

#include <TTimeStamp.h>
#include "TString.h"
#include "TObjString.h"
#include "TPaletteAxis.h"
#include "TROOT.h"
#include <sys/stat.h>
#include <fstream>
#include <iostream>

#include "OrbitalRate.h"

//#include "TMatrixD.h"


using namespace std;

int main(int argc, char* argv[]){

  TString *rootFile = NULL;
  TString outDir  = "./";
  TString mapFile = "";
  TString tleFile = "";
  int offDate = 20070408;
    //int offDate = 20060614;
  int offTime = 210000;

  if (argc < 2){
    printf("You have to insert at least the file to analyze and the mapFile \n");
    printf("Try '--help' for more information. \n");
    exit(1);
  }  

  if (!strcmp(argv[1], "--help")){
    //printf( "Usage: OrbitRate FILE -map mapFile [OPTION] \n");
    //printf( "mapFile have to be a mercator map image [gif|jpg|png] \n");
    printf( "\t --help                 Print this help and exit \n");
    printf( "\t -tle[File path]         Path where to find the tle infos \n");
    printf( "\t\tUse the script retrieve_TLE.sh to create the file.\n ");
    printf( "\t -outDir[path]          Path where to put the output without last directory.\n");
    printf( "\t -offDate               Date of resetting of the Resource counter [format YYMMDD (UTC date) default 20060928] \n");
    printf( "\t -offTime               Time of resetting of the Resource counter [format HHMMSS (UTC date) default 210000] \n");
    exit(1);
  }

  // Ok, here we should have at least one root file.  We check that
  // the filename contains ".root".
  if(strstr(argv[1], ".root"))
    rootFile = new TString(argv[1]);
  else {
    cerr << "OrbitalRate: no root file." << endl << "See --help" << endl;
    exit(EXIT_FAILURE);
  }

  for (int i = 2; i < argc; i++){
    if (!strcmp(argv[i], "-outDir")){
      if (++i >= argc){
	printf( "-outDir needs arguments. \n");
	printf( "Try '--help' for more information. \n");
	exit(1);
      } else {
	outDir = argv[i];
	continue;
      }
    }

    if (!strcmp(argv[i], "-tle")){
      if (++i >= argc){
	printf( "-tle needs arguments. \n");
	printf( "Try '--help' for more information. \n");
	exit(1);
      } else {
	tleFile = argv[i];
	continue;
      }
    }

    if (!strcmp(argv[i], "-offTime")){
      if (++i >= argc){
	printf( "-offTime needs arguments. \n");
	printf( "Try '--help' for more information. \n");
	exit(1);
      } 
      else{
	offTime = atol(argv[i]);
	continue;
      }
    }

    if (!strcmp(argv[i], "-offDate")){
      if (++i >= argc){
	printf( "-offDate needs arguments. \n");
	printf( "Try '--help' for more information. \n");
	exit(1);
      } 
      else{
	offDate = atol(argv[i]);
	continue;
      }
    }

  }

    Rate(rootFile, outDir, mapFile, tleFile, offDate, offTime);

 }


void Rate(TString *filename, TString outDirectory = "", TString mapFile = "", TString tleFile = "", int offDate = 20060614, int offTime = 210000)
{
  // **** Offset to temporarily correct the TDatime bug ****/
  //  offTime += 10000;
  //********************************************************/

  McmdScan *mcmdReader = new McmdScan();

  pamela::McmdEvent      *mcmdev     = 0;
  pamela::McmdRecord     *mcmdrc     = 0;
  pamela::EventHeader    *eh         = 0;
  pamela::PscuHeader     *ph	   = 0;
  TArrayC                *mcmddata;
  
  pamela::RunHeaderEvent *reh = new pamela::RunHeaderEvent;
  pamela::EventHeader    *eH  = new pamela::EventHeader;
  
  Quaternions *L_QQ_Q_l = new Quaternions();
  InclinationInfo *Plux     = new InclinationInfo();
  
  float timesync = 0, obt_timesync = 0;
  
  ULong64_t nevents = 0;
  stringstream 	oss;

  Long64_t offsetTime = 0;
  Long64_t timeElapsedFromTLE = 0;
  Long64_t deltaTime = 0, oldtimeElapsedFromTLE = 0;
  bool a_second_is_over;

  // Get a char* to "file" from "/dir1/dir2/.../file.root"
  TString basename;
  basename = ((TObjString*) filename->Tokenize('/')->Last())->GetString(); // we get file.root
  basename = ((TObjString*)basename.Tokenize('.')->First())->GetString(); // we get file
  
  TFile *rootFile = new TFile(*filename);

  if (rootFile->IsZombie()) printf("The %s file does not exist",basename.Data());
  TString fileName = ((TObjString*)basename.Tokenize('/')->Last())->GetString();
  TString filePath = basename;
  filePath.ReplaceAll(fileName, "");
  filePath.ReplaceAll(".root", "");

  TTree *tr = (TTree*)rootFile->Get("Mcmd");
  nevents = tr->GetEntries();
  tr->SetBranchAddress("Mcmd",&mcmdev);
  tr->SetBranchAddress("Header",&eh);
  
  Long_t firstime = 99999999;//ph->GetOrbitalTime();
  UInt_t  firstID  = 65535;
  Long_t lastime = 0;//ph->GetOrbitalTime();
  UInt_t lastID = 0;
  for(int ii = 0; ii < nevents; ii++){
    tr->GetEntry(ii);
    ph = eh->GetPscuHeader();
    Int_t tmpSize = mcmdev->Records->GetEntries();
    for (int qq=0; qq < tmpSize; qq++){
	mcmdrc = (pamela::McmdRecord*)mcmdev->Records->At(qq);
	if (mcmdrc->SeqID <= firstID) firstID = mcmdrc->SeqID;
	if (mcmdrc->SeqID >= lastID)  lastID  = mcmdrc->SeqID;
	if ((int)mcmdrc->ID1 == 226){
        L_QQ_Q_l->fill(mcmdrc->McmdData);
	if (L_QQ_Q_l->time[0] >= lastime) lastime = L_QQ_Q_l->time[0];
	if (L_QQ_Q_l->time[0] <= firstime) firstime = L_QQ_Q_l->time[0];
	}		
	};
    }
    
  TTree *tp = (TTree*)rootFile->Get("RunHeader");
  tp->SetBranchAddress("Header", &eH);
  tp->SetBranchAddress("RunHeader", &reh);
  tp->GetEntry(0);
  ph = eH->GetPscuHeader();
  ULong_t TimeSync = reh->LAST_TIME_SYNC_INFO;
  ULong_t ObtSync = reh->OBT_TIME_SYNC;
  ULong_t DeltaOBT = TimeSync - ObtSync;
  firstime = firstime - DeltaOBT;
  lastime = lastime - DeltaOBT;
    
  //Get Orientation information
  
  TH2F *_L1 = new TH2F("_L1",basename+";OnBoard Time, s; Value of l0",1000,firstime,lastime,1000,-1,1);
  TH2F *_L2 = new TH2F("_L2",basename+";OnBoard Time, s; Value of l1",1000,firstime,lastime,1000,-1,1);
  TH2F *_L3 = new TH2F("_L3",basename+";OnBoard Time, s; Value of l2",1000,firstime,lastime,1000,-1,1);
  TH2F *_L4 = new TH2F("_L4",basename+";OnBoard Time, s; Value of l3",1000,firstime,lastime,1000,-1,1); 
 
  //TH2F *_A1 = new TH2F("_A1",basename,1000,firstime,lastime,1000,-180,180);
  //TH2F *_A2 = new TH2F("_A2",basename,1000,firstime,lastime,1000,-180,180);
  //TH2F *_A3 = new TH2F("_A3",basename,1000,firstime,lastime,1000,-180,180);
  
  //TH2F *_secID = new TH2F("_secID",basename,1000,firstime,lastime,1000,firstID,lastID);
  
  TH2F *_Tangazh = new TH2F("Pitch",basename+";OnBoard Time, s;Angle of pith, deg",1000,firstime,lastime,1000,-20,20);
  TH2F *_Ryskanie = new TH2F("Yaw",basename+";OnBoard Time, s;Angle of yaw, deg",1000,firstime,lastime,1000,-20,20);
  TH2F *_Kren = new TH2F("Roll",basename+";OnBoard Time, s;Angle of roll, deg",1000,firstime,lastime,1000,-50,50);
  
  // Open the root file.
  rootFile = new TFile(filename->Data());
  if (rootFile->IsZombie()) {
    printf("The file %s does not exist\n", (filename->Data()));
    exit(EXIT_FAILURE);
  }

  // Look for a timesync in the TFile rootFile.  We also get the obt
  // of the timesync mcmd.
  bool err;
  err = lookforTimesync(rootFile, &timesync, &obt_timesync);
  if(!err) {
    cerr << "Warning!!! No timesync info has been found in the file "
	 << filename->Data() << endl;
    exit(EXIT_FAILURE);
  }

  //Get the Julian date of the Resours offset
  TDatime offRes = TDatime(offDate, offTime);
  // Add to the Resours Offset the timesync.  This is now the date at
  // the moment of the timesync.
  ULong_t TH=offRes.Convert();
  //cout<<"TH= "<<TH/86400<<"\n";
  offRes.Set(offRes.Convert() + (UInt_t) timesync);
  
  // Now I need a pointer to a cTle object.  The class misses a
  // constructor without arguments, so we have to give it a dummy TLE.
  string str1 = "RESURS-DK 1";
  string str2 = "1 29228U 06021A   06170.19643714  .00009962  00000-0  21000-3 0   196";
  string str3 = "2 29228 069.9363 054.7893 0167576 127.4359 017.0674 15.31839265   604";
  cTle *tle1 = new cTle(str1, str2, str3);

  // If we have to use a TLE file, call getTle().
  if (tleFile != "")
    tle1 = getTle(tleFile, offRes);

  cOrbit       orbit(*tle1);
  cEci         eci;
  cCoordGeo    coo;

  // Get the Julian date of the TLE epoch
  string datetime = getTleDatetime(tle1);
  TDatime tledate = TDatime(datetime.c_str());

    
  tr = (TTree*)rootFile->Get("Mcmd");
  nevents = tr->GetEntries();
  tr->SetBranchAddress("Mcmd", &mcmdev);
  tr->SetBranchAddress("Header", &eh);
  
 // oss.str("");
 // oss << basename+".txt";
  
 // ofstream outputFile;
 // outputFile.open(oss.str().c_str());
  
  Int_t tmpSize;
  
  for(UInt_t i = 0; i < nevents; i++) //Fill variables from root-ple
    { 
      tr->GetEntry(i);
      tmpSize = mcmdev->Records->GetEntries();
      ph = eh->GetPscuHeader();    
      
      for (Int_t j3 = 0; j3 < tmpSize; j3++){
        mcmdrc = (pamela::McmdRecord*)mcmdev->Records->At(j3);
	if ((int)mcmdrc->ID1 == 226){
	    L_QQ_Q_l->fill(mcmdrc->McmdData);
	    //Plux->QuaternionstoAngle(*L_QQ_Q_l);
	    for (Int_t oi = 0; oi < 6; oi++){
	        //if ((Int_t)L_QQ_Q_l->CodeErrQua[oi]==0){
		    Int_t timeO = L_QQ_Q_l->time[oi] - DeltaOBT;
		    _L1->Fill(timeO,L_QQ_Q_l->quat[oi][0]);
		    _L2->Fill(timeO,L_QQ_Q_l->quat[oi][1]);
		    _L3->Fill(timeO,L_QQ_Q_l->quat[oi][2]);
		    _L4->Fill(timeO,L_QQ_Q_l->quat[oi][3]);
		    
		    //outputFile << " " << L_QQ_Q_l->time[oi] << " " << -L_QQ_Q_l->quat[oi][0] << " " << -L_QQ_Q_l->quat[oi][1] << " " << -L_QQ_Q_l->quat[oi][2] << " " << -L_QQ_Q_l->quat[oi][3];
		    
		    //_A1->Fill(L_QQ_Q_l->time[oi],Plux->A11);
		    //_A2->Fill(L_QQ_Q_l->time[oi],Plux->A12);
		    //_A3->Fill(L_QQ_Q_l->time[oi],Plux->A13);
		    
		    //outputFile << " " << Plux->A11 << " " << Plux->A12 << " " << Plux->A13;
		     
		    //_secID->Fill(L_QQ_Q_l->time[oi],mcmdrc->SeqID);
		    
		    timeElapsedFromTLE = TH - (Long64_t) tledate.Convert()+ L_QQ_Q_l->time[oi];
		    orbit.getPosition(((double) timeElapsedFromTLE)/60., &eci);
		    //outputFile<<" "<< eci.getPos().m_z;//eci.getPos().m_x<<" "<<eci.getPos().m_y<<" "<<eci.getPos().m_z<<" "<<eci.getVel().m_x<<" "<<eci.getVel().m_y<<" "<<eci.getVel().m_z;
		    Plux->TransAngle(eci.getPos().m_x,eci.getPos().m_y,eci.getPos().m_z,eci.getVel().m_x,eci.getVel().m_y,eci.getVel().m_z,L_QQ_Q_l->quat[oi][0],L_QQ_Q_l->quat[oi][1],L_QQ_Q_l->quat[oi][2],L_QQ_Q_l->quat[oi][3]);
		    
		    _Tangazh->Fill(timeO,Plux->Tangazh);
		    _Ryskanie->Fill(timeO,Plux->Ryskanie);
		    _Kren->Fill(timeO,Plux->Kren);
		    
		    //outputFile<<" "<<Plux->Tangazh<<"  "<<Plux->Kren<<" "<<Plux->Ryskanie<<"\n";
		//}		
	    }
	}
      }
    }

    //outputFile.close();
    
    TCanvas *c1 = new TCanvas("c1","Quaternions",1200,1600);
    //TCanvas *c2 = new TCanvas("c2","Anglees",1200,1600);
    TCanvas *c4 = new TCanvas("c4","AngleesTRK",1200,1600);  
  
    c1->Divide(1,4);
    //c2->Divide(1,3);
    c4->Divide(1,3);

    c1->cd(1);
    _L1->SetMarkerColor(kBlack);
    _L1->Draw("");
  
    c1->cd(2);
    _L2->SetMarkerColor(kBlue);
    _L2->Draw("");
    
    c1->cd(3);
    _L3->SetMarkerColor(kRed);
    _L3->Draw("");
  
    c1->cd(4);
    _L4->SetMarkerColor(kGreen);
    _L4->Draw("");
  
//    c1->cd(5);
//    _secID->SetMarkerColor(kYellow);
//    _secID->Draw("");
/*  
    c2->cd(1);
    _A3->SetMarkerColor(kRed);
    _A3->Draw("");
  
    c2->cd(2);
    _A2->SetMarkerColor(kGreen);
    _A2->Draw("");
  
    c2->cd(3);
    _A1->SetMarkerColor(13);
    _A1->Draw("");
*/	
    c4->cd(1);
    _Tangazh->SetMarkerColor(142);
    _Tangazh->Draw("");
	
    c4->cd(2);
    _Ryskanie->SetMarkerColor(226);
    _Ryskanie->Draw("");
	
    c4->cd(3);
    _Kren->SetMarkerColor(142);
    _Kren->Draw("");
	
    oss.str("");
    oss << outDirectory+basename+"/"+basename << "_Inclinations_Quaternions.png";
    c1->SaveAs(oss.str().c_str());
    oss.str();

/*    oss.str("");
    oss << basename<< "_Angles.png";
    c2->SaveAs(oss.str().c_str());
    oss.str();
  */
    oss.str("");
    oss <<  outDirectory+basename+"/"+basename << "_Inclinations_Angles.png";
    c4->SaveAs(oss.str().c_str());
    oss.str();
      


  delete _L1;
  delete _L2;
  delete _L3;
  delete _L4;
  
 // delete _A1;
 // delete _A2;
 // delete _A3;
  
  //delete _secID;
  
  delete _Tangazh;
  delete _Kren;
  delete _Ryskanie;

  rootFile->Close();
}

// Get the TLE from tleFile.  The right TLE is that one with the
// closest previous date to offRes, that is the date at the time of
// the first timesync found in the root file.
//
// Warning: you must use a tle file obtained by space-track.org
// querying the database with the RESURS DK-1 id number 29228,
// selecting the widest timespan, including the satellite name in the
// results.
cTle *getTle(TString tleFile, TDatime offRes)
{
  Float_t tledatefromfile, tledatefromroot;
  fstream tlefile(tleFile.Data(), ios::in);
  vector<cTle*> ctles;
  vector<cTle*>::iterator iter;


  // Build a vector of *cTle  
  while(1) {
    cTle *tlef;
    string str1, str2, str3;

    getline(tlefile, str1);
    if(tlefile.eof()) break; 

    getline(tlefile, str2);
    if(tlefile.eof()) break;

    getline(tlefile, str3);
    if(tlefile.eof()) break;

    // We now have three good lines for a cTle.
    tlef = new cTle(str1, str2, str3);
    ctles.push_back(tlef);
  }

  // Sort by date
  sort(ctles.begin(), ctles.end(), compTLE);

  tledatefromroot = (offRes.GetYear()-2000)*1e3 + (offRes.Convert() - (TDatime(offRes.GetYear(), 1, 1, 0, 0, 0)).Convert())/ (24.*3600.);

  for(iter = ctles.begin(); iter != ctles.end(); iter++) {
    cTle *tle = *iter;

    tledatefromfile = getTleJulian(tle);

    if(tledatefromroot > tledatefromfile) {
      tlefile.close();
      cTle *thisTle = tle;
      ctles.clear();

      return thisTle;
    }
  }

  // File ended withoud founding a TLE with a date after offRes.  We'll use the last aviable date.
  cerr << "Warning: using last available TLE in " << tleFile.Data() << ".  Consider updating your tle file." << endl;

  tlefile.close();
  cTle *thisTle = ctles[ctles.size()-1];
  ctles.clear();

  return thisTle; 
}


// Return whether the first TLE is older than the second
bool compTLE (cTle *tle1, cTle *tle2)
{
  return getTleJulian(tle1) > getTleJulian(tle2);
}


// Return the date of the tle using the format (year-2000)*1e3 +
// julian day.  e.g. 6364.5 is the 31th Dec 2006 12:00:00.
// It does *not* return a cJulian date.
float getTleJulian(cTle *tle) {
  return tle->getField(cTle::FLD_EPOCHYEAR)*1e3 + tle->getField(cTle::FLD_EPOCHDAY);
}


// Look for a timesync in the TFile rootFile.  Set timesync and
// obt_timesync.  Returns 1 if timesync is found, 0 otherwise.
UInt_t lookforTimesync(TFile *rootFile, Float_t *timesync, Float_t *obt_timesync) {
  *timesync = -1;  // will be != -1 if found

  ULong64_t        	nevents    = 0;
  pamela::McmdRecord     *mcmdrc     = 0;
  pamela::McmdEvent      *mcmdev     = 0;
  TArrayC                *mcmddata;
  TTree *tr = (TTree*) rootFile->Get("Mcmd");

  tr->SetBranchAddress("Mcmd", &mcmdev);

  nevents = tr->GetEntries();

  // Looking for a timesync.  We stop at the first one found.
  long int recEntries;

  for(UInt_t i = 0; i < nevents; i++) {
    tr->GetEntry(i);
    recEntries = mcmdev->Records->GetEntries();

    for(UInt_t j = 0; j < recEntries; j++) {
      mcmdrc = (pamela::McmdRecord*)mcmdev->Records->At(j);

      if ((mcmdrc != 0) && (mcmdrc->ID1 == 0xE0)) //Is it a TimeSync?
	{
	  mcmddata = mcmdrc->McmdData;
	  *timesync = (((unsigned int)mcmddata->At(0)<<24)&0xFF000000)
	    + (((unsigned int)mcmddata->At(1)<<16)&0x00FF0000) 
	    + (((unsigned int)mcmddata->At(2)<<8)&0x0000FF00) 
	    + (((unsigned int)mcmddata->At(3))&0x000000FF);
	  *obt_timesync = (mcmdrc->MCMD_RECORD_OBT)*(1./1000.);

	  goto out;  // a timesync is found
	}
    }
  }

 out:
	
  if (*timesync == -1)
    return 0;
  else
    return 1;
}


// Return a string like YYYY-MM-DD hh:mm:ss, a datetime format.
string getTleDatetime(cTle *tle)
{
  int year, mon, day, hh, mm, ss;
  double dom; // day of month (is double!)
  stringstream date; // date in datetime format

  // create a cJulian from the date in tle
  cJulian jdate = cJulian( 2000 + (int) tle->getField(cTle::FLD_EPOCHYEAR), tle->getField(cTle::FLD_EPOCHDAY)); 

  // get year, month, day of month
  jdate.getComponent(&year, &mon, &dom);

  // build a datetime YYYY-MM-DD hh:mm:ss
  date.str("");
  day = (int) floor(dom);
  hh = (int) floor( (dom - day) * 24);
  mm = (int) floor( ((dom - day) * 24 - hh) * 60);
  ss = (int) floor( ((((dom - day) * 24 - hh) * 60 - mm) * 60));
  //  ms = (int) floor( (((((dom - day) * 24 - hh) * 60 - mm) * 60) - ss) * 1000);

  date << year << "-" << mon << "-" << day << " " << hh << ":" << mm << ":" << ss;

  return date.str();
}

//
// Solve the overflow for anticoincidence because this counter is
// stored in 2 bytes so counts from 0 to 65535.
//
// counter is the actual value.
// oldValue is meant to be the previous value of counter.
//
// Example:
// for(...) {
//   ...
//   corrected_diff = solve_ac_overflow(oldValueOfTheCounter, actualValue);
//   ...
// }
//
//
// Returns the corrected difference between counter and oldValue and
// set oldValue to the value of counter.
// Attention: oldValue is a reference.
Int_t solve_ac_overflow(Int_t& oldValue, Int_t counter)
{
  Int_t truediff = 0;

  if (counter < oldValue)  // overflow!
    truediff = 0xFFFF - oldValue + counter;
  else
    truediff = counter - oldValue;

  oldValue = counter;

  return truediff;
}