#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();
  return 1;
}


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

 ///timestamp finding variables

 float fs = 10000; //10kHz fibre bpm
 float threshold = 0.5;
  // fs = 20kHz for ethercat IC,  threshold = 2500
 // fs = 10kHz for fBPM,  threshold = 0.5
  //Decodes timestamp data from synchro stream.
  //fs is frame rate in Hz.
  //threshold is the threshold value. Use e.g. 0.5 for boolean data
  //Timestamper bitrate is assumed to be 250 bps, 4 transmissions/s

  int current_pos_in_samples = 0;
  int samples_per_50ms = floor(50e-3 * fs);
  int samples_per_one_transmission = floor(250e-3 * fs);
  int samples_per_bit = floor(4e-3 * fs);
 
  
  string outfilename;
  outfilename+=argv[1];
  outfilename+="root/timestamp/";
  outfilename+=argv[2];
  outfilename+="_timestamp.txt";
  outfile.open(outfilename);
  if (outfile.good()) {cout << outfilename << " opened successfully." << endl;}
  else {cout << outfilename << " opening failed." << endl;}

  cout << "samples_per_50ms: " << samples_per_50ms  << endl;
  cout << "samples_per_one_transmission :" << samples_per_one_transmission  << endl;
  cout << "samples_per_bit: "<< samples_per_bit  << endl;

  //%this is used for byte decoding
  int bit_multipliers[10] = {0, 1, 2, 4, 8, 16, 32, 64, 128, 0}; //1+byte+1
    //  %and this for word decoding
   int byte_multipliers[4] = {1, 256, 65536, 16777216};

  //%Here are positions of each transmission and decoded values
  vector<int> block_positions;
  vector<float> block_data;
  vector<float> byte_data;
  vector<float> bit_data;
  vector<int> bit_pattern_to_test;
  vector<float> sample_buffer;
  sample_buffer.assign(floor(4*10*samples_per_bit+samples_per_50ms),0);

  float byte_value;
  int block_nr = 0;
  float block_value;
  int nbelowthreshold = 0;
  int nabovethreshold = 0;
  int last_nbelowthreshold = 0;
  int last_nabovethreshold = 0;
  float intime;
  float analog_in2;
  float last_analog_in2 = 0;
  int transmission_start_in_samples = 0;
 

 
    //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;}
	  
	}
	//	cout << "fill hist " << int(board_b[0].nrChannels) << endl;

	for (int j = 0;j< board_b[0].nrChannels;j++){
	  if (board_b[0].maxchannel_amp>100.) TH2D_b0_signal_vs_channel->Fill(j, board_b[0].channel_amp[j]);
	}
	for (int j = 0;j< board_b[1].nrChannels;j++){
	  if (board_b[1].maxchannel_amp>100.) TH2D_b1_signal_vs_channel->Fill(j, board_b[1].channel_amp[j]);
	}
	for (int j = 0;j< board_b[2].nrChannels;j++){
	  if (board_b[2].maxchannel_amp>100.) TH2D_b2_signal_vs_channel->Fill(j, board_b[2].channel_amp[j]);
	}
	for (int j = 0;j< board_b[3].nrChannels;j++){
	  if (board_b[3].maxchannel_amp>100.) TH2D_b3_signal_vs_channel->Fill(j, board_b[3].channel_amp[j]);
	}


	///find timestamps in sma_state data for the BPM
	analog_in2 = board_b[3].sma_state;

      // cout << current_pos_in_samples << ":  " << intime << " " << analog_in2 << " ";
      current_pos_in_samples = eventID ;
      sample_buffer.erase(sample_buffer.begin());//remove the first bit in the sample_buffer, and then add the new value at the end.
      sample_buffer.push_back(analog_in2);
      last_nbelowthreshold = nbelowthreshold;
      last_nabovethreshold = nabovethreshold;
      //look for 50 ms gap between transmissions (1000 * 0.05ms frames in ethercat IC) then process the sample buffer
      if (analog_in2<threshold) {
	nbelowthreshold++;
	nabovethreshold=0;
      }
      else {
	nbelowthreshold=0; //reset search window counter if you go above threshold again without reaching the 50ms gap
	nabovethreshold++;
      }
      // cout <<"   below: " << nbelowthreshold << "   above: " << nabovethreshold << endl;
      //    if (last_nbelowthreshold>nbelowthreshold) cout << "last below "  << last_nbelowthreshold << endl;
      //  if (last_nabovethreshold>nabovethreshold) cout << "last above " << last_nabovethreshold << endl;
      if (nabovethreshold>=samples_per_50ms+samples_per_bit){ // last stop bit + 50ms above thresholds
	nabovethreshold=0; //reset search window counter
	transmission_start_in_samples = current_pos_in_samples - samples_per_50ms - 4*10 * samples_per_bit; 
	//%if the gap was found, proceed to finding start and stop bits in sample buffer...
	//	cout << transmission_start_in_samples << ": sample buffer[" << sample_buffer.size() << "]" << endl; 
	//	for (auto i: sample_buffer) std::cout << i << " "; 
	//	cout  << endl;
        //    %initialize byte counter
	//fill with the middle values where the bits should be
	byte_data.clear();
	bit_pattern_to_test.clear();
	cout << transmission_start_in_samples << ": bit pattern[40] "; 
	int count=0;
	for (int byte_nr =0; byte_nr<4;  byte_nr++){ 
	  for (int bit_nr =0; bit_nr<10;  bit_nr++){ 
	    bit_pattern_to_test.push_back( sample_buffer[ floor((bit_nr + byte_nr*10 + 0.5) *samples_per_bit) ]  >=threshold ? 1 : 0); //1 if greater than threshold else 0
	    
	    //     std::cout << bit_pattern_to_test[count] << " "; 
	    count++;
	  } //10 bits filled
	  //   cout  << endl;
	} //4 bytes filled

	//check that the stop and start bits are in the correct place
	if (bit_pattern_to_test[0]==0 && bit_pattern_to_test[9]==1 &&
	    bit_pattern_to_test[10]==0 && bit_pattern_to_test[19]==1 &&
	    bit_pattern_to_test[20]==0 && bit_pattern_to_test[29]==1 &&
	    bit_pattern_to_test[30]==0 && bit_pattern_to_test[39]==1){
	  //then
	  cout << "Timestamp found at entry " <<  transmission_start_in_samples << endl;
	  //fill byte

	  for (int jbyte_nr =0; jbyte_nr<4;  jbyte_nr++){ 
	    byte_value = 0;
	    for (int jbit = 1; jbit< 9; jbit++){
	      byte_value+=bit_pattern_to_test[jbit+ 10*jbyte_nr] * bit_multipliers[jbit];
	      //            cout << bit_pattern_to_test[jbit+ 10*jbyte_nr] << "*" << bit_multipliers[jbit] << "+";
	    }
	    //   cout << "= "<< byte_value << " " << endl;;
	    
	    byte_data.push_back(byte_value);
	  }
	  // cout << endl;
	  // %now calculate the result
	  block_nr++;
	  block_positions.push_back(transmission_start_in_samples);
	  block_value=0;
	  for (int jbyte = 0; jbyte< 4; jbyte++){
	    block_value+=byte_data[jbyte]*byte_multipliers[jbyte];
	  }
	  block_data.push_back(block_value);
	  if ( block_nr>1) {printf("Block %i at %1.1i: %f ms  Delta=%f ms\n", block_nr, block_positions[block_nr-1], block_data[block_nr-1],block_data[block_nr-1]- block_data[block_nr-2]);}
	  else { printf("Block %i at %1.1i: %f ms\n", block_nr, block_positions[block_nr-1], block_data[block_nr-1]); }

	  outfile << std::setprecision(11) << block_data[block_nr-1] << " " << block_positions[block_nr-1] << endl;
	}
	else{
	  cout << "Bad Block at  entry " <<  transmission_start_in_samples << endl;
	}
	//no matter what , clear the sample buffer and try again. 
	sample_buffer.clear();
	sample_buffer.assign(floor(4*10*samples_per_bit+samples_per_50ms),0);
      }//end of beloww threshold for 50ms










	//	cout << "fill tree" << endl;
	rootTree->Fill(); 

      } //end of frame loop
    
    outfile.close();
  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("board_0", &board_b[0], "channel_amp[320]/D:channel_position[320]:avg_position:avg_width:integratedsignalamp:max_channel_amp:maxchannel/I:nrChannels:board_number:sma_state");
  //rootTree ->Branch("board_1", &board_b[1], "channel_amp[320]/D:channel_position[320]:avg_position:avg_width:integratedsignalamp:max_channel_amp:maxchannel/I:nrChannels:board_number:sma_state");
  //rootTree ->Branch("board_2", &board_b[2], "channel_amp[320]/D:channel_position[320]:avg_position:avg_width:integratedsignalamp:max_channel_amp:maxchannel/I:nrChannels:board_number:sma_state");
  //rootTree ->Branch("board_3", &board_b[3], "channel_amp[320]/D:channel_position[320]:avg_position:avg_width:integratedsignalamp:max_channel_amp:maxchannel/I:nrChannels:board_number:sma_state");

  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;
  file.seekg(start_frame * sampleframe.sizeInFile() , std::ios::beg);
  if (sampleframe.read(&file) == 0) //read the next frame and catch if returns error
    {
      std::cerr << " ### Hitdata: First frame could not be read!" << std::endl;
      file.close();		//read error, finish!
      return 0;
    }
 
 
  //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.;
  board.nrChannels = frame.boards[boardnumber].nrChannels;
  board.board_number = boardnumber;
  board.sma_state = frame.boards[boardnumber].syncframe.sma_state;
  //  file.seekg(boardnumber*sizeof(BufferData)+4*frame*sizeof(BufferData));
  //file.read ((char*)dataptr ,sizeof(BufferData));

  if (frame.boards[boardnumber].syncframe.device_nr==boardnumber){
    
    // cout << "nrChannels" <<  frame.boards[boardnumber].nrChannels << endl;
    for (int j = 0; j<frame.boards[boardnumber].nrChannels;j++){
      //subtract the background from the data
      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;
  channel_list.clear();
  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 = frametoanalyse.nrChannels ;
  vector<Channel> channel_reducedlist; //for anomaly detection
  vector<Channel> channel_reducedlistcopy; //for anomaly detection

  //hardcoded pixels to mask for this data set.
  const int arr_0[] = {139 };
  //const  int arr_1[] = {};
  const int arr_2[] = { 11,12 };
  const int arr_3[] = { 1, 2, };

  vector<int> masked_channels[4];
  masked_channels[0].assign( arr_0, arr_0 + sizeof(arr_0) / sizeof(*arr_0) );
  //  masked_channels[0].assign( arr_0, arr_0 + sizeof(arr_0) / sizeof(*arr_0) );
  masked_channels[2].assign( arr_2, arr_2 + sizeof(arr_2) / sizeof(*arr_2) );
  masked_channels[3].assign( arr_3, arr_3 + sizeof(arr_3) / sizeof(*arr_3) );

 
  
  
  Channel tmp;
  
  int temp_lastneighbour= -128;
  for (int i = 0; i< array_length; i++){
    if (count( masked_channels[frametoanalyse.board_number].begin(), masked_channels[frametoanalyse.board_number].end(), i)) continue; //check masked pixel list, and do not add it to the list of channels for analysis. 
    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;
  
  for(int k=0; k<vector_length;k++){ 
    SumYYM+= (signal_list[k]-MeanY)*(signal_list[k]-MeanY);
    SumYYP+= (b*exp(-p*(channel_list[k]-c)*(channel_list[k]-c)) - MeanY )*(b*exp(-p*(channel_list[k]-c)*( channel_list[k]-c)) - MeanY );
  }
  //   cout << "R-squared = " << SumYYP/SumYYM << endl;

  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;
  
}
