#include "dualcamwrapper.h"

DualcamWrapper::DualcamWrapper(const CameraLayout &layout): valid(false), withLayout(true), camera0(nullptr), camera1(nullptr), cameras(2, nullptr) {
  qDebug() << "Constructor with layout";
  this->fullName = layout.devices[0];
  this->layout = layout;
  Pylon::PylonInitialize();
}


DualcamWrapper::~DualcamWrapper() {
  qDebug() << "wrapper destructor";
  if (camera0 != nullptr){
    if (camera0->IsOpen()) {
        qDebug() << "Camera 0 is open, closing it!";
    camera0->Close();
    }
    delete camera0;
    camera0 = nullptr;
  }
  if (camera1 != nullptr){
    if (camera1->IsOpen()) {
        qDebug() << "Camera 1 is open, closing it!";
    camera1->Close();
    }
    delete camera1;
    camera1 = nullptr;
  }
  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 valid;
}


double DualcamWrapper::maxFrameRate() {
  double max_rate = -1;
  // FIXME max framerate of both cameras!
  if (valid) {
     GenApi::INodeMap& nodemap = camera0->GetNodeMap();
     GenApi::INode* n = nodemap.GetNode( "AcquisitionFrameRate" );
     Pylon::CFloatParameter framerate( n );
     return framerate.GetMax();
  }
  return max_rate;
}


bool DualcamWrapper::frameRate(uint new_framerate) {
  if (valid) {
      GenApi::INodeMap& nodemap = camera0->GetNodeMap();
      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 );

      nodemap = camera1->GetNodeMap();
      n = nodemap.GetNode( "AcquisitionFrameRateEnable" );
      Pylon::CBooleanParameter enableframerate1(n);
      enableframerate1.SetValue(true);

      n = nodemap.GetNode( "AcquisitionFrameRate" );
      Pylon::CFloatParameter framerate1( n );
      framerate1.SetValue( new_framerate );
      return true;
    }
  return false;
}


double DualcamWrapper::frameRate() {
  double rate = -1.;
  //FIXME read framerate setting from both cameras
  if (valid) {
      GenApi::INodeMap& nodemap = camera0->GetNodeMap();
      GenApi::INode* n = nodemap.GetNode( "AcquisitionFrameRate" );
      Pylon::CFloatParameter framerate( n );
      rate = framerate.GetValue();
    }
  return rate;
}


double DualcamWrapper::exposureTime() {
  //FIXME exposure setting from both cameras
  double time = -1.;
  if (valid) {
      GenApi::INodeMap& nodemap = camera0->GetNodeMap();
      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) {
    // FIXME support setting exposure individually 
    if (valid) {
        GenApi::INodeMap& nodemap = camera0->GetNodeMap();
        double d = GenApi::CFloatPtr(nodemap.GetNode("ExposureTime"))->GetValue();
        GenApi::INode* n = nodemap.GetNode( "ExposureTime" );
        try {
            GenApi::CEnumerationPtr(nodemap.GetNode( "ExposureTimeMode" ))->FromString("Standard");
        } catch (...) {
            qWarning() << "Could not set exposure for cam0"; 
        }
        Pylon::CFloatParameter exp_time( n );
        exp_time.SetValue( exposure_time );

        nodemap = camera1->GetNodeMap();
        d = GenApi::CFloatPtr(nodemap.GetNode("ExposureTime"))->GetValue();
        n = nodemap.GetNode( "ExposureTime" );
        try {
            GenApi::CEnumerationPtr(nodemap.GetNode( "ExposureTimeMode" ))->FromString("Standard");
        } catch (...) {
            qWarning() << "Could not set exposure for cam1";
        }
        Pylon::CFloatParameter exp_time1( n );
        exp_time1.SetValue( exposure_time );
        return true;
      }
    return false;
}


double DualcamWrapper::gain() {
  double gain = -1.;
  if (valid) {
      GenApi::INodeMap& nodemap = camera0->GetNodeMap();
      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) {
        GenApi::INodeMap& nodemap = camera0->GetNodeMap();
        GenApi::CFloatPtr(nodemap.GetNode("Gain"))->SetValue(gain_db);

        nodemap = camera1->GetNodeMap();
        GenApi::CFloatPtr(nodemap.GetNode("Gain"))->SetValue(gain_db);
        return true;
      }
    return false;
}


ImageSettings DualcamWrapper::getImageSettings(int camindex) {
  ImageSettings settings;
  if (valid) {
    GenApi::INodeMap &nodemap = getNodemap(camindex);
    //   GenApi::INodeMap& nodemap;
    //   if (camindex == 0) {
    //     nodemap = camera0->GetNodeMap();
    //   } else {
    //     nodemap = camera1->GetNodeMap();
    //   }
      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);
    // GenApi::INodeMap& nodemap;
    // if (camindex == 0) {
    //     nodemap = camera0->GetNodeMap();
    //     camera = camera0;
    // } else {
    //     nodemap = camera1->GetNodeMap();
    //     camera = camera1;
    // }
    qDebug() << "Setting width" << layout.rois[0].width;
    Pylon::CIntegerParameter(nodemap, "Width").SetValue(layout.rois[0].width);
    qDebug() << "Setting height" << layout.rois[0].height;
    Pylon::CIntegerParameter(nodemap, "Height").SetValue(layout.rois[0].height);
    qDebug() << "Setting xoffset" << layout.rois[0].x;
    Pylon::CIntegerParameter(nodemap, "OffsetX").SetValue(layout.rois[0].x);
    qDebug() << "Setting yoffset" << layout.rois[0].y;
    Pylon::CIntegerParameter(nodemap, "OffsetY").SetValue(layout.rois[0].y);

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


void DualcamWrapper::resetCamera(int camindex) {
    GenApi::INodeMap &nodemap = getNodemap(camindex);
    // ->GetNodeMap();;
    // if (camindex == 1){
    //     nodemap = camera1->GetNodeMap();
    // }
    int64_t dfltWidth = 2048;
    int64_t dfltHeight = 1536;
  qDebug() << "resetting camera to default ROI (" << dfltWidth << ", " << dfltHeight << ")";
  try {
    // std::cerr << "MaxWidth: " << Pylon::CIntegerParameter(nodemap, "WidthMax").GetValue() << std::endl;
    Pylon::CIntegerParameter(nodemap, "Width").SetValue(dfltWidth, false);
    // std::cerr << "MaxHeight: " << Pylon::CIntegerParameter(nodemap, "HeightMax").GetValue() << std::endl;
    Pylon::CIntegerParameter(nodemap, "Height").SetValue(dfltHeight, 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";
    bool valid = true;
    try {
        camera0 = new Pylon::CInstantCamera();
        Pylon::String_t fname(layout.devices[0].c_str());
        Pylon::IPylonDevice *pDevice = Pylon::CTlFactory::GetInstance().CreateDevice(fname);
        camera0->Attach(pDevice);
        camera0->Open();
        valid = camera0->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);

    try {
        camera1 = new Pylon::CInstantCamera();
        Pylon::String_t fname(layout.devices[1].c_str());
        Pylon::IPylonDevice *pDevice = Pylon::CTlFactory::GetInstance().CreateDevice(fname);
        camera1->Attach(pDevice);
        camera1->Open();
        valid = valid & camera1->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(1);

    return valid;
}


void DualcamWrapper::closeCameras() {
  qDebug() << "Close cameras!";
  if (camera0->IsOpen()) {
      try {
        camera0->Close();
        valid = false;
        delete camera0;
        camera0 = nullptr;
      } catch (const Pylon::GenericException &e) {
        qWarning() << "An exception occurred." << e.GetDescription();
      }
    }
    if (camera1->IsOpen()) {
      try {
        camera1->Close();
        valid = false;
        delete camera1;
        camera1 = nullptr;
      } catch (const Pylon::GenericException &e) {
        qWarning() << "An exception occurred." << e.GetDescription();
      }
    }
}


Pylon::CInstantCamera *DualcamWrapper::getCamera(int camindex) {
  if (camindex == 0) {
    return camera0;
  } else {
    return camera1;
  }
}


GenApi::INodeMap& DualcamWrapper::getNodemap(int camindex){
    GenApi::INodeMap &nodemap = camera0->GetNodeMap();;
    if (camindex == 1){
        nodemap = camera1->GetNodeMap();
    }
    return nodemap;
}