#include "Process.h"
#include "qmessagebox.h"
#include "TaskQueue/TQFactory.h"
#include "mainwindow.h"

CProcess::CProcess(Automatic::stCalibParam param, void* mainWnd)
{
	m_laser_calib_param = param;
	m_laser = NULL;
	m_main_wnd = mainWnd;
    m_thread_queue=0;
    connect(this,SIGNAL(MainWndSignal(QVariant)),
            (MainWindow*)m_main_wnd,SLOT(ProcessSlot(QVariant)));
}


CProcess::~CProcess()
{
	const int n = m_laser_calib_param.laser_size();
	for (int i = 0; i < n; ++i)
	{
		std::string type = m_laser_calib_param.laser(i).type();
		if (type.find("Livox") != type.npos)
		{
			Uninit();
			break;
		}
	}

    if(m_thread_queue)
    {
        m_thread_queue->Stop();
        delete m_thread_queue;
        m_thread_queue=0;
    }
	if (m_laser)
	{
		for (int i = 0; i < n; ++i)
		{
			if (m_laser[i])
			{
				m_laser[i]->Disconnect();
				delete m_laser[i];
				m_laser[i] = NULL;
			}

		}
		delete[] m_laser;
		m_laser = NULL;
	}


	
}


bool CProcess::Init()
{


    m_thread_queue=tq::TQFactory::CreateDefaultQueue();
    m_thread_queue->Start(3);

	if (NULL == m_laser)
	{
		const int n = m_laser_calib_param.laser_size();
		m_laser = new CLaser*[n];

		for (int i = 0; i < n; ++i)
		{
			std::string type = m_laser_calib_param.laser(i).type();
			////�ж��Ƿ��Ǵ��ļ���
			if (type.find("File") != type.npos)
			{
				LOG(INFO) << "create fileLaser";
				std::string dir = m_laser_calib_param.debug_file();
				m_laser_calib_param.mutable_laser(i)->set_laser_ip(dir);

			}
			//����laser
			m_laser[i] = LaserRegistory::CreateLaser(type, i, m_laser_calib_param.laser(i));
			if (m_laser[i] == NULL)
			{
				char buf[255] = { 0 };
				sprintf(buf, " Laser type : %s  is not exits...", type.c_str());
                m_last_error=buf;
				return false;
			}
			
			if (!m_laser[i]->Connect())
			{
				char error[255] = { 0 };
				sprintf(error, "laser %d connect failed...", i);
				m_last_error = error;
				return false;
			}
			double mat[12] = { 0 };
			if (m_laser_calib_param.is_calib()==false)
			{
				create_mat_param(m_laser_calib_param.laser(i), mat);
				m_laser[i]->SetMetrix(mat);
			}
		}

		for (int i = 0; i < n; ++i)
		{
			std::string type = m_laser_calib_param.laser(i).type();
			if (type.find("Livox") != type.npos)
			{
				if (Start() == false)
				{
					Uninit();
                    m_last_error=("Livox laser init failed...");
					return false;
				}
				break;
			}
		}

	}


	// ���ӵ�PLC,����locate
	if (m_laser_calib_param.is_calib() == false)
	{
		if (0 != m_monitor.connect(m_laser_calib_param.plc().plc_ip().c_str(), m_laser_calib_param.plc().plc_port(), 1))
		{
            m_last_error=(" connect PLC failed...");
			return false;
		}
		else
		{
            m_monitor.set_callback(action_callback, monitor_callback,this);
		}


	}

	return true;
}

void CProcess::PushTask(uint16_t param, int action_type,bool test)
{
	char plate = 0;
	uint16_t laserID = param;
	memcpy(&plate, &param, 1);
	m_plate_index = plate;
	std::vector<CLaser*> lasers;
	for (int i = 0; i < 8; ++i)
	{
		if (((0x01 << i)&laserID) > 0)
		{
			const int n = m_laser_calib_param.laser_size();
			if (i >= 0 && i < n)
				lasers.push_back(m_laser[i]);
		}
	}

	//������ɨ��������

	modbus::CPLCMonitor* plc = 0;
	if (test == false)
		plc = &(m_monitor);
    tq::BaseTask* task = 0;
	if(action_type==1)
        task= new MeasureTask(lasers,  int(plate),plc, m_laser_calib_param);
    /*else if(action_type==2)
        task = new CFenceTask(lasers, m_locater, int(plate), plc, m_laser_calib_param);*/

    ((MeasureTask*)task)->SetResultCallback(this, monitor_callback);
    m_thread_queue->AddTask(task);
}

void CProcess::action_callback(bool bOn, uint16_t param, void* pointer, int action_type)
{

	CProcess* process = reinterpret_cast<CProcess*>(pointer);
	if(bOn)
		process->PushTask(param, action_type);
}

void CProcess::monitor_callback(QtMessageData data, void* pointer)
{
    static QtMessageData msg;
    memcpy(&msg,&data,sizeof(data));
    CProcess* process = reinterpret_cast<CProcess*>(pointer);
    static QVariant var;
    var.setValue(msg);
    emit process->MainWndSignal(var);
}

void CProcess::create_mat_param(Automatic::stLaserCalibParam laser, double* buf)
{
	std::string type = laser.type();
	if (type.find("Linear") != type.npos)
	{
		double x_angle = laser.axis_x_theta() / 180.0*PI;
		double y_angle = laser.axis_y_theta() / 180.0*PI;
		double z_angle = laser.axis_z_theta() / 180.0*PI;
		double t_x = laser.translation_x();
		double t_y = laser.translation_y();
		double t_z = laser.translation_z();
		Eigen::AngleAxisd rollAngle(x_angle, Eigen::Matrix<double, 3, 1>::UnitX());
		Eigen::AngleAxisd pitchAngle(y_angle, Eigen::Matrix<double, 3, 1>::UnitY());
		Eigen::AngleAxisd yawAngle(z_angle, Eigen::Matrix<double, 3, 1>::UnitZ());
		Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
		Eigen::Matrix<double, 3, 3> rotationMatrix = q.toRotationMatrix();

		Eigen::Matrix<double, 3, 4> transform;
		Eigen::Matrix<double, 3, 1> translation;
		translation << t_x, t_y, t_z;
		transform << rotationMatrix, translation;

		buf[0] = transform(0, 0);
		buf[1] = transform(0, 1);
		buf[2] = transform(0, 2);
		buf[3] = transform(0, 3);
		buf[4] = transform(1, 0);
		buf[5] = transform(1, 1);
		buf[6] = transform(1, 2);
		buf[7] = transform(1, 3);
		buf[8] = transform(2, 0);
		buf[9] = transform(2, 1);
		buf[10] = transform(2, 2);
		buf[11] = transform(2, 3);
	}
	else
	{
		buf[0] = laser.mat_r00();
		buf[1] = laser.mat_r01();
		buf[2] = laser.mat_r02();
		buf[3] = laser.mat_r03();
		buf[4] = laser.mat_r10();
		buf[5] = laser.mat_r11();
		buf[6] = laser.mat_r12();
		buf[7] = laser.mat_r13();
		buf[8] = laser.mat_r20();
		buf[9] = laser.mat_r21();
		buf[10] = laser.mat_r22();
		buf[11] = laser.mat_r23();
	}
}

/*#include "MsgDef.h"
void CProcess::result_callback(Locate_Message msg, void* ptr)
{
	
	CProcess* process = reinterpret_cast<CProcess*>(ptr);
	std::lock_guard<std::mutex> lk(process->m_mutex);
	process->m_locate_msg = msg;
	if(process->m_main_wnd)
		::PostMessage(process->m_main_wnd->m_hWnd, WM_MSG_MainWnd, WPARAM(WM_Draw_cloud), 0);
}*/