#define hit_analyse_v2_cxx
#include "hit_analyse_v2.h"

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

  opendatafiles(argc, argv);
  histograms(argc, argv);
  analyse(argc, argv);
  closedatafiles();
  return 0;
}

int opendatafiles(int argc, char ** argv){
  if (argc>2){
    //open bpm data file
    filename = Form("%s%s.da2",argv[1],argv[2]);
    file.open(filename, ifstream::in | ifstream::binary);
    
    if (!file.is_open())
      {
	std::cerr << " ### Hitdata: File could not be opened!" << filename << std::endl;
	return 0;	//file could not be opened 
      }
    else {std::cout << filename << " opened successfully." << std::endl;}
  }
  string visualize_check = argv[5]; //plot data
  if (visualize_check == "vis_true") {visualize = true;}
  else{ visualize= false;}
  
  return 1;
}

int closedatafiles(){

  if (file.is_open()) file.close();
  // if (timestampfile.is_open()) timestampfile.close();
  //if (offsetfile.is_open())  offsetfile.close();

  rootFile->Write();
  rootFile->Close();
}


int analyse(int argc, char **argv)
{
  int first_frame = 0;  //  1440000    
 int nr_frames = -1;
 int increment = 1;

 

 
    //Read first record to find board configuration
    Fullframe sampleframe;
    if (sampleframe.read(&file) == 0)
      {
	std::cerr << " ### Hitdata: First frame could not be read!" << std::endl;
	file.close();
	return 0;
      }
    else {
      std::cout << "Sample frame size (bytes): " << sampleframe.sizeInFile() << std::endl;
    }
		
    //Check file size
    file.seekg(0, std::ios::beg);
    std::streamsize fsize = file.tellg();
    file.seekg(0, std::ios::end);
    fsize = file.tellg() - fsize;

    //Determine real frames to read
    unsigned int max_frames = fsize / sampleframe.sizeInFile();
    if ((max_frames == -1) || (max_frames < nr_frames))
      nr_frames = max_frames;

    std::cout << "     Hitdata: Nr frames to be read: " << nr_frames << std::endl;

    ///set the background levels from first N events
    int bkg_frames = 1000;
    if (set_background_v2(0, bkg_frames)==0) return 0;

    BPMbeamrecon_Zeroed.Position = -128.;
    BPMbeamrecon_Zeroed.Focus = -1.;
    BPMbeamrecon_Zeroed.Peak = -1.;
    BPMbeamrecon_Zeroed.Position = -128.;
    BPMbeamrecon_Zeroed.Rsqr = -1.;
    BPMbeamrecon_Zeroed.Skew = -128.;
    BPMbeamrecon_Zeroed.Position = -128.;
    BPMbeamrecon_Zeroed.Sum = 0.;
    BPMbeamrecon_Zeroed.n_channels = 0;

    //read board
   //Read!
    std::cout << "Reading data starting from frame: " << first_frame << std::endl;
    file.seekg(first_frame * sampleframe.sizeInFile(), std::ios::beg);
    for (int frame_nr = first_frame; frame_nr < nr_frames; frame_nr++)
      {
	eventID=frame_nr;
	if ((frame_nr%100000) == 0)
	  std::cout << "        Frame " << frame_nr << std::endl;

	file.seekg((frame_nr*increment) * sampleframe.sizeInFile() , std::ios::beg);
	if (sampleframe.read(&file) == 0) //read the next frame and catch if returns error
	  {
  	    std::cerr << " ### Hitdata: Frame " << frame_nr << " could not be read! Stopping." << std::endl;
	    file.close();		//read error, finish!
	    return 0;
	  }
	for (int boardnumber = 0; boardnumber<4; boardnumber++){
	  board_b[boardnumber] = readboard(sampleframe,boardnumber);//a bit redundant but does some analysis
	  // std::cout << board_b[0].integratedsignalamp << std::endl;
	  if (boardnumber==0&&board_b[0].integratedsignalamp>1000 && board_b[0].maxchannel_amp>100.){
	    BPMbeamrecon_0 = beamreconstruction(board_b[0], 80.); // do the linear regression fit of the beam;
	    //   std::cout << "doing regression" << std::endl;  
	  }
	  else if (boardnumber==0) {BPMbeamrecon_0=BPMbeamrecon_Zeroed;}
	  
	  if (boardnumber==1&&board_b[1].integratedsignalamp>1000 && board_b[1].maxchannel_amp>100.){
	    BPMbeamrecon_1 = beamreconstruction(board_b[1], 80.); // do the linear regression fit of the beam;
	    //   std::cout << "doing regression" << std::endl;  
	  }
	  else  if (boardnumber==1) {BPMbeamrecon_1=BPMbeamrecon_Zeroed;}
	  if (boardnumber==2&&board_b[2].integratedsignalamp>1000 && board_b[2].maxchannel_amp>100.){
	    BPMbeamrecon_2 = beamreconstruction(board_b[2], 80.); // do the linear regression fit of the beam;
	    //   std::cout << "doing regression" << std::endl;  
	  }
	  else  if (boardnumber==2) {BPMbeamrecon_2=BPMbeamrecon_Zeroed;}
	  if (boardnumber==3&&board_b[3].integratedsignalamp>1000 && board_b[3].maxchannel_amp>100.){
	    BPMbeamrecon_3 = beamreconstruction(board_b[3], 80.); // do the linear regression fit of the beam;
	    //   std::cout << "doing regression" << std::endl;  
	  }
	  else  if (boardnumber==3) {BPMbeamrecon_3=BPMbeamrecon_Zeroed;}
	  
	}
	for (int j = 0;j<320;j++){
	  if (board_b[0].maxchannel_amp>100.) TH2D_b0_signal_vs_channel->Fill(j, board_b[0].channel_amp[j]);
	  if (board_b[1].maxchannel_amp>100.) TH2D_b1_signal_vs_channel->Fill(j, board_b[1].channel_amp[j]);
	  if (board_b[2].maxchannel_amp>100.) TH2D_b2_signal_vs_channel->Fill(j, board_b[2].channel_amp[j]);
	  if (board_b[3].maxchannel_amp>100.) TH2D_b3_signal_vs_channel->Fill(j, board_b[3].channel_amp[j]);

	}
	rootTree->Fill(); 

      }
    

  return 1;
}

void histograms(int fargc, char ** argv){
    
  //open output root file
  rootfilename = Form("%s/root/%s.root",argv[1],argv[2]); 
  rootFile = new TFile(rootfilename,"RECREATE");
  if ( rootFile->IsOpen() ) {printf("ROOT file opened successfully\n");
  }
  else { printf("ROOT file failed to open. \n");}
  rootTree = new TTree("t","HIT Data Root Tree");
  rootTree ->Branch("BPMbeamrecon_0", &BPMbeamrecon_0, "Position/D:Focus:Peak:Rsqr:Skew:Kurtosis:Sum:n_channels/I");
  rootTree ->Branch("BPMbeamrecon_1", &BPMbeamrecon_1, "Position/D:Focus:Peak:Rsqr:Skew:Kurtosis:Sum:n_channels/I");
  rootTree ->Branch("BPMbeamrecon_2", &BPMbeamrecon_2, "Position/D:Focus:Peak:Rsqr:Skew:Kurtosis:Sum:n_channels/I");
  rootTree ->Branch("BPMbeamrecon_3", &BPMbeamrecon_3, "Position/D:Focus:Peak:Rsqr:Skew:Kurtosis:Sum:n_channels/I");
  
  rootTree ->Branch("eventID",&eventID,"eventID/I");

  TH2D_b0_signal_vs_channel = new TH2D("TH2D_b0_signal_vs_channel","TH2D_b0_signal_vs_channel",320,0,320,1200,-2000,20000); 
  TH2D_b1_signal_vs_channel = new TH2D("TH2D_b1_signal_vs_channel","TH2D_b1_signal_vs_channel",320,0,320,1200,-2000,20000); 
  TH2D_b2_signal_vs_channel = new TH2D("TH2D_b2_signal_vs_channel","TH2D_b2_signal_vs_channel",320,0,320,1200,-2000,20000); 
  TH2D_b3_signal_vs_channel = new TH2D("TH2D_b3_signal_vs_channel","TH2D_b3_signal_vs_channel",320,0,320,1200,-2000,20000); 

}


//Function for average
double avg ( vector<Channel> v )
{
        double return_value = 0.0;
        int n = v.size();
       
        for ( int i=0; i < n; i++)
        {
            return_value += v[i].chnumber;
        }
       
        return ( return_value / double(n));
}
//****************End of average funtion****************


//Function for variance
double variance ( vector<Channel> v , double mean )
{
        double sum = 0.0;
        double temp =0.0;
        double var =0.0;
       
        for ( int j =0; j < v.size(); j++)
        {
	  temp = pow((v[j].chnumber - mean) , 2);
            sum += temp;
        }
       
        return var = sum/double(v.size() -2);
}
//****************End of variance funtion****************


int set_background_v2(int start_frame, int max_frames){
  std::cout << "Setting background levels." << std::endl;
  for (int j = 0; j<320; j++){
    for (int k = 0; k<4; k++){
      board_b_bkg[k].channel_amp[j] = 0.;
    }
  }  
   
  //Read first record to find board configuration
  Fullframe sampleframe;
 
  //Read
  // file.seekg(sampleframe.sizeInFile(), std::ios::beg);
  for (int frame_nr = start_frame; frame_nr < max_frames; frame_nr++)
    {
      file.seekg(frame_nr * sampleframe.sizeInFile() , std::ios::beg);
      if (sampleframe.read(&file) == 0) //read the next frame and catch if returns error
	{
	  std::cerr << " ### Hitdata: Frame " << frame_nr << " could not be read!" << std::endl;
	  file.close();		//read error, finish!
	  return 0;
	}
      for (int boardnumber = 0; boardnumber<4; boardnumber++){
	for (int j = 0; j<sampleframe.boards[boardnumber].nrChannels;j++){
	  board_b_bkg[boardnumber].channel_amp[j] += sampleframe.boards[boardnumber].data[j] / double(max_frames);
	  //  std::cout <<  j << " " << board.channel_amp[j] << "    "  << dataptr->sensor_data[j] << std::endl;
	}
      }
    }

   std::cout << "Background set." << std::endl;
  return 1;
}

bpm_frame_v2 readboard(Fullframe frame, int boardnumber){

  bpm_frame_v2 board;
  
  board.integratedsignalamp = 0.;
  board.maxchannel_amp = 0.;
  //  file.seekg(boardnumber*sizeof(BufferData)+4*frame*sizeof(BufferData));
  //file.read ((char*)dataptr ,sizeof(BufferData));

  if (frame.boards[boardnumber].syncframe.device_nr==boardnumber){
    for (int j = 0; j<frame.boards[boardnumber].nrChannels;j++){
      //subtract the background from the data
      if (boardnumber==0){
      // board_0 has the even and odd channels swapped in the hardware.
	if(j%2==0){
	  board.channel_amp[j] = frame.boards[boardnumber].data[j+1] -  board_b_bkg[boardnumber].channel_amp[j+1];
	}
	else{
	  board.channel_amp[j] = frame.boards[boardnumber].data[j-1] -  board_b_bkg[boardnumber].channel_amp[j-1];
	}
      }
      else{
	board.channel_amp[j] = frame.boards[boardnumber].data[j] -  board_b_bkg[boardnumber].channel_amp[j];
	//   std::cout <<  j << " " << board.channel_amp[j] << "    "  << frame.boards[boardnumber].data[j] << std::endl;
      }



      //sum the signal across channels
      board.integratedsignalamp +=  board.channel_amp[j]; 

      //find the peak channel
      if (board.channel_amp[j]> board.maxchannel_amp) {
	board.maxchannel = j;
	board.maxchannel_amp = board.channel_amp[j];
	//	cout << maxchannel_b0 << " " <<maxchannelamp_b0 << endl;
      }

      //set the channel positions in mm
      board.channel_position[j] = 0.8*j + 0.2*(floor(j/64));
      //cout << board.channel_position[j] << " " << j << " " << (floor(j/64)) << endl;
    }
  } 
  else std::cerr << "Error reading board data." << std::endl;
   
  

    
  return board;
}


beamRecon beamreconstruction(bpm_frame_v2 frametoanalyse, double threshold = 30.){

  ///////////////// linear regression using Integration by parts of gaussian function.

  beamRecon beam;
  double SumT, SumS, SumS2, SumST, SumT2, SumY, SumYS, SumYT, sigmaABC, muABC,p,c, b, b_den, b_num, SumYYP, SumYYM, MeanY;
  TMatrixD M1(3,3);
  TMatrixD M1inv(3,3);
  TVectorD ABC(3);
  TVectorD M2(3);
  vector<double> signal_list;
  vector<double> channel_list;

  SumY = 0.;
  SumS = 0.;
  SumT = 0.;
  SumS2 = 0.;
  SumST = 0.;
  SumT2 = 0.;
  SumYS = 0.;
  SumYT = 0.;
  b_den = 0.;
  b_num = 0.;
  b = 0.;
  p = 0.;
  c = 0.;
  SumYYM = 0.;
  SumYYP = 0.;
  MeanY = 0.;

  
  //  const int array_length = sizeof(frametoanalyse.channel_amp)/sizeof(double);
  const int array_length = 320;
  vector<Channel> channel_reducedlist; //for anomaly detection
  vector<Channel> channel_reducedlistcopy; //for anomaly detection

  Channel tmp;
  
  int temp_lastneighbour= -128;
  for (int i = 0; i< array_length; i++){
    if (frametoanalyse.channel_amp[i]>=threshold) {
      //  cout << "ch: " << i << endl;
      //    signal_list.push_back(frametoanalyse.channel_amp[i]);
      //    channel_list.push_back(frametoanalyse.channel_position[i]);
      tmp.amplitude = frametoanalyse.channel_amp[i];
      tmp.position = frametoanalyse.channel_position[i];
      tmp.chnumber = i;
      tmp.last_neighbour = temp_lastneighbour; 
      temp_lastneighbour = i ;
      channel_reducedlist.push_back(tmp);
      if (channel_reducedlist.size()>1){
      	channel_reducedlist[channel_reducedlist.size() - 2].next_neighbour = i;
      }

    }
  }
  
  //anomaly detection
  //remove channels without neighbours. 
  for (int i = 0; i<channel_reducedlist.size() ;i++){

    if (abs(channel_reducedlist[i].chnumber - channel_reducedlist[i].last_neighbour)<=1 || abs(channel_reducedlist[i].chnumber-channel_reducedlist[i].next_neighbour)<=1  ){
      channel_reducedlistcopy.push_back(channel_reducedlist[i]);
      //    cout << channel_reducedlist[i].chnumber << " " << channel_reducedlist[i].last_neighbour << " " << channel_reducedlist[i].next_neighbour << endl;
    }
  }
  //  channel_reducedlist.clear();//empty list to reuse it.   
  double cluster_average;
  double cluster_variance;
  if(channel_reducedlistcopy.size()>2){
    cluster_average = avg(channel_reducedlistcopy);
    cluster_variance = variance(channel_reducedlistcopy, cluster_average);
    //   cout << cluster_average << " " << cluster_variance << endl;
  }

  //include all channels +/- 2*variance of the main cluster
    for (int i = 0; i< array_length; i++){
      if (abs(i-cluster_average)<2*cluster_variance){
	signal_list.push_back(frametoanalyse.channel_amp[i]);
	channel_list.push_back(frametoanalyse.channel_position[i]);
      }
    }
    //  sort(channel_reducedlist.begin(),channel_reducedlist.end(),CompareChannels);
    
   

  
  const int vector_length = channel_list.size();
  beam.n_channels = vector_length;
  beam.Sum = std::accumulate(signal_list.begin(), signal_list.end(),0);
  if (vector_length<=3) return beam;
  
  double S[vector_length];
  double T[vector_length];
  
  

  
  for(int k=0; k<vector_length;k++){
    if (k==0){
      S[k]=0.;		T[k]=0.;
    }
    else{
      S[k] = S[k-1]+0.5*( signal_list[k] + signal_list[k-1] ) *  ( channel_list[k] - channel_list[k-1] );
      T[k] = T[k-1]+0.5*( channel_list[k] * signal_list[k] + channel_list[k-1] * signal_list[k-1] ) * ( channel_list[k] - channel_list[k-1] );
    }
    //  cout << S[k] << " " << T[k] << endl;
    SumS += S[k];	      SumT += T[k];
    SumY += signal_list[k];
    SumS2 += S[k]*S[k]; 	      SumST += S[k]*T[k];	      SumT2 += T[k]*T[k];
    SumYS += signal_list[k]*S[k];
    SumYT += signal_list[k]*T[k];
    MeanY+=signal_list[k];
  }
  MeanY/=vector_length;

	      
  M1(0,0) = SumT2;	      M1(0,1) = SumST;	      M1(0,2) = SumT;	      M1(1,0) = SumST;	      M1(1,1) = SumS2;
  M1(1,2) = SumS;	      M1(2,0) = SumT;	      M1(2,1) = SumS;
  M1(2,2) = vector_length;
  
  M2(0) = SumYT;	      M2(1) = SumYS;	      M2(2) = SumY;
  M1inv = M1.Invert();	      ABC = M1inv * M2;

	     

	      
  //calculate b,p,c ---> y = b*exp(-p*(x-c)*(x-c))
  p = -ABC(0)/2.;	     c = -ABC(1)/ABC(0);

  for(int k=0; k<vector_length;k++){
    b_num += exp(-p*(channel_list[k]-c)*(channel_list[k]-c)) * signal_list[k];
    b_den +=  exp(-2*p*(channel_list[k]-c)*(channel_list[k]-c));
		
  }
  b = b_num/b_den;

  beam.Position = -ABC(1)/ ABC(0);
  beam.Focus = 2.3548/sqrt(2*p);
  beam.Peak = b;
  beam.Rsqr = SumYYP/SumYYM;
  beam.Skew = gsl_stats_wskew_m_sd(&signal_list[0],1,&channel_list[0],1,vector_length,beam.Position,beam.Focus/2.3548); //skewness (symmetry)
  beam.Kurtosis = gsl_stats_wkurtosis_m_sd(&signal_list[0],1,&channel_list[0],1,vector_length,beam.Position,beam.Focus/2.3548); //excess kurtosis (well behaved tails)
  
  return beam;
  
}