/**Call when viewport size or zoom has changed.
 * Recompute camera zoom and  reps requiring vpMs.
 */
void ViewWrapper2D::viewportChanged()
{
	if (!mView->getRenderer()->IsActiveCameraCreated())
		return;

	mView->setZoomFactor(mZoom2D->getFactor());

	double viewHeight = mView->getViewport_s().range()[1];
	mView->getRenderer()->GetActiveCamera()->SetParallelScale(viewHeight / 2);

	// Heuristic guess for a good clip depth. The point is to show 3D data clipped in 2D
	// with a suitable thickness projected onto the plane.
	double clipDepth = 2.0; // i.e. all 3D props rendered outside this range is not shown.
	double length = clipDepth*10;
	clipDepth = viewHeight/120 + 1.5;
	mView->getRenderer()->GetActiveCamera()->SetPosition(0,0,length);
	mView->getRenderer()->GetActiveCamera()->SetClippingRange(length-clipDepth, length+0.1);

	mSliceProxy->setToolViewportHeight(viewHeight);
	double anyplaneViewOffset = settings()->value("Navigation/anyplaneViewOffset").toDouble();
	mSliceProxy->initializeFromPlane(mSliceProxy->getComputer().getPlaneType(), false, Vector3D(0, 0, 1), true, viewHeight, anyplaneViewOffset, true);

	DoubleBoundingBox3D BB_vp = getViewport();
	Transform3D vpMs = mView->get_vpMs();
	DoubleBoundingBox3D BB_s = transform(vpMs.inv(), BB_vp);
	PLANE_TYPE plane = mSliceProxy->getComputer().getPlaneType();

	mToolRep2D->setViewportData(vpMs, BB_vp);
	if (mSlicePlanes3DMarker)
	{
		mSlicePlanes3DMarker->getProxy()->setViewportData(plane, mSliceProxy, BB_s);
	}

	mViewFollower->setView(BB_s);
}

double ManualImage2ImageRegistrationWidget::getAccuracy(QString uid)
{
    DataPtr fixedData = mServices->registration()->getFixedData();
    if (!fixedData)
        return 1000.0;
    DataPtr movingData = mServices->registration()->getMovingData();
    if (!movingData)
        return 1000.0;

    Landmark masterLandmark = fixedData->getLandmarks()->getLandmarks()[uid];
    Landmark targetLandmark = movingData->getLandmarks()->getLandmarks()[uid];
    if (masterLandmark.getUid().isEmpty() || targetLandmark.getUid().isEmpty())
        return 1000.0;

    Vector3D p_master_master = masterLandmark.getCoord();
    Vector3D p_target_target = targetLandmark.getCoord();
    Transform3D rMmaster = fixedData->get_rMd();
    Transform3D rMtarget = movingData->get_rMd();

    Vector3D p_target_r = rMtarget.coord(p_target_target);
    Vector3D p_master_r = rMmaster.coord(p_master_master);

    double  targetPoint[3];
    double  masterPoint[3];
    targetPoint[0] = p_target_r[0];
    targetPoint[1] = p_target_r[1];
    targetPoint[2] = p_target_r[2];
    masterPoint[0] = p_master_r[0];
    masterPoint[1] = p_master_r[1];
    masterPoint[2] = p_master_r[2];

    return (vtkMath::Distance2BetweenPoints(targetPoint, masterPoint));
}

Vector3D Frame3D::getEulerXYZ() const
{
	Transform3D t;
	t = mAngleAxis;
	Vector3D ea = t.matrix().block<3, 3> (0, 0).eulerAngles(0, 1, 2);
	return ea;
}

 float Transform3D::norm(Transform3D T){
     float pos_norm = arma::norm(T.translation());
     UnitQuaternion q = UnitQuaternion(Rotation3D(T.submat(0,0,2,2)));
     float angle = q.getAngle();
     //TODO: how to weight these two?
     return pos_norm + Rotation3D::norm(T.rotation());
 }

void LandmarkRegistrationWidget::cellChangedSlot(int row, int column)
{
	QTableWidgetItem* item = mLandmarkTableWidget->item(row, column);
	QString uid = item->data(Qt::UserRole).toString();

	if (column == 0)
	{
		QString name = item->text();
		mServices->patient()->setLandmarkName(uid, name);
	}
	if (column == 1)
	{
		Qt::CheckState state = item->checkState();
		mServices->patient()->setLandmarkActive(uid, state == Qt::Checked);
		this->performRegistration(); // automatic when changing active state (Mantis #0000674)s
	}
	if (column == 2)
	{
		QString val = item->text();
		// remove formatting stuff:
		val = val.replace('(', " ");
		val = val.replace(')', " ");
		val = val.replace(',', " ");

		Transform3D rMtarget = this->getTargetTransform();

		Vector3D p_r = Vector3D::fromString(val);
		Vector3D p_target = rMtarget.inv().coord(p_r);
		this->setTargetLandmark(uid, p_target);
	}
}

vtkPolyDataPtr polydataFromTransforms(TimedTransformMap transformMap_prMt, Transform3D rMpr)
{
  vtkPolyDataPtr retval = vtkPolyDataPtr::New();

  vtkPointsPtr points = vtkPointsPtr::New();
  vtkCellArrayPtr lines = vtkCellArrayPtr::New();

  points->Allocate(transformMap_prMt.size());

  TimedTransformMap::iterator mapIter = transformMap_prMt.begin();
  while(mapIter != transformMap_prMt.end())
  {
    Vector3D point_t = Vector3D(0,0,0);

    Transform3D prMt = mapIter->second;
    Transform3D rMt = rMpr * prMt;
    Vector3D p = rMt.coord(point_t);
    points->InsertNextPoint(p.begin());

		++mapIter;
  }

  lines->Initialize();
  std::vector<vtkIdType> ids(points->GetNumberOfPoints());
  for (unsigned i=0; i<ids.size(); ++i)
    ids[i] = i;
  lines->InsertNextCell(ids.size(), &(*ids.begin()));

  retval->SetPoints(points);
  retval->SetLines(lines);
  return retval;
}

std::vector<Vector3D> CustomMetric::getPointCloud() const
{
	std::vector<Vector3D> retval;

	DataPtr model = this->getModel();

	std::vector<Transform3D> pos = this->calculateOrientations();
	std::vector<Vector3D> cloud;
	Transform3D rrMd;

	if (model)
	{
		rrMd = model->get_rMd();
		cloud = model->getPointCloud();
	}
	else
	{
		cloud.push_back(Vector3D::Zero());
		rrMd = Transform3D::Identity();
	}

	for (unsigned i=0; i<pos.size(); ++i)
	{
		Transform3D rMd = pos[i] * rrMd;

		for (unsigned j=0; j<cloud.size(); ++j)
		{
			Vector3D p_r = rMd.coord(cloud[j]);
			retval.push_back(p_r);
		}
	}

	return retval;
}

void CustomMetric::updateTexture(MeshPtr model, Transform3D rMrr)
{
	if (!model)
		return;

	if (!this->getTextureFollowTool() || !model->hasTexture())
		return;

	// special case:
	// Project tool position down to the model, then set that position as
	// the texture x pos.

	Transform3D rMt = mSpaceProvider->getActiveToolTipTransform(CoordinateSystem::reference());
	Transform3D rMd = rMrr * model->get_rMd();
	Vector3D t_r = rMt.coord(Vector3D::Zero());
	Vector3D td_r = rMt.vector(Vector3D::UnitZ());

	DoubleBoundingBox3D bb_d = model->boundingBox();
	Vector3D bl = bb_d.bottomLeft();
	Vector3D tr = bb_d.topRight();
	Vector3D c = (bl+tr)/2;
	Vector3D x_min_r(c[0], bl[1], c[2]);
	Vector3D x_max_r(c[0], tr[1], c[2]);
	x_min_r = rMd.coord(x_min_r);
	x_max_r = rMd.coord(x_max_r);

	double t_x = dot(t_r, td_r);
	double bbmin_x = dot(x_min_r, td_r);
	double bbmax_x = dot(x_max_r, td_r);
	double range = bbmax_x-bbmin_x;
	if (similar(range, 0.0))
		range = 1.0E-6;
	double s = (t_x-bbmin_x)/range;
	model->getTextureData().getPositionY()->setValue(s);
}

void Rect3D::updatePosition(const DoubleBoundingBox3D bb, const Transform3D& M)
{
	mPoints = vtkPointsPtr::New();
	mPoints->InsertPoint(0, M.coord(bb.corner(0,0,0)).begin());
	mPoints->InsertPoint(1, M.coord(bb.corner(0,1,0)).begin());
	mPoints->InsertPoint(2, M.coord(bb.corner(1,1,0)).begin());
	mPoints->InsertPoint(3, M.coord(bb.corner(1,0,0)).begin());
	mPolyData->SetPoints(mPoints);
}

//compute the small iteration
VelocityScrew6D<> computeG(Transform3D<> Tin, Transform3D<> Tdes){
	//compute positional difference
	Vector3D<> dif=Tdes.P()-Tin.P();
	//compute rotation difference
	Rotation3D<> rot=Tdes.R()*inverse(Tin.R());
	//assign values to deltau
	VelocityScrew6D<> deltau(dif(0),dif(1),dif(2),(rot(2,1)-rot(1,2))/2,(rot(0,2)-rot(2,0))/2,(rot(1,0)-rot(0,1))/2);
	return(deltau);
}

/**Create a transform to a space defined by an origin and two perpendicular unit vectors that
 * for the x-y plane.
 * The original space is A and the space defined by ijc are B
 * The returned transform M_AB converts a point in B to A:
 * 		p_A = M_AB * p_B
 */
Transform3D createTransformIJC(const Vector3D& ivec, const Vector3D& jvec, const Vector3D& center)
{
	Transform3D t = Transform3D::Identity();
	t.matrix().col(0).head(3) = ivec;
	t.matrix().col(1).head(3) = jvec;
	t.matrix().col(2).head(3) = cross(ivec, jvec);
	t.matrix().col(3).head(3) = center;
	return t;
}

Vector3D ViewFollower::findCenter_r_fromShift_s(Vector3D shift_s)
{
	Transform3D sMr = mSliceProxy->get_sMr();
	Vector3D c_s = sMr.coord(mDataManager->getCenter());

	Vector3D newcenter_s = c_s + shift_s;

	Vector3D newcenter_r = sMr.inv().coord(newcenter_s);
	return newcenter_r;
}

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 Correlator::addData(MocapStream::RigidBodyID id1, Transform3D T1, MocapStream::RigidBodyID id2, Transform3D T2){
			//Generate key for the map of correlationStats
			std::pair<MocapStream::RigidBodyID, MocapStream::RigidBodyID> key = {id1,id2};

			//Check if this hypothesis has been eliminated
			if(eliminatedHypotheses.count(key) != 0) return;

			if(recordedStates.count(key) == 0){
				//Initialise if key missing. (true => calculate cov)
				recordedStates[key] = StreamPair();
			} else {
				
				// std::cout << "number of samples = " << recordedStates[key].first.size() << std::endl;
				// std::cout << "diff1 = " << diff1 << std::endl; 
				// if( id1 == 1 && id2 == 18) std::cout << "T2 = " << T2 << std::endl; 
				// std::cout << "diff2 = " << diff2 << std::endl; 
				// std::cout << "T2 = " << T2 << std::endl; 

				//If we have no recorded states yet, or the new states differ significantly from the previous, add the new states
				if( stateIsNew(T1, recordedStates[key].first) 
					|| stateIsNew(T2, recordedStates[key].second) )
				{
					//Check if bad sample (for the particular solver we are using):	
					Rotation3D R1 = T1.rotation();
					UnitQuaternion q1(R1);
					Rotation3D R2 = T2.rotation();
					UnitQuaternion q2(R2);
					if(q1.kW() == 0 || q2.kW() == 0) return;

					//Check det
					if(std::fabs(arma::det(R1) - 1) > 0.1){
						std::cout << __FILE__ << " : Det R1 = " << arma::det(R1) << std::endl;
					}
					if(std::fabs(arma::det(R2) - 1) > 0.1){
						std::cout << __FILE__ << " : Det R2 = " << arma::det(R2) << std::endl;

					}

					std::cout << "Recording Sample..." << std::endl; 

					if(recordedStates[key].first.size() >= number_of_samples){
						recordedStates[key].first.erase(recordedStates[key].first.begin());
						recordedStates[key].first.push_back(T1);
						recordedStates[key].second.erase(recordedStates[key].second.begin());
						recordedStates[key].second.push_back(T2);
						//Now we are ready to compute
						computableStreams.insert(key);
					} else {
						recordedStates[key].first.push_back(T1);
						recordedStates[key].second.push_back(T2);
					}
				}

			}
		}

Vector3D ToolTipCalibrationCalculator::get_sampledPoint_ref()
{
  CoordinateSystem csT = mSpaces->getT(mTool); //from
  CoordinateSystem csRef = mSpaces->getT(mRef); //to

  Transform3D refMt = mSpaces->get_toMfrom(csT, csRef);

  Vector3D P_ref = refMt.coord(mP_t);

  return P_ref;
}

/**called when transform is changed
 * reset it in the prop.*/
void GeometricRep2D::transformChangedSlot()
{
	if (!mSlicer || !mMesh)
		return;

	Transform3D rMs = mSlicer->get_sMr().inv();
	Transform3D dMr = mMesh->get_rMd().inv();
	Transform3D dMs = dMr * rMs;

	mActor->SetUserMatrix(dMs.inv().getVtkMatrix());
}

void Environment3D::worldToPlanner(Transform3D &state) const
{
    MPVec3 pPos = state.getPosition();
    this->worldToPlanner(pPos);
    
    MPQuaternion pRot = state.getRotation();
    this->worldToPlanner(pRot);
    
    state.setPosition(pPos);
    state.setRotation(pRot);
}

bool Render::on_motion_notify_event(GdkEventMotion* event)
{
  bool redraw=true;
  Vector2f dragp(event->x, event->y);
  Vector2f delta = m_downPoint - dragp;
  double factor = 0.3;
  Vector3d delta3f(-delta.x*factor, delta.y*factor, 0);
  get_model()->setMeasuresPoint(Vector2d((10.+event->x)/(get_width()-20),
					 (10.+get_height()-event->y)/(get_height()-20)));
  if (event->state & GDK_BUTTON1_MASK) { // move or rotate
    if (event->state & GDK_SHIFT_MASK) { // move object
      if (false);//delta3f.x<1 && delta3f.y<1) redraw=false;
      else {
	Shape *shape;
	TreeObject *object;
	if (!m_view->get_selected_stl(object, shape))
	  return true;
	if (!object && !shape)
	  return true;
	Transform3D *transf;
	if (!shape)
	  transf = &object->transform3D;
	else
	  transf = &shape->transform3D;
	transf->move(delta3f);
	m_downPoint = dragp;
	//m_view->get_model()->CalcBoundingBoxAndCenter();
      }
    }
    else { // rotate
      m_arcBall->dragAccumulate(event->x, event->y, &m_transform);
    }
    if (redraw) queue_draw();
    return true;
  }
  else {
    if (event->state & GDK_BUTTON2_MASK) { // zoom
      double factor = 1.0 + 0.01 * (delta.x - delta.y);
      m_zoom *= factor;
    }
    else if (event->state & GDK_BUTTON3_MASK) { // pan
      Matrix4f matrix;
      memcpy(&matrix.m00, &m_transform.M[0], sizeof(Matrix4f));
      Vector3f m_transl = matrix.getTranslation();
      m_transl += delta3f;
      matrix.setTranslation(m_transl);
      memcpy(&m_transform.M[0], &matrix.m00, sizeof(Matrix4f));      
    }
    m_downPoint = dragp;
    if (redraw) queue_draw();
    return true;
  }
  return Gtk::DrawingArea::on_motion_notify_event (event);
}

void ImGuiProperty(const Transform3D &transform3D) {
    auto position = transform3D.position();
    auto center = transform3D.center();
    auto scale = transform3D.scale();
    auto orientation = transform3D.orientation();

    ImGui::InputFloat3("Position", &position[0]);
    ImGui::InputFloat3("Center", &center[0]);
    ImGui::InputFloat("Scale", &scale);
    ImGui::InputFloat4("Orientation", &orientation[0]);
}

void ViewWrapper2D::setSlicePlanesProxy(SlicePlanesProxyPtr proxy)
{
	mSlicePlanes3DMarker = SlicePlanes3DMarkerIn2DRep::New("uid");
	PLANE_TYPE plane = mSliceProxy->getComputer().getPlaneType();
	mSlicePlanes3DMarker->setProxy(plane, proxy);

	DoubleBoundingBox3D BB_vp = getViewport();
	Transform3D vpMs = mView->get_vpMs();
	mSlicePlanes3DMarker->getProxy()->setViewportData(plane, mSliceProxy, transform(vpMs.inv(), BB_vp));

	mView->addRep(mSlicePlanes3DMarker);
}