#include "Camera_Basler_GigE.h"
#include <QDebug>
#include <QTime>

#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#define min(a,b) (a<b ? a : b)

// Constructor allocates the image buffer
//CGrabBuffer::CGrabBuffer(const size_t ImageSize):
//        m_pBuffer(NULL)
//{
//    m_pBuffer = new uint8_t[ ImageSize ];
//    if (NULL == m_pBuffer)
//    {
//        GenICam::GenericException e("Not enough memory to allocate image buffer", __FILE__, __LINE__);
//        throw e;
//    }
//}
//
//// Freeing the memory
//CGrabBuffer::~CGrabBuffer()
//{
//    if (NULL != m_pBuffer)
//        delete[] m_pBuffer;
//}

Pylon::PylonAutoInitTerm Camera_Basler_GigE::autoInitTerm;
CTlFactory& Camera_Basler_GigE::TlFactory = CTlFactory::GetInstance();
ITransportLayer *Camera_Basler_GigE::pTl = TlFactory.CreateTl(Camera_t::DeviceClass());
DeviceInfoList_t Camera_Basler_GigE::devices;
int Camera_Basler_GigE::started=0;
int Camera_Basler_GigE::connect(int camera_id)
{
	try
	{
		if(camera_id >= (int)devices.size())
		{
			return 2;
		}

		deviceInfo = devices[ camera_id ];

		//CInstantCamera p= new CInstantCamera(	
		pCamera = new Camera_t(pTl->CreateDevice(deviceInfo));
		pCamera->Open();
        setXmlID(QString(deviceInfo.GetSerialNumber()).toInt());
		//pStreamGrabber = new Camera_t::StreamGrabber_t(pCamera->GetStreamGrabber(0));
		//pStreamGrabber->Open();
		
		//pStreamGrabber->Close();
		pCamera->Close();
		pCamera->DestroyDevice();
		delete pCamera;
        pCamera = NULL;
		//pTl->DestroyDevice(pCamera->GetDevice());
		
		this->writeInfo(QString("Camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> connected successfully."));
		//emit connected(camera_id);
	}
    catch (GenICam::GenericException &e)
    {
		this->writeInfo(QString("Connection to camera ") + QString(deviceInfo.GetFullName().c_str()) + QString(" failed."));
        return 1;
    }
	
	return 0;
}

int Camera_Basler_GigE::start()
{
	
	try
	{
		pCamera = new Camera_t(pTl->CreateDevice(deviceInfo));
		GenApi::INodeMap& nodemap = pCamera->GetNodeMap();


		pCamera->Open();

		
		pCamera->RegisterConfiguration( new CSoftwareTriggerConfiguration, RegistrationMode_ReplaceAll, Cleanup_Delete);

		GenApi::CEnumerationPtr pixelFormat( nodemap.GetNode( "PixelFormat"));

        if(cameraColor() == CameraColor_Mono) {
            //	pCamera->PixelFormat.SetValue(PixelFormat_Mono8);
            if ( IsAvailable( pixelFormat->GetEntryByName( "Mono8"))) {
                pixelFormat->FromString( "Mono8");
                qDebug() << "New PixelFormat  : " << pixelFormat->ToString() << endl;
            }
            
            
            
        }
        if(cameraColor() == CameraColor_BGR) {
            //	pCamera->PixelFormat.SetValue(PixelFormat_YUV422Packed);
            if ( IsAvailable( pixelFormat->GetEntryByName( "YUV422Packed"))) {
                pixelFormat->FromString( "YUV422Packed");
                qDebug() << "New PixelFormat  : " << pixelFormat->ToString() << endl;
            }
        }
		//pCamera->GevStreamChannelSelector.SetValue(GevStreamChannelSelector_StreamChannel0);
		//pCamera->GevSCPSPacketSize.SetValue(4096);

		//pCamera->GevStreamChannelSelector.SetValue(GevStreamChannelSelector_StreamChannel0);
		//pCamera->GevSCFTD.SetValue(rand()%200);

		//pCamera->OffsetX.SetValue(0);
		//pCamera->OffsetY.SetValue(0);
		//pCamera->Width.SetValue(pCamera->Width.GetMax());
		//pCamera->Height.SetValue(pCamera->Height.GetMax());

        GenApi::CIntegerPtr offsetX( nodemap.GetNode( "OffsetX"));
        GenApi::CIntegerPtr offsetY( nodemap.GetNode( "OffsetY"));
        GenApi::CIntegerPtr width( nodemap.GetNode( "Width"));
        GenApi::CIntegerPtr height( nodemap.GetNode( "Height"));

        // On some cameras the offsets are read-only,
        // so we check whether we can write a value. Otherwise, we would get an exception.
        // GenApi has some convenience predicates to check this easily.
        if ( IsWritable( offsetX))
        {
			if(m_offsetx >= 0)
			//pCamera->OffsetX.SetValue(m_offsetx);
            offsetX->SetValue( m_offsetx);
        }
        if ( IsWritable( offsetY))
        {
			if(m_offsety >= 0)
		//	pCamera->OffsetY.SetValue(m_offsety);
            offsetY->SetValue( m_offsety);
        }
        
        // Some properties have restrictions. Use GetInc/GetMin/GetMax to make sure you set a valid value.
        //int64_t newWidth = 202;
        //newWidth = Adjust(newWidth, width->GetMin(), width->GetMax(), width->GetInc());

        //int64_t newHeight = 101;
        //newHeight = Adjust(newHeight, height->GetMin(), height->GetMax(), height->GetInc());

        width->SetValue(min(m_width,width->GetMax()));
        height->SetValue(min(m_height,height->GetMax()));


		
		
		
		if(m_offsetx < 0)
			pCamera->CenterX.SetValue(true);
		if(m_offsety < 0)
			pCamera->CenterY.SetValue(true);

		bool frameStartAvailable = false;
		bool acquisitionStartAvailable = false;
		{
			GenApi::IEnumEntry* frameStart = pCamera->TriggerSelector.GetEntry(TriggerSelector_FrameStart);
			frameStartAvailable = frameStart && GenApi::IsAvailable(frameStart);
			GenApi::IEnumEntry* acquisitionStart = pCamera->TriggerSelector.GetEntry(TriggerSelector_AcquisitionStart);
			acquisitionStartAvailable = acquisitionStart && GenApi::IsAvailable(acquisitionStart);
		}
		TriggerSelectorEnums triggerSelectorValue = TriggerSelector_FrameStart;
		if ( acquisitionStartAvailable && !frameStartAvailable)
		{
			qDebug()<<"ack_start available, frame start not";
			pCamera->TriggerSelector.SetValue(TriggerSelector_AcquisitionStart);
			pCamera->TriggerMode.SetValue(TriggerMode_On);
			triggerSelectorValue = TriggerSelector_AcquisitionStart;
		}
		else  
		{   
			if ( acquisitionStartAvailable )
			{
				qDebug()<<"ack_start available, frame start yes";
				pCamera->TriggerSelector.SetValue(TriggerSelector_AcquisitionStart);
				pCamera->TriggerMode.SetValue(TriggerMode_Off);
			}
			assert( frameStartAvailable);
			pCamera->TriggerSelector.SetValue(TriggerSelector_FrameStart);
			pCamera->TriggerMode.SetValue(TriggerMode_On);
		}

		pCamera->TriggerSelector.SetValue( triggerSelectorValue );
		pCamera->TriggerSource.SetValue( TriggerSource_Software);
		pCamera->AcquisitionMode.SetValue(AcquisitionMode_Continuous);
		pCamera->ExposureMode.SetValue(ExposureMode_Timed);
        
        setExposureTimeRaw(m_exposuretimeraw);
        setBalanceRatioRaw_red(m_balanceratioraw_red);
        setBalanceRatioRaw_green(m_balanceratioraw_green);
        setBalanceRatioRaw_blue(m_balanceratioraw_blue);

//		const size_t ImageSize = (size_t)(pCamera->PayloadSize.GetValue());
	//	pStreamGrabber = new Camera_t::StreamGrabber_t(pCamera->GetStreamGrabber(0));
	//	pStreamGrabber->Open();

	//	pStreamGrabber->MaxBufferSize.SetValue(ImageSize);
	//	pStreamGrabber->MaxNumBuffer.SetValue(c_nBuffers);

	//	pStreamGrabber->PrepareGrab();
/*
		for (uint32_t i = 0; i < c_nBuffers; ++i)
		{
			CGrabBuffer *pGrabBuffer = new CGrabBuffer(ImageSize);
			pGrabBuffer->SetBufferHandle(pStreamGrabber->RegisterBuffer(
											 pGrabBuffer->GetBufferPointer(), ImageSize));
			BufferList.push_back(pGrabBuffer);
		}

		for (std::vector<CGrabBuffer*>::const_iterator x = BufferList.begin(); x != BufferList.end(); ++x)
		{
			pStreamGrabber->QueueBuffer((*x)->GetBufferHandle(), NULL);
		}
*/
	//	pCamera->AcquisitionStart.Execute();
		pCamera->GevStreamChannelSelector.SetValue(GevStreamChannelSelector_StreamChannel0);
		pCamera->GevSCPD.SetValue(27054/18);
		pCamera->GevSCFTD.SetValue(60000+(started++)*9000);
		 pCamera->StartGrabbing( GrabStrategy_OneByOne);
		pCamera->AcquisitionStart.Execute();
		this->writeInfo(QString("Camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> started."));
	}
    catch (GenICam::GenericException &e)
    {
		this->writeInfo(QString("Starting camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> failed."));
		this->writeInfo(QString("Error: <strong>") + QString(e.GetDescription()));
		qDebug("%s", e.GetDescription());
        return 1;
    }
	return 0;
}

int Camera_Basler_GigE::sync()
{
	try
	{
		
		pCamera->TriggerSoftware.Execute();
		
		//qDebug()<<"Sync executed"<<cameraName<<QTime::currentTime();
	}
    catch (GenICam::GenericException &e)
    {
        return 1;
    }
	return 0;
}

int Camera_Basler_GigE::grab(cv::Mat* output)
{
	CGrabResultPtr ptrGrabResult;
	try
	{
		//qDebug()<<"grabing from"<<cameraName;
        // Wait for the grabbed image with a timeout of 1 seconds
		
        if (pCamera->GetGrabResultWaitObject().Wait( 1000))
        {
            // Get the grab result from the grabber's result queue
            GrabResult Result;
			//qDebug()<<"get Object OK"<<cameraName;
           // pStreamGrabber->RetrieveResult(Result);
			pCamera->RetrieveResult( 0, ptrGrabResult, TimeoutHandling_Return);

            if ( ptrGrabResult->GetWidth() > 0  && ptrGrabResult->GetHeight() > 0)
            {
                // Grabbing was successful, process image
				//qDebug()<<"grabbed OK"<<cameraName;
				resultX = ptrGrabResult->GetWidth();// GetSizeX();
				resultY = ptrGrabResult->GetHeight();

				if(cameraColor() == CameraColor_Mono)
				{
					if(resultBuffer == NULL)
						resultBuffer = new unsigned char[resultX*resultY*pixelSize];

					if(output != NULL)
					{
						memcpy(output->ptr(), (unsigned char*)ptrGrabResult->GetBuffer(), resultX*resultY*pixelSize);
					}
					else
					{
						memcpy(resultBuffer, (unsigned char*)ptrGrabResult->GetBuffer(), resultX*resultY*pixelSize);
					}
				}

				if(cameraColor() == CameraColor_BGR)
				{
					if(resultBuffer == NULL)
						resultBuffer = new unsigned char[resultX*resultY*pixelSize];
					
					if(resultBufferYUV == NULL)
						resultBufferYUV = new unsigned char[resultX*resultY*2];

					memcpy(resultBufferYUV, (unsigned char*)ptrGrabResult->GetBuffer(), resultX*resultY*2);
                    

					cv::Mat matYUV(resultY,resultX,CV_8UC2,resultBufferYUV);
					cv::Mat matBGR(resultY,resultX,CV_8UC3);

					if(output != NULL)
					{
						cv::cvtColor(matYUV,*output,CV_YUV2BGR_Y422, 3);
						//memcpy(output->ptr(), (unsigned char*)Result.Buffer(), resultX*resultY*pixelSize);
					}
					else
					{
						cv::cvtColor(matYUV,matBGR,CV_YUV2BGR_Y422, 3);
						memcpy(resultBuffer, matBGR.ptr(), resultX*resultY*pixelSize);
					}
				}

                // Reuse the buffer for grabbing the next image
             //   pStreamGrabber->QueueBuffer(Result.Handle(), NULL);
            }
            else if (Failed == Result.Status())
            {
                // Error handling

                // Reuse the buffer for grabbing the next image
              //  pStreamGrabber->QueueBuffer(Result.Handle(), NULL);
				this->writeInfo(QString("Grabbing image from camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> failed."));
				return 1;
			}
			else
			{
                // Error handling

                // Reuse the buffer for grabbing the next image
              //  pStreamGrabber->QueueBuffer(Result.Handle(), NULL);
				this->writeInfo(QString("Grabbing image from camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> failed."));
				return 1;
			}
				
        }
        else
        {
            // Timeout
          //  pStreamGrabber->CancelGrab();
			pCamera->StopGrabbing();
			//for (GrabResult r; pStreamGrabber->RetrieveResult(r););
			 while( pCamera->RetrieveResult( 0, ptrGrabResult, TimeoutHandling_Return))
        {
           qDebug() << "Skipped " << ptrGrabResult->GetNumberOfSkippedImages() << " images." << endl;
    //        nBuffersInQueue++;
        }
			this->writeInfo(QString("Camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> timeout."));
			return 1;
        }
	}
    catch (GenICam::GenericException &e)
    {
		this->writeInfo(QString("Grabbing image from camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> failed."));
		this->writeInfo(QString("Error: <strong>") + QString(e.GetDescription()));
		
        return 1;
    }
	return 0;
}

int Camera_Basler_GigE::stop()
{
	started--;
	CGrabResultPtr ptrGrabResult;
	try
	{
     //   pStreamGrabber->CancelGrab();

		pCamera->StopGrabbing();
		
		 while( pCamera->RetrieveResult( 0, ptrGrabResult, TimeoutHandling_Return))
        {
           qDebug() << "Skipped " << ptrGrabResult->GetNumberOfSkippedImages() << " images." << endl;
   //         nBuffersInQueue++;
        }
//		for (GrabResult r; pStreamGrabber->RetrieveResult(r););

//		pCamera->AcquisitionStop.Execute();

/*		for (std::vector<CGrabBuffer*>::iterator it = BufferList.begin(); it != BufferList.end(); it++)
		{
			pStreamGrabber->DeregisterBuffer((*it)->GetBufferHandle());
			delete *it;
		}

		BufferList.clear();*/

		//pStreamGrabber->FinishGrab();
	//	pStreamGrabber->Close();
		pCamera->Close();
		//pTl->DestroyDevice(pCamera->GetDevice());
		pCamera->DestroyDevice();

		if(resultBuffer != NULL)
		{
			delete[] resultBuffer;
			resultBuffer = NULL;
		}
					
		if(resultBufferYUV != NULL)
		{
			delete[] resultBufferYUV;
			resultBufferYUV = NULL;
		}

		this->writeInfo(QString("Camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> stopped."));
	}
    catch (GenICam::GenericException &e)
    {
		this->writeInfo(QString("Stopping camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> failed."));
		this->writeInfo(QString("Error: <strong>") + QString(e.GetDescription()));
		qDebug("%s", e.GetDescription());
        return 1;
    }
	return 0;
}

int Camera_Basler_GigE::disconnect()
{
	try
	{
		//delete pCamera;
		//delete pStreamGrabber;
		this->writeInfo(QString("Camera <strong>") + QString(deviceInfo.GetFullName().c_str()) + QString("</strong> disconnected."));
	}
    catch (GenICam::GenericException &e)
    {
        return 1;
    }
	return 0;
}
