#include "PLCMonitor.h"
#include <unistd.h>
#include "../common.h"

namespace modbus
{
#ifndef WIN32
#define Sleep(T)  usleep((T)*1000)
#endif
    const double CPLCMonitor::PLC_LASER_TIMEOUT_READY = 1.0;		//完成信号到心跳延时
    const double CPLCMonitor::PLC_LASER_TIMEOUT_PINGPANG = 0.1;
    const double CPLCMonitor::PLC_LASER_TIMEOUT_START = 0.3;
    const double CPLCMonitor::PLC_LASER_TIMEOUT_MAXMEASURE = 6.0;

	CPLCMonitor::CPLCMonitor(void* pOwnerObject)
        :_monitoring(false)
		, _heartbeat_write_clock(clock())
		, m_callback(0)
		,m_pointer(0)
        ,m_PlcDataCallback(0)
        ,m_thread_read(0)
    {
        memset(&_value, 0, sizeof(_value));
    }

    CPLCMonitor::~CPLCMonitor()
    {
		_monitoring = false;
		if (m_thread_read)
		{
            if(m_thread_read->joinable())
                m_thread_read->join();
            m_thread_read = 0;
		}
    }

    int CPLCMonitor::connect(const char *ip, int port, int slave_id)
    {
        cs_pcl.lock();
        if (-1 == dev.initialize(ip, port, slave_id))
        {
            cs_pcl.unlock();
            return -1;
        }
        cs_pcl.unlock();

        _monitoring = true;
        if (FALSE == Start())
        {
            dev.deinitialize();
            return -1;
        }
        m_thread_read = new std::thread(thread_monitor, this);
		m_strIP = ip;
		m_port = port;
		m_slave_id = slave_id;

        return 0;
    }

    void CPLCMonitor::set_callback(CommandCallback func, PLCMonitorCallback monitorCallback,void* pointer)
	{
		m_callback = func;
        m_PlcDataCallback=monitorCallback;
		m_pointer = pointer;
	}

	int CPLCMonitor::reconnect()
	{
        cs_pcl.lock();
		if (-1 == dev.initialize(m_strIP.c_str(), m_port, m_slave_id))
		{
            cs_pcl.unlock();
			return -1;
		}
        cs_pcl.unlock();
		return 0;
	}

    void CPLCMonitor::disconnect()
    {
        _monitoring = false;
        Join(1000);


        cs_pcl.lock();
        dev.deinitialize();
        cs_pcl.unlock();
    }

    int CPLCMonitor::read_registers(int addr, int nb, uint16_t *dest)
    {
        cs_pcl.lock();
        int rc = dev.read_registers(addr, nb, dest);
        cs_pcl.unlock();
		if (0 != rc)
		{
			reconnect();//端开，重连
		}
        return rc;
    }

    int CPLCMonitor::write_registers(int addr, int nb, uint16_t *dest)
    {
        cs_pcl.lock();
        int rc = dev.write_registers(addr, nb, dest);
        cs_pcl.unlock();

        return rc;
    }

    int CPLCMonitor::read_register(int addr, uint16_t *dest)
    {
        cs_pcl.lock();
        int rc = dev.read_register(addr, dest);
        cs_pcl.unlock();
        return rc;
    }

    int CPLCMonitor::write_register(int addr, uint16_t *dest)
    {
        cs_pcl.lock();
        int rc = dev.write_register(addr, dest);
        cs_pcl.unlock();

        return rc;
    }

    int CPLCMonitor::clear_laserstatus_register(uint16_t value)
    {
        cs_pcl.lock();
        int rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
        cs_pcl.unlock();
        _heartbeat_write_clock = clock();

        return rc;
    }

    int CPLCMonitor::write_laserstatus_register(uint16_t value)
    {
        uint16_t dest = 0xff;
        cs_pcl.lock();

        int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
        if (PLC_LASER_ERROR != dest)
        {
            rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
        }
        cs_pcl.unlock();

        _heartbeat_write_clock = clock();

		if (0 != rc)
		{
			reconnect();//端开，重连
		}

        return rc;
    }

    int CPLCMonitor::write_laserresult_register(int addr,uint16_t *pvalue)
    {
        uint16_t dest = 0xff;
        cs_pcl.lock();

		int rc= dev.write_registers(addr, 10, pvalue);
        /*int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
        if (PLC_LASER_ERROR != dest)
        {
            rc = dev.write_registers(REG_WHISKBOOMLASER_STATUS, 7, pvalue);
        }*/
        cs_pcl.unlock();

        _heartbeat_write_clock = clock();

        return rc;
    }

	void CPLCMonitor::MeasureComplete(bool bOK)
	{
		//_measure_finished = TRUE;
		if (!bOK)
		{
			write_laserstatus_register(PLC_LASER_FINISH_FAILED);
		}
		else
		{
			write_laserstatus_register(PLC_LASER_FINISH_OK);
		}
	}

    bool CPLCMonitor::setMeasureResult(int addr, struct whiskboom_laser_value * p)
    {
		if (p == 0||addr<0)	//失败，没有摆扫结果，硬件错误；
		{
			return false;
		}
		else
		{
			uint16_t response[10] = { 0 };
			memset(response, 0, sizeof(uint16_t) * 10);
			if (p->corrected)
			{
				response[6] = eLaser_data_ok;
			}
			else {
				response[6] = eLaser_data_failed;
			}
			response[0] = (p->x);
			response[1] = p->y;
			response[2] = (int(p->a) % 18000);
			response[3] = p->l;
			response[4] = p->w;
			response[5] = p->h;
			response[7] = p->l;			////轴距
			write_laserresult_register(addr,(uint16_t *)response);
			return true;
		}
    }

	void CPLCMonitor::Monitoring()
	{
		const int SIGNAL_NUM = 64;
		const int SIGNAL_ADDR = 0;
		static uint16_t value[SIGNAL_NUM] = { 0 };
        static uint16_t last_value[SIGNAL_NUM] = { 0 };
		int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, last_value);
		
        LOG(INFO) << "\tStart signal monitoring thread";
		while (_monitoring)
		{
			memset(value, 0, SIGNAL_NUM * sizeof(uint16_t));
			int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, value);
			
			if (rc == 0)
			{
				for (int i = REG_PARKSTATUS; i < SIGNAL_NUM; ++i)
				{
					if (value[i] != last_value[i])
					{
                        if(m_PlcDataCallback && m_pointer)
                        {
                            QtMessageData data;
                            data.msg_type=ePlcSignal;
                            data.signal_size=SIGNAL_NUM;
                            memcpy(data.plc_data,value,sizeof(uint16_t)*SIGNAL_NUM);
                            m_PlcDataCallback(data,m_pointer);
                        }
                        break;
					}
				}
				memcpy(last_value, value, sizeof(uint16_t)*SIGNAL_NUM);
			}

            Sleep(200);
		}
        LOG(INFO) << "\tStop signal monitor thread!";
	}

    void CPLCMonitor::thread_monitor(void* lp)
	{
		CPLCMonitor* plc = (CPLCMonitor*)lp;
		plc->Monitoring();
	}

    void CPLCMonitor::Run()
    {
        uint16_t value[2] = { 0xffff,0xffff };
        uint16_t parking_status = 0xff;
        uint16_t response[7] = { 0 };
        int rc = -1;
        clock_t current_clock;

        uint16_t finite_state_machines = PLC_LASER_READY;
        write_laserstatus_register(finite_state_machines);
        uint16_t las_parkstatus = 0;

        while (_monitoring)
        {
            memset(value, 0xff, sizeof(value));

            rc = read_registers(REG_PARKSTATUS, 2, value);
            if (-1 == rc)
            {
                fprintf(stderr, "CPLCMonitor: read regiser failed\n");
            }

            parking_status = value[0];
            finite_state_machines = value[1];


			//上次状态为：开始测量，测量中
			if (PLC_LASER_START == finite_state_machines
				|| PLC_LASER_WORKING == finite_state_machines)
			{
                if (double(current_clock - _heartbeat_write_clock)/CLOCKS_PER_SEC >= PLC_LASER_TIMEOUT_READY)
				{
					write_laserstatus_register(PLC_LASER_PING);
					finite_state_machines = PLC_LASER_PING;
				}
			}


            //PLC 我要停车等待状态
            if (PLC_PARKING_WAIT == parking_status|| 2 == parking_status)
            {
                current_clock = clock();

                //激光上次状态为：测量完成成功或者测量完成失败的状态
                if (PLC_LASER_FINISH_OK == finite_state_machines
                    || PLC_LASER_FINISH_FAILED == finite_state_machines)
                {
                    //超过10秒，开始返回就绪状态
                    if (double(current_clock - _heartbeat_write_clock)/CLOCKS_PER_SEC >= PLC_LASER_TIMEOUT_READY)
                    {
						//先清除数据
						if (PLC_LASER_FINISH_FAILED == finite_state_machines)
						{
							/*memset(response, 0, 7 * sizeof(uint16_t));
							write_laserresult_register((uint16_t *)response);*/
						}
                        write_laserstatus_register(PLC_LASER_READY);
						finite_state_machines = PLC_LASER_READY;
                    }
                }


                //激光上次状态为：就绪或者心跳状态1，
                else if (PLC_LASER_READY == finite_state_machines
                    || PLC_LASER_PONG == finite_state_machines)
                {
                    //超过3秒，开始返回心跳状态2
                    if (double(current_clock - _heartbeat_write_clock)/CLOCKS_PER_SEC >= PLC_LASER_TIMEOUT_PINGPANG)
                    {
                        write_laserstatus_register(PLC_LASER_PING);
						finite_state_machines = PLC_LASER_PING;
                    }
                }
                //激光上次状态为：心跳状态2
                else if (PLC_LASER_PING == finite_state_machines)
                {
                    //超过3秒，开始返回心跳状态1
                    if (double(current_clock - _heartbeat_write_clock)/CLOCKS_PER_SEC >= PLC_LASER_TIMEOUT_PINGPANG)
                    {
                        write_laserstatus_register(PLC_LASER_PONG);
						finite_state_machines = PLC_LASER_PONG;
                    }
                }
                //激光上次状态为：测量开始，测量中）
                else if (PLC_LASER_START == finite_state_machines
                    || PLC_LASER_WORKING == finite_state_machines)
                {
                    //此时激光可能在测量状态中,让激光测量停止，结果：
                    //    1. 如果未在测量状态，激光接受状态不会有任何返回值，
                    //    2. 如果在测量状态会有测量失败信息须处理
                    LOG(ERROR) << "\t 测量中开始信号终止";
                   /* cs_cmd.Lock();
					for (std::vector<CLaserNet*>::iterator it = _cmdlistLaserPointer.begin(); it != _cmdlistLaserPointer.end(); ++it)
						(*it)->Stop();
					cs_cmd.Unlock();*/

                    write_laserstatus_register(PLC_LASER_READY);
					finite_state_machines = PLC_LASER_READY;
                }
                //不可修复错误，等系统重启
                else if (PLC_LASER_ERROR == finite_state_machines)
                {
                    fprintf(stderr, "\n\n\n\n\n\n不可修复错误，等系统重启\n\n\n\n\n\n");
                }
                else
                {
                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$完全不可能出现的状态\n\n\n\n\n\n");
                }
                las_parkstatus = PLC_PARKING_WAIT;

            }
            //PLC 我要停车测量开始
            else if (parking_status>0)
            {
                //激光上次状态为：就绪，心跳状态1，心跳状态2
                if(PLC_LASER_READY == finite_state_machines
                        || PLC_LASER_PING == finite_state_machines
                        || PLC_LASER_PONG == finite_state_machines
                        || PLC_LASER_FINISH_OK == finite_state_machines
                        || PLC_LASER_FINISH_FAILED == finite_state_machines)
                {
					/////  测量
                    if (PLC_PARKING_WAIT == las_parkstatus && parking_status==1)
                    {
						//置测量结果为未完成状态
                        _measure_finished = FALSE;
						
                        //测量开始
						if (m_callback)
						{
							//char plate = parking_status;
							
							uint16_t param = 0x03;
							m_callback(true, param, m_pointer, parking_status);
						}

                        //返回测量开始状态
                        write_laserstatus_register(PLC_LASER_START);
						finite_state_machines = PLC_LASER_START;
                    }
					////// 电子围栏测量
					/*else if ((PLC_LASER_READY == finite_state_machines
						|| PLC_LASER_PING == finite_state_machines
						|| PLC_LASER_PONG == finite_state_machines)
						&&parking_status == 2)
					{
						//置测量结果为未完成状态
						_measure_finished = FALSE;

						//测量开始
						if (m_callback)
						{
							uint16_t param = 0x03;
							m_callback(true, param, m_pointer, parking_status);
						}

						//返回测量开始状态
						write_laserstatus_register(PLC_LASER_START);
						finite_state_machines = PLC_LASER_START;
					}*/
                }

				//上次状态为：测量完成成功，测量完成错误，
				if (PLC_LASER_FINISH_OK == finite_state_machines
					|| PLC_LASER_FINISH_FAILED == finite_state_machines)
				{
                    if (double(current_clock - _heartbeat_write_clock)/CLOCKS_PER_SEC >= PLC_LASER_TIMEOUT_PINGPANG)
					{
						write_laserstatus_register(PLC_LASER_PING);
						finite_state_machines = PLC_LASER_PING;
					}
				}

                //激光上次状态为:开始测量
                else if (PLC_LASER_START == finite_state_machines)
                {
					current_clock = clock();  
                    //超过200毫秒，返回测量中
                    if (double(current_clock - _heartbeat_write_clock)/CLOCKS_PER_SEC >= PLC_LASER_TIMEOUT_START)
                    {
                        write_laserstatus_register(PLC_LASER_WORKING);
						finite_state_machines = PLC_LASER_WORKING;
                    }
                }
                else if (PLC_LASER_WORKING == finite_state_machines)
                {
                    if (TRUE == _measure_finished)
                    {
						//测量完成
                       /* memset(response, 0, sizeof(uint16_t) * 7);
                        if (_value.corrected)
                        {
                            response[0] = PLC_LASER_FINISH_OK;
                        }
                        else {
                            response[0] = PLC_LASER_FINISH_FAILED;
                        }
						response[1] = _value.x;
						response[2] = _value.y;
						response[3] = (int(_value.a) % 18000);
						response[4] = _value.l;
						response[5] = _value.w;
						response[6] = _value.h;
                        write_laserresult_register((uint16_t *)response);*/
						write_laserstatus_register(PLC_LASER_FINISH_FAILED);
						finite_state_machines = PLC_LASER_FINISH_FAILED;
                    }
                    else {
                        current_clock = clock();
                        if (double(current_clock - _heartbeat_write_clock)/CLOCKS_PER_SEC > PLC_LASER_TIMEOUT_MAXMEASURE) //10分钟还没返回测量结果， 说明系统可能出错了
                        {
                            write_laserstatus_register(PLC_LASER_ERROR);
							finite_state_machines = PLC_LASER_ERROR;
                        }
                    }
                }
                else if (PLC_LASER_ERROR == finite_state_machines)
                {

                    fprintf(stderr, "\n\n\n\n\n\n不可修复错误，等系统重启\n\n\n\n\n\n");
                }
                else
                {
                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$完全不可能出现的状态\n\n\n\n\n\n");
                }

                las_parkstatus = PCL_PARKING_REQUEST;
            }
            else
            {
				if (PCL_PARKING_ERROR != las_parkstatus)
                {
                    LOG(ERROR)<<("\t严重警告:PLC我要停车状态异常，不可修复错误，等系统重启");
					//停止所有激光
					if (m_callback)
					{
						char laser = 0xFF;
						m_callback(false, uint16_t(laser), m_pointer, parking_status);
					}
                    write_laserstatus_register(PLC_LASER_READY);
					finite_state_machines = PLC_LASER_READY;
                }
                Sleep(200);
				las_parkstatus = PCL_PARKING_ERROR;
            }
            Sleep(50);
        }
    }
}