//
// file :         readthread.cpp
//
// description :  reading thread
//
// project :      TANGO Device Server
//
// $Author: graziano $ 
//
// $Revision: 1.1 $
//
// $Log: readthread.cpp,v $
// Revision 1.1  2017-12-21 11:26:12  graziano
// first commit
//
//
//
//
//
//
// copyleft :   Sincrotrone Trieste S.C.p.A.
//              Strada Statale 14 - km 163,5 in AREA Science Park
//              34012 Basovizza, Trieste ITALY
//

#include "readthread.h"
#include "ConexAgp.h"
#include <math.h>

static const char __FILE__rev[] = __FILE__ " $Revision: 1.1 $";

namespace ConexAgp_ns
{

//+------------------------------------------------------------------
//
//	method:			readthread::readthread()
//
//	description:	readthread constructor
//
//-------------------------------------------------------------------
readthread::readthread(Tango::DeviceImpl* devImpl):Tango::LogAdapter(devImpl)
{ 
	DEBUG_STREAM << "readthread::readthread(): constructor... :" << __FILE__rev << endl;
	device = devImpl;
}

//+------------------------------------------------------------------
//
//	method:			readthread::~readthread()
//
//	description:	readthread destructor
//
//-------------------------------------------------------------------
readthread::~readthread()
{
	DEBUG_STREAM << "readthread::~readthread(): destructor... !" << endl;
}

//+------------------------------------------------------------------
//
//	method:			readthread::run()
//
//	description:	Run
//
//-------------------------------------------------------------------
void readthread::run(void *)
{
	INFO_STREAM << "readthread::run(): running... !" << endl;
	/*int pausesec,pausenano,pausesec_dfl,pausenano_dfl,
	    cnt_err = 0,
		max_cnt_err = 3,
		ret;*/

	abortflag = false;
	ConexAgp *_device = static_cast<ConexAgp *>(device);


	while (!abortflag)
	{
		DEBUG_STREAM << "readthread::run(): looping "<< endl;
		string tstate;
		_device->SendReceive(GET_STATE, tstate);
		if(tstate.length() == 6)
		{
			/*uint16_t positioner_err = ((tstate[0]-0x30) << 12) |
										((tstate[1]-0x30) << 8) |
										((tstate[2]-0x30) << 4) |
										((tstate[3]-0x30));
			uint8_t controller_state = (((tstate[4]-0x30) << 4) | ((tstate[5]-0x30)));*/

			stringstream pos_err;
			pos_err << tstate[0] << tstate[1] << tstate[2] << tstate[3];
			uint16_t positioner_err;
			pos_err >> hex >> positioner_err;
			stringstream contr_state;
			contr_state << tstate[4] << tstate[5];
			uint16_t controller_state;
			contr_state >> hex >> controller_state;

			INFO_STREAM << __func__<<": state '" << tstate << "' len="<<tstate.length() << " positioner_err=0x"<<hex<<positioner_err<<" controller_state=0x"<<(int)controller_state<<dec;

			stringstream _status;
			Tango::DevState _state;
			if(positioner_err != 0)
			{
				_state = Tango::ALARM;
				_status << "Positioner error=0x"<<hex << positioner_err << dec;
				if(positioner_err & 0x0020)
					_status << " (Motion time out)";
			}
			else
			{
				map<uint8_t,state_desc>::iterator it = _device->controller_states.find(controller_state);
				if(it != _device->controller_states.end())
				{
					_state = it->second._state;
					_status << it->second._status;
				}
				else
				{
					_state = Tango::UNKNOWN;
					_status << "Unknown state 0x" << (int)controller_state;
				}
			}
			_device->set_state(_state);
			_device->push_change_event("State");
			_device->push_archive_event("State");
			_device->set_status(_status.str());
		}
		else
		{
			INFO_STREAM << __func__<<": error parsing state '" << tstate << "' len="<<tstate.length();
		}
		usleep(10000);
		double posx;
		_device->SendReceive(string(GET_POSITION), posx);
		*(_device->attr_Position_read) = posx;
		_device->push_change_event("Position",_device->attr_Position_read);
		_device->push_archive_event("Position",_device->attr_Position_read);
		usleep(10000);
		double postx;
		_device->SendReceive(string(GET_TARGET_POSITION), postx);
		*(_device->attr_TargetPosition_read) = postx;
		usleep(10000);
		INFO_STREAM << __func__<<": " << posx << " -> "<<*(_device->attr_Position_read);

		abort_sleep(1);
	}
	INFO_STREAM << "readthread::run(): exit!!!" << endl;
}

//+------------------------------------------------------------------
//
//	method:			readthread::abort_sleep
//
//	description:	Resume from sleep if abort_flag set (sec.)
//
//-------------------------------------------------------------------
void readthread::abort_sleep(double time)
{

	for (int i = 0; i < (time/0.3); i++) {
		if (abortflag)
			break;
		omni_thread::sleep(0,300000000);
	}

}


}