//=============================================================================
//
// file :        MythenPool.cpp
//
// description : Monitoring Pilatus Sensors through the device Lima Pilatus
//
// project :	MythenWAXS Project
//
// $Author: noureddine $
//
// copyleft :    Synchrotron SOLEIL
//               L'Orme des merisiers - Saint Aubin
//               BP48 - 91192 Gif sur Yvette
//               FRANCE
//=============================================================================


#include <tango.h>

//- YAT stuff
#include <yat/utils/XString.h>

#include <iomanip>   
#include "MythenPool.h"

//@@@@TODO
//temporary and only for simulation 
const int CHANNEL_SIZE = 5;
const int PIXEL_SIZE = 0.05;
//////////////////////////////

namespace MythenWAXS_ns
{

//----------------------------------------------------------------------------------------------------------------------
//- MythenPool Ctor
//----------------------------------------------------------------------------------------------------------------------

MythenPool::MythenPool(Tango::DeviceImpl *dev, PropertyLoader prop)
: yat4tango::DeviceTask(dev),
m_device(dev),
m_prop(prop)
{
	DEBUG_STREAM << "MythenPool::MythenPool() - [BEGIN]" << endl;
	set_state(Tango::INIT);
	set_periodic_msg_period(TASK_PERIODIC__MS);//1Hz 
	enable_periodic_msg(false);
	DEBUG_STREAM << "MythenPool::MythenPool() - [END]" << endl;
}

//----------------------------------------------------------------------------------------------------------------------
//- MythenPool Dtor
//----------------------------------------------------------------------------------------------------------------------

MythenPool::~MythenPool()
{
	DEBUG_STREAM << "MythenPool::~MythenPool() - [BEGIN]" << endl;

	DEBUG_STREAM << "MythenPool::~MythenPool() - [END]" << endl;
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::set_state(Tango::DevState state)
{
	{
		//- AutoLock the following
		yat::MutexLock scoped_lock(m_proxy_lock);
		m_state = state;
	}
}


//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

Tango::DevState MythenPool::get_state()
{
	{
		//- AutoLock the following
		yat::MutexLock scoped_lock(m_proxy_lock);
		compute_state_status();
		return m_state;
	}
}


//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::set_status(const std::string& status)
{
	{
		//- AutoLock the following
		yat::MutexLock scoped_lock(m_proxy_lock);
		m_status.str("");
		m_status << status.c_str();
	}
}


//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

std::string MythenPool::get_status()
{
	{
		//- AutoLock the following
		yat::MutexLock scoped_lock(m_proxy_lock);
		return (m_status.str());
	}
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::write_energy(double value)
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
	{
		long nb_channels = 0;
		m_proxy_mythens.at(i)->read_attribute("nbChannels", nb_channels, m_prop.proxy_nb_retry);
		for (unsigned j = 0; j < nb_channels; j++)
		{
			stringstream ss("");
			std::string name("");
			name = "energy";
			ss << name << std::setfill('0') << std::setw(2) << j + 1;//index of energy start with '01'
			m_proxy_mythens.at(i)->write_attribute(ss.str(), value, m_prop.proxy_nb_retry);
		}
	}
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::write_threshold(double value)
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
	{
		long nb_channels = 0;
		m_proxy_mythens.at(i)->read_attribute("nbChannels", nb_channels, m_prop.proxy_nb_retry);
		for (unsigned j = 0; j < nb_channels; j++)
		{
			stringstream ss("");
			std::string name("");
			name = "threshold";
			ss << name << std::setfill('0') << std::setw(2) << j + 1;//index of threshold start with '01'
			m_proxy_mythens.at(i)->write_attribute(ss.str(), value, m_prop.proxy_nb_retry);
		}
	}
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::write_exposure_time(double value)
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
	{
		m_proxy_mythens.at(i)->write_attribute("exposureTime", value, m_prop.proxy_nb_retry);
	}
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::write_nb_frames(long value)
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
	{
		m_proxy_mythens.at(i)->write_attribute("nbFrames", value, m_prop.proxy_nb_retry);
	}
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::snap()
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
	{
		////m_proxy_mythens.at(i)->command("Snap", m_prop.proxy_nb_retry);
	}
	enable_periodic_msg(true);	//in order to compute_frame_xy()
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::stop()
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
	{
		////m_proxy_mythens.at(i)->command("Stop", m_prop.proxy_nb_retry);
	}
}


//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

const std::vector<double>& MythenPool::get_frame_x()
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	return m_x_values;
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

const std::vector<double>& MythenPool::get_frame_y()
{
	yat::MutexLock scoped_lock(m_proxy_lock);
	return m_y_values;
}
//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::compute_frame_xy()
{
	yat::MutexLock scoped_lock(m_proxy_lock);

	m_x_values.clear();
	m_y_values.clear();
	unsigned nb_total_channel = 0;
	//get the motor position tth2C
	double theta_position;
	m_proxy_motor->read_attribute("position", theta_position, m_prop.proxy_nb_retry);
	//compute tth
	for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
	{
		long nb_channels = 0;
		m_proxy_mythens.at(i)->read_attribute("nbChannels", nb_channels, m_prop.proxy_nb_retry);
		for (unsigned j = 0; j < nb_channels; j++)
		{
			std::vector<double> frame;
			stringstream ss("");
			std::string name("");
			name = "frame";
			ss << name << std::setfill('0') << std::setw(2) << j + 1;//index of threshold start with '01'
			m_proxy_mythens.at(i)->read_attribute(ss.str(), frame, m_prop.proxy_nb_retry);
			for (unsigned k = 0;k < CHANNEL_SIZE;k++)
			{
				double twotheta =	theta_position -
									m_deltas.at(nb_total_channel + j) -
									atan((k - m_centers.at(nb_total_channel + j)) * PIXEL_SIZE / (m_distances.at(nb_total_channel + j)));
				
				m_x_values.push_back(twotheta*(180.0/M_PI));
				m_y_values.push_back(frame.at(k));
			}
		}
		nb_total_channel += nb_channels;
	}
	//	INFO_STREAM << "m_twotheta_values.size() = " << m_twotheta_values.size() << std::endl;
}
//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::decode_equation_parameters()
{
	try
	{
		INFO_STREAM << "\n" << endl;
		INFO_STREAM << "----------------------------------------------------" << endl;
		INFO_STREAM << "Decode the EquationPrameters Device Property ..." << endl;
		INFO_STREAM << "----------------------------------------------------" << endl;
		//there is 9 lines
		//each line contains : delta=value;center=value;distance=value;
		for (unsigned i = 0; i < m_prop.equation_parameters.size(); i++)
		{
			Tokenizer line(m_prop.equation_parameters.at(i), ';');
			if (line.count() != 3)
			{
				Tango::Except::throw_exception(	"LOGIC_ERROR",
											"Check the format of the 'EquationParameters' Device Property !",
											"MythenPool::decode_equation_parameters()");
			}

			for (int itoken = 0; itoken < line.count(); itoken++)
			{
				try
				{
					string item_name;
					double item_val;
					//try catch used to check the end of items
					Tokenizer field(line[itoken], '=');
					if (field.count() != 2)
					{
						Tango::Except::throw_exception(	"LOGIC_ERROR",
													"Check the format of the 'EquationParameters' Device Property !",
													"MythenPool::decode_equation_parameters()");
					}

					item_name = field[0];
					istringstream iss_val(field[1]);//val

					iss_val >> item_val;
					if (item_name == "delta")
						m_deltas.push_back(item_val);
					else if (item_name == "center")
						m_centers.push_back(item_val);
					else if (item_name == "distance")
						m_distances.push_back(item_val);
				}
				catch (exception& e)
				{
					//indicate that token has not the good format or no token anymore, Nothing to do
					//DEBUG_STREAM<<"token has not the good format or no token anymore ! "<<endl;
					continue;
				}
			}
		}

		//display values
		for (int i = 0; i < m_deltas.size(); i++)
		{
			INFO_STREAM << "m_deltas[i] = " << m_deltas[i] << std::endl;
		}
		INFO_STREAM << "\n" << std::endl;

		for (int i = 0; i < m_centers.size(); i++)
		{
			INFO_STREAM << "m_centers[i] = " << m_centers[i] << std::endl;
		}
		INFO_STREAM << "\n" << std::endl;

		for (int i = 0; i < m_distances.size(); i++)
		{
			INFO_STREAM << "m_distances[i] = " << m_distances[i] << std::endl;
		}
		INFO_STREAM << "\n" << std::endl;

	}
	catch (exception& e)
	{
		//indicate that token has not the good format or no token anymore
		ERROR_STREAM << e.what() << endl;
		ostringstream ossMsgErr;
		ossMsgErr << e.what();
		Tango::Except::throw_exception("LOGIC_ERROR",
									ossMsgErr.str().c_str(),
									"MythenPool::decode_equation_parameters()");
	}
}
//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::compute_state_status()
{
	{
		//- AutoLock the following
		yat::MutexLock scoped_lock(m_proxy_lock);
		DEBUG_STREAM << "MythenPool::compute_state_status() - [BEGIN]" << endl;
		unsigned proxy_num;
		Tango::DevState global_state;
		stringstream proxy_status;
		proxy_status.str("");
		vector <Tango::DevState> states;
		unsigned int fault_count = 0;
		unsigned int running_count = 0;
		unsigned int standby_count = 0;
		if (m_state != Tango::INIT)
		{
			//////////////////////////////////////////////////////////

			try
			{
				//more than one proxy mythen devices 
				for (unsigned i = 0; i < m_prop.proxy_mythen_names.size(); i++)
				{
					proxy_num = i;
					Tango::DevState simple_state;
					m_proxy_mythens.at(i)->command_out("State", simple_state, m_prop.proxy_nb_retry);
					proxy_status << "Proxy[" << proxy_num << "]\t\t" << " --> " << Tango::DevStateName[simple_state] << "\n";
					states.push_back(simple_state);
				}

				//only one proxy motor device
				Tango::DevState simple_state;
				m_proxy_motor->command_out("State", simple_state, m_prop.proxy_nb_retry);
				proxy_status << "Proxy[" << proxy_num << "]\t\t" << " --> " << Tango::DevStateName[simple_state] << "\n";
				states.push_back(simple_state);

				//counting each kind of state
				for (int i = 0; i < states.size(); i++)
				{
					switch (states.at(i))
					{
						case Tango::FAULT: fault_count++;
							break;
						case Tango::STANDBY: standby_count++;
							break;
						case Tango::RUNNING: running_count++;
							break;
						case Tango::MOVING: running_count++;
							break;
						default:	fault_count++;
							break; //NOP
					}

				}

				//////
				if (fault_count)
				{
					global_state = Tango::FAULT; //if at least one device is in fault
				}
				else if (running_count)
				{
					global_state = Tango::RUNNING; //if all devices are RUNNING				
				}
				else if (standby_count == states.size())
				{
					global_state = Tango::STANDBY; //if all devices are STANDBY
				}
				else
				{
					global_state = Tango::ALARM; //Otherwise ALARM            
				}
			}
			catch (Tango::DevFailed& e)
			{
				ERROR_STREAM << e << ENDLOG;
				proxy_status << "Proxy[" << proxy_num << "]\t\t" << " --> Unable to read State !\n";
				global_state = Tango::FAULT;
			}
			//////////////////////////////////////////////////////////

			//if pilatus is in "FAULTy" state
			if (global_state == Tango::FAULT || global_state == Tango::OFF || global_state == Tango::INIT)
			{
				set_state(Tango::FAULT);
				std::stringstream status;
				status.str("");
				status << "At least one Device is in FAULTy state !\n";
				status << "---------------------------------------------------\n";
				set_status(status.str());
				DEBUG_STREAM << "MythenPool::compute_state_status() - [END]" << endl;
				return;
			}

			//if pilatus is in "RUNNING" state
			if (global_state == Tango::RUNNING || global_state == Tango::MOVING)
			{
				set_state(Tango::RUNNING);
				std::stringstream status;
				status.str("");
				status << "Acquisition is Running ...\n";
				status << "---------------------------------------------------\n";
				set_status(status.str());
				DEBUG_STREAM << "MythenPool::compute_state_status() - [END]" << endl;
				return;
			}

			if (global_state == Tango::STANDBY)
			{
				//STANDBY    
				set_state(Tango::STANDBY);
				std::stringstream status;
				status.str("");
				status << "Waiting for request ...\n";
				status << "---------------------------------------------------\n";
				set_status(status.str());
				DEBUG_STREAM << "MythenPool::compute_state_status() - [END]" << endl;
				return;
			}

			if (global_state == Tango::ALARM)
			{
				//ALARM
				set_state(Tango::ALARM);
				std::stringstream status;
				status.str("");
				status << "At least one Device is not in the expected state !\n";
				status << "---------------------------------------------------\n";
				set_status(status.str());
				DEBUG_STREAM << "MythenPool::compute_state_status() - [END]" << endl;
				return;
			}
		}
	}
}

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void MythenPool::process_message(yat::Message& msg) throw (Tango::DevFailed)
{
	try
	{
		switch (msg.type())
		{
				//-----------------------------------------------------
			case yat::TASK_INIT:
			{
				INFO_STREAM << " " << std::endl;
				INFO_STREAM << "--------------------------------------------" << std::endl;
				INFO_STREAM << "-> MythenPool::TASK_INIT" << endl;
				INFO_STREAM << "--------------------------------------------" << std::endl;

				std::string proxy_name;
				try
				{
					INFO_STREAM << "" << endl;
					//create proxies to Mthen2Detector
					for (int i = 0;i < m_prop.proxy_mythen_names.size();i++)
					{
						proxy_name = m_prop.proxy_mythen_names.at(i);
						INFO_STREAM << "\t- Create the Proxy to the device (" << proxy_name << ")" << endl;
						m_proxy_mythens.push_back(new yat4tango::DeviceProxyHelper(proxy_name, m_device));
						m_proxy_mythens[i]->get_device_proxy()->ping();
					}

					//create proxy to the motor
					proxy_name = m_prop.proxy_motor_name;
					INFO_STREAM << "\t- Create the Proxy to the motor (" << proxy_name << ")" << endl;
					m_proxy_motor.reset(new yat4tango::DeviceProxyHelper(proxy_name, m_device));
					m_proxy_motor->get_device_proxy()->ping();
				}
				catch (const std::bad_alloc& e)
				{
					ERROR_STREAM << e.what() << ENDLOG;
					std::stringstream status;
					status << "Unable to create proxy to ( " << proxy_name << " ) :\n"
					 << "--> Memory allocation exception.\n"
					 << endl;
					set_state(Tango::INIT);
					set_status(status.str());
					return;
				}
				catch (Tango::DevFailed& e)
				{
					ERROR_STREAM << e << ENDLOG;
					std::stringstream status;
					status << "Unable to create proxy to ( " << proxy_name << " ) :\n"
					 << "--> check the value of the 'ProxiesNames' property.\n"
					 << "--> check the state of the device ( " << proxy_name << " ).\n"
					 << endl;
					set_state(Tango::INIT);
					set_status(status.str());
					return;
				}

				//decode constants from equation_constants property
				decode_equation_parameters();

				//set state STANDBY if everything is ok
				set_state(Tango::STANDBY);

				//enable_periodic_msg(true);
			}
				break;
				//-----------------------------------------------------
			case yat::TASK_EXIT:
			{
				INFO_STREAM << " " << std::endl;
				INFO_STREAM << "--------------------------------------------" << std::endl;
				INFO_STREAM << "-> MythenPool::TASK_EXIT" << endl;
				INFO_STREAM << "--------------------------------------------" << std::endl;
			}
				break;
				//-----------------------------------------------------
			case yat::TASK_TIMEOUT:
			{
				INFO_STREAM << " " << std::endl;
				INFO_STREAM << "--------------------------------------------" << std::endl;
				INFO_STREAM << "-> MythenPool::TASK_TIMEOUT" << endl;
				INFO_STREAM << "--------------------------------------------" << std::endl;
			}
				break;
				//-----------------------------------------------------                
			case yat::TASK_PERIODIC:
			{
				//- AutoLock the following
				yat::MutexLock scoped_lock(m_proxy_lock);
				Tango::DevState state = get_state();
				if (state == Tango::STANDBY)
				{
					DEBUG_STREAM << " " << std::endl;
					DEBUG_STREAM << "--------------------------------------------" << std::endl;
					DEBUG_STREAM << "-> MythenPool::TASK_PERIODIC" << endl;
					DEBUG_STREAM << "--------------------------------------------" << std::endl;
					try
					{
						INFO_STREAM << "--------------------------------------------" << std::endl;
						INFO_STREAM << "Compute frameX & frameY" << endl;
						INFO_STREAM << "--------------------------------------------" << std::endl;
						compute_frame_xy();
						enable_periodic_msg(false);
					}
					catch (Tango::DevFailed &df)
					{
						ERROR_STREAM << df << endl;
						std::stringstream status;
						status.str("");
						status << "Origin\t: " << df.errors[0].origin << endl;
						status << "Desc\t: " << df.errors[0].desc << endl;
						set_status(status.str());
						set_state(Tango::FAULT);
					}
				}
			}
				break;
				//-----------------------------------------------------
		}
	}
	catch (yat::Exception& ex)
	{
		//- TODO Error Handling
		ex.dump();
		std::stringstream error_msg("");
		error_msg << "Origin\t: " << ex.errors[0].origin << endl;
		error_msg << "Desc\t: " << ex.errors[0].desc << endl;
		error_msg << "Reason\t: " << ex.errors[0].reason << endl;
		ERROR_STREAM << "Exception from - MythenPool::process_message() : " << error_msg.str() << endl;
		throw;
	}
}

} // namespace