#include "dualcamwrapper.h"

DualcamWrapper::DualcamWrapper(const CameraLayout &layout): valid(false), withLayout(true) {
  qDebug() << "Constructor with layout";
  this->layout = layout;
  Pylon::PylonInitialize();
}


DualcamWrapper::~DualcamWrapper() {
  qDebug() << "wrapper destructor";
  for (int i =0; i < cameras.GetSize(); ++i) {
    if (cameras[i].IsOpen()) {
        cameras[i].Close();
        qDebug() << "Camera " << i << " is open, closing it!";
    }
  }
  terminate();
  qDebug() << "Successfully deleted cameras";
}


void DualcamWrapper::terminate() {
  qDebug() << "Terminate";
  try {
    Pylon::PylonTerminate();
  } catch (const Pylon::GenericException &e) {
    std::cerr << e.GetDescription() << std::endl;
  }
}


bool DualcamWrapper::isOpen() {
  return cameras.IsOpen();
}


double DualcamWrapper::maxFrameRate(int camindex) {
    assert(camindex >= 0 && camindex < 2);
    double max_rate = -1;
    if (valid) {
        GenApi::INodeMap& nodemap = getNodemap(camindex);
        GenApi::INode* n = nodemap.GetNode( "AcquisitionFrameRate" );
        Pylon::CFloatParameter framerate( n );
        return framerate.GetMax();
    }
  return max_rate;
}


bool DualcamWrapper::frameRate(uint new_framerate, int camindex) {
  qDebug() << "Setting FrameRate to " << new_framerate << " for camera " << camindex;
  if (valid) {
      if (camindex == -1) {
        frameRate(new_framerate, 0);
        frameRate(new_framerate, 1);
        return true;
      } else if (camindex >= 0 && camindex < 2) {
        GenApi::INodeMap& nodemap = getNodemap(camindex);
        GenApi::INode* n = nodemap.GetNode( "AcquisitionFrameRateEnable" );
        Pylon::CBooleanParameter enableframerate(n);
        enableframerate.SetValue(true);
        n = nodemap.GetNode( "AcquisitionFrameRate" );
        Pylon::CFloatParameter framerate( n );
        framerate.SetValue( new_framerate );
        return true;
      } else {
        return false;
      }
    }
  return false;
}


double DualcamWrapper::frameRate(int camindex) {
  qDebug() << "Reading FrameRate from camera " << camindex;
  assert(camindex >= 0 && camindex < 2);
  double rate = -1.;
  if (valid) {
      GenApi::INodeMap& nodemap = getNodemap(camindex);
      GenApi::INode* n = nodemap.GetNode( "AcquisitionFrameRate" );
      Pylon::CFloatParameter framerate( n );
      rate = framerate.GetValue();
    }
  return rate;
}


double DualcamWrapper::exposureTime(int camindex) {
  qDebug() << "Reading ExposureTime from camera " << camindex;
  assert(camindex > 0 && camindex < 2);
  double time = -1.;
  if (valid) {
      GenApi::INodeMap& nodemap = getNodemap(camindex);
      GenApi::INode* n = nodemap.GetNode( "ExposureTime" );
      Pylon::CFloatParameter exposure_time( n );
      time = exposure_time.GetValue();
    }
  return time;
}


bool DualcamWrapper::exposureTime(double exposure_time, int camindex) {
  qDebug() << "Setting exposure time to " << exposure_time << " for camera " << camindex;
    if (valid) {
        if (camindex == -1) {
            exposureTime(exposure_time, 0);
            exposureTime(exposure_time, 1);
        } else {
            try {
                GenApi::INodeMap& nodemap = getNodemap(camindex);
                GenApi::INode* n = nodemap.GetNode( "ExposureTime" );
                Pylon::CFloatParameter exp_time( n );
                exp_time.SetValue( exposure_time );
                GenApi::CEnumerationPtr(nodemap.GetNode( "ExposureTimeMode" ))->FromString("Timed");;
            } catch (...) {

                qWarning() << "Could not set exposure for cam " << camindex;
            }
        }
      }
    return false;
}

uint32_t DualcamWrapper::sensorWidth(int camindex) {
  qDebug() << "Reading SensorWidth from camera " << camindex;
  assert(camindex >= 0 && camindex < 2);
  uint32_t width = -1;
  if (valid) {
    GenApi::INodeMap &nodemap = getNodemap(camindex);
    Pylon::CIntegerParameter pwidth( nodemap, "SensorWidth" );
    width = (uint32_t)pwidth.GetValue();
  }
  return width;
}

uint32_t DualcamWrapper::sensorHeight(int camindex) {
  qDebug() << "Reading SensorHeight from camera " << camindex;
  assert(camindex >= 0 && camindex < 2);
  uint32_t height = -1;
  if (valid) {
    GenApi::INodeMap &nodemap = getNodemap(camindex);
    Pylon::CIntegerParameter pheight( nodemap, "SensorHeight" );
    height = (uint32_t)pheight.GetValue();
  }
  return height;
}

double DualcamWrapper::gain(int camindex) {
  qDebug() << "Reading Gain from camera " << camindex;
  assert(camindex >= 0 && camindex < 2);
  double gain = -1.;
  if (valid) {
      GenApi::INodeMap& nodemap = getNodemap(camindex);
      GenApi::INode* n = nodemap.GetNode( "Gain" );
      Pylon::CFloatParameter detector_gain( n );
      gain = detector_gain.GetValue();
    }
  return gain;
}


bool DualcamWrapper::gain(double gain_db, int camindex) {
    if (valid) {
        if (camindex == -1) {
            gain(gain_db, 0);
            gain(gain_db, 1);
        } else if (camindex >= 0 && camindex < 2) {
            GenApi::INodeMap& nodemap = getNodemap(camindex);
            GenApi::CFloatPtr(nodemap.GetNode("Gain"))->SetValue(gain_db);
            return true;
        } else{
            return false;
        }
    }
    return false;
}


ImageSettings DualcamWrapper::getImageSettings(int camindex) {
  ImageSettings settings;
  if (valid) {
    GenApi::INodeMap &nodemap = getNodemap(camindex);
    Pylon::CEnumParameter pixelFormat( nodemap, "PixelFormat" );
    Pylon::CPixelTypeMapper pixelTypeMapper( &pixelFormat );
    Pylon::EPixelType pixelType = pixelTypeMapper.GetPylonPixelTypeFromNodeValue( pixelFormat.GetIntValue() );
    Pylon::CIntegerParameter width( nodemap, "Width" );
    Pylon::CIntegerParameter height( nodemap, "Height" );
    settings.pixelType = pixelType;
    settings.width = (uint32_t)width.GetValue();
    settings.height = (uint32_t)height.GetValue();
    settings.orientation = Pylon::EImageOrientation::ImageOrientation_TopDown;
  }
  return settings;
}


bool DualcamWrapper::grabFrame(MyImage &img, int camindex) {
  Pylon::CGrabResultPtr frame;
  Pylon::CInstantCamera* camera;
  qDebug() << "grabFrame from camera " << camindex;
  if (valid) {
    GenApi::INodeMap &nodemap = getNodemap(camindex);
    qDebug() << "Setting width" << layout.rois[camindex].width;
    Pylon::CIntegerParameter(nodemap, "Width").SetValue(layout.rois[camindex].width);
    qDebug() << "Setting height" << layout.rois[0].height;
    Pylon::CIntegerParameter(nodemap, "Height").SetValue(layout.rois[camindex].height);
    qDebug() << "Setting xoffset" << layout.rois[camindex].x;
    Pylon::CIntegerParameter(nodemap, "OffsetX").SetValue(layout.rois[camindex].x);
    qDebug() << "Setting yoffset" << layout.rois[camindex].y;
    Pylon::CIntegerParameter(nodemap, "OffsetY").SetValue(layout.rois[camindex].y);

    camera->StartGrabbing();
    camera->RetrieveResult( 5000, frame, Pylon::TimeoutHandling_ThrowException);
    camera->StopGrabbing();
  }
  img.setFrame(frame);
  return frame.IsValid();
}


void DualcamWrapper::setROI() {
  for (int camindex = 0; camindex < 2; camindex++){
    qDebug() << "Setting ROI: w" << layout.rois[camindex].width << " h: "<< layout.rois[camindex].height << " x " << layout.rois[camindex].x << " y " << layout.rois[camindex].y;
    try {
    GenApi::INodeMap &nodemap = getNodemap(camindex);
    Pylon::CIntegerParameter(nodemap, "Width").SetValue(layout.rois[camindex].width);
    Pylon::CIntegerParameter(nodemap, "Height").SetValue(layout.rois[camindex].height);
    Pylon::CIntegerParameter(nodemap, "OffsetX").SetValue(layout.rois[camindex].x);
    Pylon::CIntegerParameter(nodemap, "OffsetY").SetValue(layout.rois[camindex].y);
    } catch (const Pylon::GenericException &e) {
    std::cerr << e.GetDescription() << std::endl;
    }
  }
}


void DualcamWrapper::resetCamera(int camindex) {
  GenApi::INodeMap &nodemap = getNodemap( camindex );
  uint32_t width = sensorWidth(camindex);
  uint32_t height = sensorHeight(camindex);
  qDebug() << "resetting camera to default ROI (" << width << ", " << height << ")";
    try {
      Pylon::CIntegerParameter(nodemap, "Width").SetValue(width, false);
      Pylon::CIntegerParameter(nodemap, "Height").SetValue(height, false);
      Pylon::CIntegerParameter(nodemap, "OffsetX").SetValue(0);
      Pylon::CIntegerParameter(nodemap, "OffsetY").SetValue(0);
    } catch (const Pylon::GenericException &e) {
      std::string message = e.GetDescription();
      std::cerr << "An exception occurred." << std::endl << e.GetDescription() << std::endl;
      valid = false;
    }
}


bool DualcamWrapper::openCameras(std::string &message) {
    qDebug() << "opening cameras";
    Pylon::CTlFactory& tlFactory = Pylon::CTlFactory::GetInstance();
    cameras.Initialize(2);
    try {
        cameras[0].Attach( tlFactory.CreateDevice( layout.devices[0].c_str() ) );
        cameras[0].Open();
        cameras[1].Attach( tlFactory.CreateDevice( layout.devices[1].c_str() ) );
        cameras[1].Open();
        valid = cameras[0].IsOpen();
        valid = valid & cameras[1].IsOpen();
        message = "Successfully opened camera!";
    } catch (const Pylon::GenericException &e) {
        message = e.GetDescription();
        std::cerr << "An exception occurred." << std::endl << e.GetDescription() << std::endl;
        valid = false;
        return valid;
    }
    // resetCamera(0);
    // resetCamera(1);
    setROI();
    return valid;
}


void DualcamWrapper::closeCameras() {
  qDebug() << "Close cameras!";
  if (cameras[0].IsOpen()) {
    cameras[0].Close();
  }
  if (cameras[1].IsOpen()) {
    cameras[1].Close();
  }
  valid = false;
}

Pylon::CInstantCameraArray &DualcamWrapper::getCameraArray() {
  return cameras;
}

// Pylon::CInstantCamera DualcamWrapper::getCamera(int camindex) {
//     return this.cameras[camindex];
// }


GenApi::INodeMap& DualcamWrapper::getNodemap(int camindex){
  GenApi::INodeMap &nodemap = cameras[camindex].GetNodeMap();
  return nodemap;
}

void DualcamWrapper::enableSoftwareTrigger(int camindex){
  qDebug() << "Enabling software trigger for camera " << camindex;
  GenApi::INodeMap &nodemap = getNodemap( camindex );
  Pylon::CEnumParameter(nodemap, "TriggerMode").SetValue("On");
  Pylon::CEnumParameter(nodemap, "TriggerSource").SetValue("Software");
  // Pylon::CEnumParameter(nodemap, "TriggerActivation").SetValue("LevelHigh");
}

void DualcamWrapper::disableSoftwareTrigger(int camindex){
  qDebug() << "Disabling software trigger for camera " << camindex;
  GenApi::INodeMap& nodemap = getNodemap(camindex);
  Pylon::CEnumParameter(nodemap, "TriggerMode").SetValue("Off");
}

bool DualcamWrapper::softwareTriggerEnabeled(int camindex){
  qDebug() << "Checking software trigger for camera " << camindex;
  GenApi::INodeMap& nodemap = getNodemap(camindex);
  return Pylon::CEnumParameter(nodemap, "TriggerMode").GetValue() == "On";
}