//------------------------------------------------------------------------------
// Application task example																	--
// (C) Piero Giubilato 2008-2010, Berkeley Lab										   --
//------------------------------------------------------------------------------

//______________________________________________________________________________
// {Trace}
// [File name] "task_TEAM_Main.cpp"
// [Author] "Piero Giubilato"
// [Version] "0.5"
// [Modified by] "Piero Giubilato"
// [Last revision] "27 Jul 2009"
// [Language] "C++"
// [Compiler] "Visual C++ 8.x 9.x"
// [Member of] "Cool SEAL"
// [Project] "SEAL"
// [Description] "TEAM imaging task"
// [Key documentation]
// "Visual C++ Reference Help"
// {Trace}
//______________________________________________________________________________

// Standard components
//#include <time.h>

// Root Components
#include <TSystem.h>
#include <TVirtualFFT.h>
#include <TFile.h>

// Application components
#include "global.h"
#include "cool.h"
#include "data_Cluster.h"
#include "task_TEAM_Main.h"


// *****************************************************************************
// **										 Initialization										**
// *****************************************************************************

//______________________________________________________________________________
task_TEAM_Main::task_TEAM_Main()
{
   // Standard constructor
	dbg_Print("task_TEAM _Main::task_TEAM_Main:()", DBG_LVL_ZERO);

	// Init
	Init();
}

//______________________________________________________________________________
task_TEAM_Main::~task_TEAM_Main()
{
   // Standard destructor
	dbg_Print("task_TEAM_Main::~task_TEAM_Main:()", DBG_LVL_ZERO);

	// Removes the GUI
	delete task_GUI;
	
	// Delete the detector object
	delete task_Det;
}

//______________________________________________________________________________
void task_TEAM_Main::Init()
{
   // Debug
	dbg_Print("task_TEAM_Main::init:()", DBG_LVL_MAKE);
   
	// Initialize the detector description
	task_Det = new task_TEAM_Detector();

	// Initialize the GUI
	task_GUI = new task_TEAM_GUI(this);

	// Status flags
	sts_dark_Ok = false;

	// Reset data structures
	db_Raw = 0;
	df_Dark = 0;
	df_Image = 0;
	df_CImage = 0;
	df_FImage = 0;
	db_Movie = 0;
}


// *****************************************************************************
// **										 GUI events slots										**
// *****************************************************************************

//______________________________________________________________________________
void task_TEAM_Main::evn_btn_Main(const int arg)
{
   // Event for the main buttons
	dbg_Print("task_TEAM_Main::evn_btn_Main:(const int)", DBG_LVL_MAKE);
   
	// Choose
	if (arg == task_GUI->btn_Main[0]->obj_Id()) comm_Open();
	if (arg == task_GUI->btn_Main[1]->obj_Id()) acq_Dark();
}

//______________________________________________________________________________
void task_TEAM_Main::evn_btn_Acq(const int arg)
{
   // Event for the acquisition buttons
	dbg_Print("task_TEAM_Main::evn_btn_Acq:(const int)", DBG_LVL_MAKE);
   
	// Play
	gui_CoolButton* btn = task_GUI->btn_Acq[0];
	if (arg == btn->obj_Id()) {
		if (task_GUI->btn_Acq[0]->btn_Status()) {
			btn->Text("Stop");
			btn->backColor(gui_CoolButton::kc_Red);
			acq_Main();
		} else {
			btn->Text("Start");
			btn->backColor(gui_CoolButton::kc_Green);
		}
	}
}


// *****************************************************************************
// **										  Communication										**
// *****************************************************************************

//______________________________________________________________________________
void task_TEAM_Main::comm_Open()
{
   // Opens the communication with the FPGA board
	dbg_Print("task_TEAM_Main::comm_Open:()", DBG_LVL_MAKE);
   
	// Try opening a communication, sending aclear in case
	// of first attempt failure
	cool::Comm->Open();
	if (!cool::Comm->isOpen()) {
		cool::Comm->Clear();
		cool::Comm->Open();
	}

	// Check and lite the led in case
	if (cool::Comm->isOpen()) task_GUI->led_Main[0]->Status(gui_Led::ks_On);
	else task_GUI->led_Main[0]->Status(gui_Led::ks_Off);
}


// *****************************************************************************
// **								      Data & FPGA Setup 									**
// *****************************************************************************

//______________________________________________________________________________
void task_TEAM_Main::data_Setup(UInt_t fCount)
{
	// Defines all the data structures required 
	dbg_Print("task_TEAM_Main::data_Setup:()", DBG_LVL_MAKE);
   
	// Retrives the detector definition
	geom_Frame* det = task_Det->gf_TEAM; 

	// Fits data structures to the detector and expeted frame count
	if (db_Raw) delete db_Raw;			db_Raw = new data_Burst<UShort_t>(det, fCount);
	if (df_Dark) delete df_Dark;		df_Dark = new data_Frame<Float_t>(det);
	if (df_Image) delete df_Image;	df_Image = new data_Frame<Double_t>(det);
	if (df_CImage) delete df_CImage;	df_CImage = new data_Frame<Float_t>(det);
	if (df_FImage) delete df_FImage;	df_FImage = new data_Frame<Double_t>(det);
	if (db_Movie) delete db_Movie;	db_Movie = new data_Burst<Float_t>(det, fCount);

	// Clears the led indicating data status
	task_GUI->led_Main[1]->Status(gui_Led::ks_Off);	// Dark image

	// Sets the map
	task_GUI->map_Data->data_Set(db_Raw->frame(0)->layer(0));
	task_GUI->map_Data->data_Refresh(0, 0, db_Raw->frame(0)->layer(0)->dp_col_Count(), db_Raw->frame(0)->layer(0)->dp_row_Count());
	task_GUI->map_Elab->data_Set(db_Raw->frame(0)->layer(0));
	task_GUI->map_Elab->data_Refresh(0, 0, db_Raw->frame(0)->layer(0)->dp_col_Count(), db_Raw->frame(0)->layer(0)->dp_row_Count());
		
	// Info
	cool::Out->Print("Total allocated data: ", WHITE);
	cool::Out->Print(data_Object::stc_data_Size() / 1024, LWHITE, true);
}


//______________________________________________________________________________
void task_TEAM_Main::fpga_Setup()
{
	// Matches the FPGA to the selected data
	dbg_Print("task_TEAM_Main::fpga_Setup:()", DBG_LVL_MAKE);
   
	// Check comm status
	if (!cool::Comm->isOpen()) {	
		cool::Out->Print("You need to open communication to set FPGA!\n", LYELLOW);
		return;
	}
	
	// Reset the FPGA
	cool::Comm->Reset();

	// Set the clock
	cool::Reg->adc_clk_Div(4);				// Clock divider, = 3.12 Mhz
	
	// Sets the acquisition mode and driver
	cool::Reg->acq_Mode(task_GUI->cmb_Acq[0]->cbox_Val());	// Acq(trigger) mode
	cool::Reg->acq_drv_Delay(0);										// No driving delay
	cool::Reg->acq_drv_Sel(db_Raw->frame_Fit()->HrdID());		// TEAM driver
	
	// Sets data pipeline
	cool::Reg->acq_data_Mode(4);		// TEAM data source;
	cool::Reg->acq_chn_Enable(15);	// 4 channel acquisition
	
	// Sets data count for the packet builder
	cool::Reg->acq_frm_Count(db_Raw->frame_Count());			// Frame data count
	cool::Reg->acq_dp_Count(db_Raw->frame_Fit()->chip(0)->dp_Count());	// Total data count (per channel!)
	
	// Sets data triggering mode
	cool::Reg->acq_trg_Mode(0);			// Internal (software this case )trigger
	cool::Reg->acq_trg_Period(10000);	// 1 Hz hrd trigger period	
}

//______________________________________________________________________________
void task_TEAM_Main::team_Setup()
{
	// Preset the data
	dbg_Print("task_TEAM_Main::team_Setup:()", DBG_LVL_FLOW);
	
	// Check comm status
	if (!cool::Comm->isOpen()) {	
		cool::Out->Print("You need to open communication to set TEAM!\n", LYELLOW);
		return;
	}
	
	// Set up a pivot buffer
	Int_t buff_Count = 22;
	Int_t buff_Len = 16;
	UShort_t wr_Buff[22][16] = {
		
		// Sets DataNConfig to config mode
		{5, 0, 0, 0x8802, 0x0000, 0x0000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},

		// Sets clock
		{5, 0, 0, 0x87c2, 0x0000, 0x0002, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x87c2, 0x0000, 0x0003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		
		// Sets register DAC values
		{5, 0, 0, 0x8405, 0x11E4, 0x1AB8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x8406, 0x0000, 0x177C, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x8407, 0x1AB8, 0x0000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	
		// Sets digitizer to send clocks to the sensor and update the DAC values.
		{5, 0, 0, 0x8002, 0x000A, 0x8031, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x8402, 0x0012, 0x8002, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x8402, 0x0012, 0xA002, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x8402, 0x0012, 0x8002, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	
		// Selects real data from digitizers
		{5, 0, 0, 0x8004, 0x0000, 0x0000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x8404, 0x0000, 0x0000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	
		// Sets image size (64 columns per output and 1024 rows, the full sector)
		{5, 0, 0, 0x800c, 0x0140, 0x0400, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x840c, 0x0140, 0x0400, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	
		// Sets APS clock speed
		{5, 0, 0, 0x800d, 0x0000, 0x0010, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x800d, 0x0000, 0x0018, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x800d, 0x0000, 0x0013, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x840d, 0x0000, 0x0003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},

		// Sets delays from qwrap and C0
		{5, 0, 0, 0x800e, 0x0000, 0x0340, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{5, 0, 0, 0x840e, 0x0000, 0x0340, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},

		// Sets image size on V4 (here it is 1024 * 1024)
		{5, 0, 0, 0x8803, 0x0400, 0x0400, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},

		// Sets DataNConfig
		{5, 0, 0, 0x8802, 0x0000, 0x0045, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
	};


	// Sends the buffer to the FPGA TEAM module
	for (UInt_t i = 0; i < buff_Count; i++) cool::Comm->raw_Write(wr_Buff[i], &buff_Len);

	// Info
	cool::Out->Print("TEAM mode set\n", LGREEN);
}


// *****************************************************************************
// **										    Acquisition 										**
// *****************************************************************************

//______________________________________________________________________________
void task_TEAM_Main::acq_Dark(UInt_t fCount)
{	
	// Gets the dark frame
	dbg_Print("task_TEAM_Main::acq_Pedestal:(UInt)", DBG_LVL_MAKE);
   
	// Sets up the data
	data_Setup(1);
	fpga_Setup();
	team_Setup();
	UInt_t i = 0;

	// Check for connection
	if (!cool::Comm->isOpen()) return;

	// Main acquisition loop
	for (i = 0 ; i < fCount; i++) {
		
		// Acquires one shot!
		if (acq_Shot(ka_Dark) == cool::Comm->kbr_Free) {
			if (!db_Raw->frame(0)->layer(0)->dp_Value(0)) {
				i--;
				continue;
			} else {
				*df_Dark += *(db_Raw->frame(0));
			}
		} else {
			break;
		}
	}

	// Averages
	if (i == fCount) {
		*df_Dark /= fCount;
		task_GUI->led_Main[1]->Status(gui_Led::ks_On);
		sts_dark_Ok = true;
	} else {
		*df_Dark = 0;
		task_GUI->led_Main[1]->Status(gui_Led::ks_Off);
		sts_dark_Ok = false;
	}
}

//______________________________________________________________________________
void task_TEAM_Main::acq_Main()
{
	// Main acquisition loop
	dbg_Print("task_TEAM_Main::acq_Main:()", DBG_LVL_MAKE);
   
	// Grabs the number of required bursts
	Int_t db_Count = 0;
	Int_t db_Inc = 1;

	// Check the acquisition mode (Single, Continuous, Countdown)
	UInt_t acq_Style = (UInt_t)(task_GUI->cmb_Acq[0]->cbox_Val());

	// Acquisition
	if (acq_Style == 0)			{db_Count = 1; db_Inc = 1;}	// SS
	else if  (acq_Style == 1)	{db_Count = 1; db_Inc = 0;}	// Continuous	
	else {db_Count = (UInt_t)(task_GUI->num_Acq[0]->num_Val()); db_Inc = 1;} // Countdown
		
	// Sets the data and th FPGA
	UInt_t df_Count = (UInt_t)(task_GUI->num_Acq[1]->num_Val());  
	if (!sts_dark_Ok) {
		data_Setup(1);
		fpga_Setup();
		//team_Setup();
	}
	
	// Main acquisition loop
	for (UInt_t i = 0 ; i < db_Count; i+= db_Inc) {
		
		// Checks the button status
		if (!task_GUI->btn_Acq[0]->btn_Status()) break;
		
		// Acquires one shot!
		if (acq_Shot(0) == cool::Comm->kbr_Free) {

			// Check for bad events
			//if (!db_Raw->frame(0)->layer(0)->dp_Value(0)) {
			//	cool::Out->Print("Broken frame\n", YELLOW);
			//	continue;
			//}

			// *** Fake clusters
			//db_Raw->frame(0)->layer(0)->dp_all_Populate(100, 20, 3);
	
			// *** Fake lattice
			//db_Raw->frame(0)->layer(0)->dp_all_Lattice(15, 50);
				
			// *** Fake Sin
			//db_Raw->frame(0)->layer(0)->dp_all_Wave(0.05, 0);

			// Move the UShort raw data to a more comfortable space
			*df_Image = *(db_Raw->frame(0));
				
			// Pedestal subtraction in case
			//if (acq_Mode == ka_DarkSub) *df_Image -= *df_Dark;	
			*df_Image -= *df_Dark;	

			// Refresh the map
			task_GUI->map_Data->data_Set(df_Image->layer(0)); 
			task_GUI->map_Data->data_Refresh();

			// Clustering
			//std::cout << "Clustering:\n";
			data_Stack<UShort_t>* stack;
			stack = db_Raw->frame(0)->layer(0)->dp_all_Clusterize(20, 20);
			stack->List();
			stack->cut_set_Mult(20, 2000);
			stack->cut_set_Width(1, 5);
			stack->cut_set_Height(1, 5);
			stack->cut_set_Top(20, 2000);
			stack->cut_set_Mass(100, 60000);
			stack->cut_Apply();

			//TFile f("StackTest.root","RECREATE");
			//stack->Write("Teststack");
			//f.Close();
			stack->List();
			delete stack;

			// Read clustering	
			//data_Stack<UShort_t>* rStack = new data_Stack<UShort_t>();
			//TFile r("StackTest.root", "READ");
			//rStack->Read("Teststack");
			//r.Close();
			//rStack->List();
			//delete rStack;

			// FFT
			//acq_FFT();
		}

		// Anyway, allow the g-system to refresh, and to the user to interact
		gSystem->ProcessEvents();
		Sleep(50);
	}

	// Reset the button
	task_GUI->btn_Acq[0]->btn_Status(false);  
}

//______________________________________________________________________________
UInt_t task_TEAM_Main::acq_Shot(UInt_t acq_Mode)
{
	// Single shot call
	dbg_Print("task_TEAM_Main::acq_Shot:()", DBG_LVL_MAKE);
   
	// This is for the TEAM control module (5 is the TEAM ID in the USB chain)
	// It triggers the start bit to the gretina controller
	UShort_t wr_Buff[16] = {5, 0, 0, 0x8801, 0x000, 0x0001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	Int_t wr_Len = 16;

	// Launch the read thread, the 'false' indicating a do not wait mode
	cool::Comm->burst_Read(db_Raw, false, 500);
			
	// TEAM is the target, trigger the TEAM sync bit!
	//std::cout << "Sending gretina start bit...\n";
	cool::Comm->raw_Write(wr_Buff, &wr_Len); 
				
	// Waits until the read request has been completed completed. Here
	// we are just waiting, but one could/should use this time to
	// perform data processing or others tasks. Important: you can
	// access the communication ONLY when the status comes back to
	// 'kbr_Free'!
	while (cool::Comm->burst_read_Status() != cool::Comm->kbr_Free) {
				
		// Use this to process the GUI events pipeline
		std::cout << "*";
		gSystem->ProcessEvents();
						
		// Frees (milliseconds) CPU time. Values between (10 - 50) are
		// usually ok to give good user feedback.
		Sleep(20);
	}

	//Rreturn the acquisition status
	return cool::Comm->burst_read_Exited();
}


// *****************************************************************************
// **												Support											**
// *****************************************************************************

//______________________________________________________________________________
void task_TEAM_Main::acq_FFT()
{
	// Try an FFT
	dbg_Print("task_TEAM_Main::acq_FFT:()", DBG_LVL_MAKE);

	Int_t N = 10; 
	Double_t *in = new Double_t[N];
   Int_t nDim = 2;
	Int_t dim_Size[2] = {1024, 1024};	
	
	// This works fine
	TVirtualFFT *fftr2c = TVirtualFFT::FFT(nDim, dim_Size, "R2C");
	fftr2c->SetPoints(df_Image->layer(0)->dp_Front());
	fftr2c->Transform();
	
	Double_t re, im;
	for (Int_t y = 0; y < 1024; y++) {
		for (Int_t x = 0; x < 512; x++) {
			fftr2c->GetPointComplex(x + y * 512, re, im, false);
			//df_FImage->layer(0)->dp_Value(x, y) = fftr2c->GetPointReal(x);
			df_FImage->layer(0)->dp_Value(x, y) = re;
		}
	}

	// Update graphics
	task_GUI->map_Elab->data_Set(df_FImage->layer(0));
	task_GUI->map_Elab->data_Refresh();  

}