#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) {
  if (valid) {
      if (camindex == -1) {
        frameRate(new_framerate, 0);
        frameRate(new_framerate, 1);
        return true;
      } else if (camindex == 0 && camindex ==1) {
        GenApi::INodeMap& nodemap = getNodemap(0);
        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) {
  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) {
  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) {
    if (valid) {
        if (camindex == -1) {
            exposureTime(exposure_time, 0);
            exposureTime(exposure_time, 1);
        } else {
            GenApi::INodeMap& nodemap = getNodemap(camindex);
            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 );
        }
      }
    return false;
}


double DualcamWrapper::gain(int 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[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 );
    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";
    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);
    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;
}