#include "camera.h"

using namespace interfaceBALLUFF;
    std::vector<camInfo> availableCameras(){
	    mvIMPACT::acquire::DeviceManager devmg_;
    	std::vector<camInfo> infovec;
    	unsigned int count = devmg_.deviceCount();

    	for(unsigned int i=0;i<count;i++){
    		camInfo actual;
    		mvIMPACT::acquire::Device* dev_ =devmg_.getDevice(i);
    		mvIMPACT::acquire::GenICam::ImageFormatControl imgform_(dev_);
    		actual.id = dev_->deviceID.read();
    		actual.serial = dev_->serial.readS();
    		actual.name = dev_->product.readS();
    		actual.ver = dev_->deviceVersion.readS();
    		std::stringstream ss;
    		ss<< imgform_.heightMax.read()<<"x"<<imgform_.widthMax.read();
    		actual.resolution = ss.str();
    		std::cout<<" "<<actual.id<<" "<<actual.serial<<" "<<actual.name<<" "<<actual.ver<<" "<<actual.resolution<<" "<<std::endl;
    		infovec.push_back(actual);
    		dev_->close();
    	}
    	return infovec;
    } 
      
      void Camera::init(const std::string& settings) {
        if(!_pDev) {
          std::cout<<"Camera got invalid device pointer"<<std::endl;
          throw std::runtime_error("Camera got invalid device pointer");
        }
        if(this->_pDev->isInUse())
          std::cout<<this->_pDev->serial.readS()+": Device in use"<<std::endl;
        try{
          this->_pDev->open();
          funcintf_ = std::make_unique<mvIMPACT::acquire::FunctionInterface>(this->_pDev);
          imgdest_ = std::make_unique<mvIMPACT::acquire::ImageDestination>(this->_pDev);
          acqC_ = std::make_unique<mvIMPACT::acquire::GenICam::AcquisitionControl>(this->_pDev);
          formCont_ = std::make_unique<mvIMPACT::acquire::GenICam::ImageFormatControl>(this->_pDev);
          if(settings!="") {
            int res_set = funcintf_->loadSetting(settings);
            if( res_set != DMR_NO_ERROR )
              std::cout<<this->_pDev->serial.readS()+": Setting at: "+settings+" was not founded!"<<std::endl;
            else
              std::cout<<this->_pDev->serial.readS()+": Setting from *.xml file was setted"<<std::endl;
          }
          // Setting = std::make_unique<Sett4CAM>(this->_pDev, log_mode, log_path);
          std::cout<<this->_pDev->serial.readS()+": Device opened"<<std::endl;

          //turn off auto modes
          acqC_->exposureAuto.writeS("Off");

        }
        catch( const ImpactAcquireException& e )
        {
            std::cerr << "An error occurred while opening the device " << this->_pDev->serial.read()
                << "(error code: " << e.getErrorCodeAsString() << ")." << std::endl;
        }
      } 


      Camera::Camera(unsigned int numID, const std::string& settings)
      : _lastTime(Clock::now())
        {
        try {
            this->devMan = new mvIMPACT::acquire::DeviceManager;
            this->_pDev = this->devMan->getDevice(numID);
            this->init(settings);
            conditionalSetProperty( _pDev->interfaceLayout, mvIMPACT::acquire::dilGenICam);
        } catch (...) {
            throw std::runtime_error("Camera is NOT connected");
        }
      }

      Camera::Camera(std::string serial, const std::string& settings)
      : _lastTime(Clock::now())
       {
       try {
            this->devMan = new mvIMPACT::acquire::DeviceManager;
            this->_pDev = this->devMan->getDeviceBySerial(serial);
            this->init(settings);
            conditionalSetProperty( _pDev->interfaceLayout, mvIMPACT::acquire::dilGenICam);
       } catch (...) {
            throw std::runtime_error("Camera is NOT connected");
       }
      }

      void Camera::set_idPf(mvIMPACT::acquire::TImageDestinationPixelFormat idpf){
        imgdest_->pixelFormat.write(idpf);
      }

      void Camera::set_idPf(const std::string idpf){
        imgdest_->pixelFormat.writeS(idpf);
      }

      std::string Camera::get_idPf(){
        std::cout<<"format is : "<<imgdest_->pixelFormat.readS()<<std::endl;
        return imgdest_->pixelFormat.readS();
      }

      std::string Camera::get_Name(){
          return this->_pDev->serial.readS();
      }

      uint8_t Camera::get_ID(){
          return this->_pDev->deviceID.read();
      }

      int Camera::get_Format_px(){
          std::string format = this->get_idPf();
          if(format == "Mono16"){
              return CV_16UC1;
          }else if(format == "Mono8"){
              return CV_8UC1;
          }else if(format == "RGB888Packed"){
              return CV_8UC3;
          }else if(format == "RGB161616Packed"){
              return CV_16UC3;
          }
          return -1;
      }

      void Camera::set_Exposure_us(double duration){
          acqC_->exposureTime.write(duration);
      }

      double Camera::get_Exposure_us(){
          return acqC_->exposureTime.read();
      }

      camera::frame Camera::createImage(int imTYPE, mvIMPACT::acquire::Request* pRequest){
        start_time = std::chrono::high_resolution_clock::now();
        camera::frame image;
        if(pRequest->isOK()){
          const int width = pRequest->imageWidth.read();
          const int height = pRequest->imageHeight.read();
          image.timestamp_us = pRequest->infoTimeStamp_us.read();
          image.id = pRequest->infoFrameID.read();
          // image.image = { height, width, imTYPE, reinterpret_cast< unsigned char*>( pRequest->imageData.read()) };
          cv::Mat pic(height, width, imTYPE, reinterpret_cast< unsigned char*>( pRequest->imageData.read()) );
          pic.copyTo(image.image);
          end_time = std::chrono::high_resolution_clock::now();
          std::chrono::duration<double> tim = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
          image.acquisition_time = tim.count();
          std::cout<<this->_pDev->serial.readS()+": Single sw frame "+ std::to_string(image.timestamp_us)+(" captured")<<std::endl;
        }else{
          std::cout<<this->_pDev->serial.readS()+": Not valid request pointer code : "+pRequest->requestResult.readS()<<std::endl;
        }
        return image;
      }

      mvIMPACT::acquire::Device* Camera::get(){
        return this->_pDev;
      }

      bool Camera::isConnected(){
        return this->connected;
      }
      
      void Camera::reset(){
        std::cout<<this->_pDev->serial.readS()+": Function interface of device reseted"<<std::endl;
        int a = funcintf_->imageRequestReset(0,0);
        if(a != DMR_NO_ERROR){
          std::cout<<this->_pDev->serial.readS()+": Failed to reset device with code "+std::to_string(a)<<std::endl;
        }
        if(_lastReq) _lastReq->unlock();
        if(_prevReq) _prevReq->unlock();
      }

      void Camera::close(){
        if(_pDev->isOpen()){
          try{
            funcintf_->imageRequestReset( 0, 0 );
            _pDev->close();
          }     
          catch( const ImpactAcquireException& e ){
            std::cout<<this->_pDev->serial.readS()+": Device not possible to close with err code: "+e.getErrorCodeAsString()<<std::endl;
            return;
          }
         std::cout<<this->_pDev->serial.readS()+": Device closed"<<std::endl;
        }
      }

      void Camera::forceClose(){
          _pDev->close();
      }

      bool Camera::loadConfig(const std::string& path2XML){
          mvIMPACT::acquire::FunctionInterface funcintf(this->_pDev);
          int res_set = funcintf.loadSetting(path2XML);
          if( res_set != DMR_NO_ERROR )
            return false;
          else
            return true;
      }
     
      /* Read Image by default setting or setting from "setting file", not with external trigger */
      camera::frame Camera::readImage(const int imTYPE, int timeout_ms, bool unlocking){
        funcintf_->imageRequestSingle();
        if(_lastReq && unlocking){
          _lastReq->unlock();
        }
        std::cout<<this->_pDev->serial.readS()+": make req"<<std::endl;
        int requestNr = funcintf_->imageRequestWaitFor( timeout_ms ); //modify here - time which will be camera waiting for trigger (-1 = unlimited)
        std::cout<<std::string(this->_pDev->serial.readS()+": req received")<<std::endl;
        camera::frame res;
        if(funcintf_->isRequestNrValid(requestNr)){
          std::cout<<this->_pDev->serial.readS()+": req "+std::to_string(requestNr)+" is valid"<<std::endl;
          mvIMPACT::acquire::Request* pRequest = funcintf_->getRequest(requestNr);
          _lastReq = pRequest;
          res = createImage(imTYPE, pRequest);
        }else{
          std::cout<<this->_pDev->serial.readS()+": Not valid request NR -> possibly long waited"<<std::endl;
        }
        return res;
      }

      void Camera::reduceResolution(int ratio){
          this->formCont_->decimationVertical.write(ratio);
          this->formCont_->decimationHorizontal.write(ratio);
      }

