/**Move the tool pos / axis pos to a new position given
 * by the input click position in vp space.
 */
void ViewWrapper2D::setAxisPos(Vector3D click_vp)
{
	ToolPtr tool = mServices->getToolManager()->getManualTool();

	Transform3D sMr = mSliceProxy->get_sMr();
	Transform3D rMpr = mServices->getPatientService()->get_rMpr();
	Transform3D prMt = tool->get_prMt();

	// find tool position in s
	Vector3D tool_t(0, 0, tool->getTooltipOffset());
	Vector3D tool_s = (sMr * rMpr * prMt).coord(tool_t);

	// find click position in s.
	Transform3D vpMs = mView->get_vpMs();
	Vector3D click_s = vpMs.inv().coord(click_vp);

	// compute the new tool position in slice space as a synthesis of the plane part of click and the z part of original.
	Vector3D cross_s(click_s[0], click_s[1], tool_s[2]);
	// compute the position change and transform to patient.
	Vector3D delta_s = cross_s - tool_s;
	Vector3D delta_pr = (rMpr.inv() * sMr.inv()).vector(delta_s);

	// MD is the actual tool movement in patient space, matrix form
	Transform3D MD = createTransformTranslate(delta_pr);
	// set new tool position to old modified by MD:
	tool->set_prMt(MD * prMt);
}

void ToolTipCalibrateWidget::calibrateSlot()
{
  ToolPtr refTool = mTools->getTool();
  //Todo, we only allow the reference point with id 1 to be used to calibrate
  //this could be done more dynamic.
  if(!refTool || !refTool->hasReferencePointWithId(1))
    return;

  ToolPtr tool = mServices->tracking()->getActiveTool();
  CoordinateSystem to = mServices->spaceProvider()->getT(tool);
  Vector3D P_t = mServices->spaceProvider()->getActiveToolTipPoint(to);

  ToolTipCalibrationCalculator calc(mServices->spaceProvider(), tool, refTool, P_t);
  Transform3D calibration = calc.get_calibration_sMt();

  QMessageBox msgBox;
  msgBox.setText("Do you want to overwrite "+tool->getName()+"s calibration file?");
  msgBox.setInformativeText("This cannot be undone.");
  msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);
  msgBox.setDefaultButton(QMessageBox::Ok);
  int ret = msgBox.exec();

  if(ret == QMessageBox::Ok)
  {
    tool->setCalibration_sMt(calibration);
    mCalibrationLabel->setText("Calibration:\n"+qstring_cast(calibration));
  }
}

/** extract information from the IGTLinkUSStatusMessage
 *  and store locally. Also reset the old local info with
 *  information from the probe in toolmanager.
 */
void VideoConnection::updateStatus(ProbeDefinitionPtr msg)
{
	ToolPtr tool = mBackend->tracking()->getFirstProbe();
	if (!tool || !tool->getProbe())
	{
		//Don't throw away the ProbeDefinition. Save it until it can be used
		if (mUnusedProbeDefinitionVector.empty())
			connect(mBackend->tracking().get(), &TrackingService::stateChanged, this, &VideoConnection::useUnusedProbeDefinitionSlot);
		mUnusedProbeDefinitionVector.push_back(msg);
		return;
	}
	ProbePtr probe = tool->getProbe();

	// start with getting a valid data object from the probe, in order to keep
	// existing values (such as temporal calibration).
	// Note that the 'active' data is get while the 'uid' data is set.
	ProbeDefinition data = probe->getProbeDefinition();

	data.setUid(msg->getUid());
	data.setType(msg->getType());
	data.setSector(msg->getDepthStart(), msg->getDepthEnd(), msg->getWidth());
	data.setOrigin_p(msg->getOrigin_p());
	data.setSize(msg->getSize());
	data.setSpacing(msg->getSpacing());
	data.setClipRect_p(msg->getClipRect_p());
	data.setUseDigitalVideo(true);

	probe->setProbeDefinition(data);
	probe->setActiveStream(msg->getUid());
}

ToolPtr Tool::getNullObject()
{
	static ToolPtr mNull;
	if (!mNull)
		mNull.reset(new ToolNull, null_deleter());
	return mNull;
}

void ToolTabsContainerWidget::CallOnActiveTools( ToolFunction& func )
{
    const ToolPtr activeTab = FindActiveTool();
    if ( activeTab != 0 )
    {
        activeTab->CallOnSelfAndActiveSubTools( func );
    }
}

void FastOrientationRegistrationWidget::enableToolSampleButtonSlot()
{
  ToolPtr tool = mServices->tracking()->getActiveTool();
  bool enabled = tool &&
	  tool->getVisible() &&
	  (!tool->hasType(Tool::TOOL_MANUAL) || settings()->value("giveManualToolPhysicalProperties").toBool()); // enable only for non-manual tools.

  mSetOrientationButton->setEnabled(enabled);
}

void ToolManualCalibrationWidget::matrixWidgetChanged()
{
	ToolPtr tool = mTool->getTool();
	if (!tool)
      return;

  Transform3D M = mMatrixWidget->getMatrix();
  tool->setCalibration_sMt(M);
}

QString StringPropertySelectTool::convertInternal2Display(QString internal)
{
  ToolPtr tool = mTrackingService->getTool(internal);
  if (!tool)
  {
	return "<no tool>";
  }
  return qstring_cast(tool->getName());
}

PlaybackTool::PlaybackTool(ToolPtr base, PlaybackTimePtr time) :
	ToolImpl(base->getUid(), "playback "+base->getName()), mBase(base),
    mTime(time),
    mVisible(false)
{
	connect(mTime.get(), SIGNAL(changed()), this, SLOT(timeChangedSlot()));

	connect(mBase.get(), SIGNAL(toolProbeSector()), this, SIGNAL(toolProbeSector()));
	connect(mBase.get(), SIGNAL(tooltipOffset(double)), this, SIGNAL(tooltipOffset(double)));
}

Vector3D ViewFollower::findVirtualTooltip_s()
{
	ToolPtr tool = mSliceProxy->getTool();
	Transform3D sMr = mSliceProxy->get_sMr();
	Transform3D rMpr = mDataManager->get_rMpr();
	Transform3D prMt = tool->get_prMt();
	Vector3D pt_s = sMr * rMpr * prMt.coord(Vector3D(0,0,tool->getTooltipOffset()));
	pt_s[2] = 0; // project into plane
	return pt_s;
}

void ToolManualCalibrationWidget::toolCalibrationChanged()
{
	ToolPtr tool = mTool->getTool();
  if (!tool)
    return;

  mMatrixWidget->blockSignals(true);
  mMatrixWidget->setMatrix(tool->getCalibration_sMt());
  mMatrixWidget->blockSignals(false);
}

void PatientLandMarksWidget::updateToolSampleButton()
{
	ToolPtr tool = mServices->tracking()->getActiveTool();

	bool enabled = tool && tool->getVisible() && (!tool->hasType(Tool::TOOL_MANUAL) || settings()->value("giveManualToolPhysicalProperties").toBool()); // enable only for non-manual tools.
	mToolSampleButton->setEnabled(enabled);

	if (mServices->tracking()->getActiveTool())
		mToolSampleButton->setText("Sample " + qstring_cast(tool->getName()));
	else
		mToolSampleButton->setText("No tool");
}

/** Imbue probe with all stream and probe info from grabber.
 *
 * Call when active probe is changed or when streaming config is changed (new streams, new probeDefinition)
 *
 * Find the active probe, then insert all current streams into that probe.
 *
 */
void VideoConnection::connectVideoToProbe()
{
	ToolPtr tool = mBackend->tracking()->getFirstProbe();
	if (!tool)
		return;

	ProbePtr probe = tool->getProbe();
	if (!probe)
		return;

	for (unsigned i=0; i<mSources.size(); ++i)
		probe->setRTSource(mSources[i]);
}

void VideoConnection::resetProbe()
{
	ToolPtr tool = mBackend->tracking()->getFirstProbe();
	if (!tool || !tool->getProbe())
		return;
	ProbePtr probe = tool->getProbe();
	if (probe)
	{
		ProbeDefinition data = probe->getProbeDefinition();
		data.setUseDigitalVideo(false);
		probe->setProbeDefinition(data);
	}
}

VideoSourcePtr VideoImplService::getGuessForActiveVideoSource(VideoSourcePtr old)
{

    if(old && old->getUid().contains("playback"))
        return old;

    QStringList nameFilters;
    nameFilters << "TissueAngio.fts" << "TissueFlow.fts" << "ScanConverted.fts";
    // ask for playback stream:
    foreach(USAcquisitionVideoPlaybackPtr uSAcquisitionVideoPlayback,mUSAcquisitionVideoPlaybacks)
    {
        if (uSAcquisitionVideoPlayback->isActive() && nameFilters.contains(uSAcquisitionVideoPlayback->getType()) )
            return uSAcquisitionVideoPlayback->getVideoSource();
     }

    // ask for playback stream:
    foreach(USAcquisitionVideoPlaybackPtr uSAcquisitionVideoPlayback,mUSAcquisitionVideoPlaybacks)
    {
        if (uSAcquisitionVideoPlayback->isActive())
            return uSAcquisitionVideoPlayback->getVideoSource();
    }

    // ask for active stream in first probe:
    ToolPtr tool = mBackend->tracking()->getFirstProbe();
    if (tool && tool->getProbe() && tool->getProbe()->getRTSource())
    {
        // keep existing if present
        if (old)
        {
            if (tool->getProbe()->getAvailableVideoSources().count(old->getUid()))
                    return old;
        }

        return tool->getProbe()->getRTSource();
    }

    std::vector<VideoSourcePtr> allSources = this->getVideoSources();
    // keep existing if present
    if (old)
    {
        if (std::count(allSources.begin(), allSources.end(), old))
                return old;
    }
    // ask for anything
    if (!allSources.empty())
        return allSources.front();

    // give up: return empty
    return mEmptyVideoSource;
}

void ToolTabsContainerWidget::CurrentTabChanged( const int newTabIndex )
{
    Q_UNUSED(newTabIndex);

    if ( m_mainWindow )
    {
        m_mainWindow->Reload();
    }

    const ToolPtr activeTab = FindActiveTool();
    if ( activeTab != 0 )
    {
        activeTab->Activated();
    }
}

void ToolTipCalibrateWidget::testCalibrationSlot()
{
  ToolPtr selectedTool = mTools->getTool();
  if(!selectedTool || !selectedTool->hasReferencePointWithId(1))
    return;

  CoordinateSystem to = mServices->spaceProvider()->getT(mServices->tracking()->getActiveTool());
  Vector3D sampledPoint = mServices->spaceProvider()->getActiveToolTipPoint(to);

  ToolTipCalibrationCalculator calc(mServices->spaceProvider(), mServices->tracking()->getActiveTool(), selectedTool, sampledPoint);
  Vector3D delta_selectedTool = calc.get_delta_ref();

  mDeltaLabel->setText("<b>Delta:</b> "+qstring_cast(delta_selectedTool)+" <br> <b>Length:</b>  "+qstring_cast(delta_selectedTool.length()));

  report("Delta: "+qstring_cast(delta_selectedTool)+" Length:   "+qstring_cast(delta_selectedTool.length()));
}

void ViewWrapper3D::toolsAvailableSlot()
{
	std::vector<ToolRep3DPtr> reps = RepContainer::findReps<ToolRep3D>(mView->getReps());

	TrackingService::ToolMap tools = mServices->tracking()->getTools();
	TrackingService::ToolMap::iterator iter;
	for (iter = tools.begin(); iter != tools.end(); ++iter)
	{
		ToolPtr tool = iter->second;
		if (tool->hasType(Tool::TOOL_REFERENCE))
			continue;

		ToolRep3DPtr toolRep = RepContainer(mView->getReps()).findFirst<ToolRep3D>(tool);

		std::vector<ToolRep3DPtr>::iterator oldRep = std::find(reps.begin(), reps.end(), toolRep);
		if (oldRep!=reps.end())
			reps.erase(oldRep);

		if (tool->hasType(Tool::TOOL_MANUAL) && !settings()->value("showManualTool").toBool())
		{
			if (toolRep)
				mView->removeRep(toolRep);
			continue;
		}
		//    mManualTool->setVisible(settings()->value("showManualTool").toBool());

		if (!toolRep)
		{
			toolRep = ToolRep3D::New(mServices->spaceProvider(), tool->getUid() + "_rep3d_" + this->mView->getUid());
			if (settings()->value("showToolPath").toBool())
				toolRep->getTracer()->start();
		}

		toolRep->setSphereRadius(settings()->value("View3D/sphereRadius").toDouble()); // use fraction of set size
		toolRep->setSphereRadiusInNormalizedViewport(true);

		toolRep->setTool(tool);
		toolRep->setOffsetPointVisibleAtZeroOffset(true);
		mView->addRep(toolRep);
	}

	// remove reps for tools no longer present
	for (unsigned i=0; i<reps.size(); ++i)
	{
		mView->removeRep(reps[i]);
	}
}

TreeNodePtr ToolTreeNode::getParent() const
{
	if (this->repo()->getMode()=="flat")
		return this->repo()->getNodeForGroup("tool");

	if (mTool->getUid() == this->getServices()->tracking()->getManualTool()->getUid())
		return this->repo()->getNode(CoordinateSystem(csPATIENTREF).toString());

	if (mTool->hasType(Tool::TOOL_REFERENCE))
		return this->repo()->getNode(CoordinateSystem(csPATIENTREF).toString());

	ToolPtr ref = this->getServices()->tracking()->getReferenceTool();
	if (ref)
		return this->repo()->getNode(ref->getUid());

	return this->repo()->getNode(CoordinateSystem(csPATIENTREF).toString());
}

/**Move the tool pos / axis pos to a new position given
 * by delta movement in vp space.
 */
void ViewWrapper2D::shiftAxisPos(Vector3D delta_vp)
{
	delta_vp = -delta_vp;
	ToolPtr tool = mServices->getToolManager()->getManualTool();

	Transform3D sMr = mSliceProxy->get_sMr();
	Transform3D rMpr = mServices->getPatientService()->get_rMpr();
	Transform3D prMt = tool->get_prMt();
	Transform3D vpMs = mView->get_vpMs();
	Vector3D delta_s = vpMs.inv().vector(delta_vp);

	Vector3D delta_pr = (rMpr.inv() * sMr.inv()).vector(delta_s);

	// MD is the actual tool movement in patient space, matrix form
	Transform3D MD = createTransformTranslate(delta_pr);
	// set new tool position to old modified by MD:
	tool->set_prMt(MD * prMt);
}