/* public static */
void
DistanceToPoint::computeDistance(const geom::Geometry& geom,
                                          const geom::Coordinate& pt,
                                          PointPairDistance& ptDist)
{
	if ( const LineString* ls = dynamic_cast<const LineString*>(&geom) )
	{
		computeDistance(*ls, pt, ptDist);
	}
	else if ( const Polygon* pl = dynamic_cast<const Polygon*>(&geom) )
	{
		computeDistance(*pl, pt, ptDist);
	}
	else if ( const GeometryCollection* gc = dynamic_cast<const GeometryCollection*>(&geom) )
	{
		for (size_t i = 0; i < gc->getNumGeometries(); i++)
		{
			const Geometry* g = gc->getGeometryN(i);
			computeDistance(*g, pt, ptDist);
		}
	}
	else
	{
		// assume geom is Point
		ptDist.setMinimum(*(geom.getCoordinate()), pt);
	}
}

inline virtual double operator()(const RenderItem * r1, const RenderItem * r2) const {
	if (supported(r1, r2))
		return computeDistance(dynamic_cast<const R1*>(r1), dynamic_cast<const R2*>(r2));
	else if (supported(r2,r1))
		return computeDistance(dynamic_cast<const R1*>(r2), dynamic_cast<const R2*>(r1));
	else
		return NOT_COMPARABLE_VALUE;
}

void countANCodingUndetectableErrors(uintll_t n, uintll_t A, uint128_t* counts, uintll_t count_counts) 
{
  double shardsDone = 0.0;

#pragma omp parallel 
  {
    const uintll_t CNT_MESSAGES = 0x1ull << n; 
    const uintll_t CNT_SLICES = CNT_MESSAGES / SZ_SHARDS; 
    const uintll_t CNT_SHARDS = CNT_SLICES * CNT_SLICES;
    uintll_t* counts_local = new uintll_t[count_counts];
    memset(counts_local, 0, count_counts*sizeof(uintll_t));
#pragma omp for schedule(dynamic,1)
    for (uintll_t shardX = 0; shardX < CNT_MESSAGES; shardX += SZ_SHARDS) 
    {
      // 1) Triangle for main diagonale
      uintll_t m1, m2;

      for (uintll_t x = 0; x < SZ_SHARDS; ++x) {
        m1 = (shardX + x) * A;
        ++counts_local[computeDistance(m1, m1)];
        for (uintll_t y = (x + 1); y < SZ_SHARDS; ++y) {
          m2 = (shardX + y) * A;
          counts_local[computeDistance(m1, m2)]+=2;
        }
      }

      // 2) Remainder of the slice
      for (uintll_t shardY = shardX + SZ_SHARDS; shardY < CNT_MESSAGES; shardY += SZ_SHARDS) {
        for (uintll_t x = 0; x < SZ_SHARDS; ++x) {
          m1 = (shardX + x) * A;
          for (uintll_t y = 0; y < SZ_SHARDS; ++y) {
            m2 = (shardY + y) * A;
            counts_local[computeDistance(m1, m2)]+=2;
          }
        }
      }

      uintll_t shardsComputed = CNT_SLICES - (shardX / SZ_SHARDS);
      float inc = static_cast<double>(shardsComputed * 2 - 1) / CNT_SHARDS * 100;
#pragma omp atomic
      shardsDone += inc;
    } // for

    // 3) Sum the counts
    for (uintll_t i = 0; i < count_counts; ++i) {
#pragma omp atomic
      counts[i] += counts_local[i];
    }

    delete[] counts_local;
  } // parallel
}

// add a obstacle square to the map, marking its distance in the relevant heading bins
void FovObstacleMap::putObstacle(GridSquare pt)
{
    if(!inFieldOfView(pt)) {
//std::cerr << "out of FOV: d = " << computeDistance(pt) << ", range = (" << minRange << ", " << maxRange << "), hdiff = " << computeHeadingDiff(computeHeading(pt), robotHeading) << ", fov/2 = " << fov/2.0 << std::endl;
        return;  // we don't want to consider this point
    }
    
    CellData& cellData = map.cell(pt.x, pt.y);
    cellData.isKnownObstacle = true;
    Length distance = computeDistance(pt);

    // if the difference between max and min is greater than pi, the cell lies 
    //  across the discontinuity in heading so array access must be separate
    Length *minObstacleDistances;
    int N;
    if(cellData.maxHeading - cellData.minHeading > wykobi::PI)
    {
        N = closestObstacleByHeading.cells(-wykobi::PI, cellData.minHeading, minObstacleDistances);
        setMinObstacleDistance(distance, minObstacleDistances, N);

        N = closestObstacleByHeading.cells(cellData.minHeading, wykobi::PI, minObstacleDistances);
        setMinObstacleDistance(distance, minObstacleDistances, N);
    }
    else
    {
        N = closestObstacleByHeading.cells(cellData.minHeading, cellData.maxHeading, minObstacleDistances);
        setMinObstacleDistance(distance, minObstacleDistances, N);
    }
}

float Action::reachGrasp(geometry_msgs::Pose pose_target, const std::string surface_name)
{
  if (!reachPregrasp(pose_target, surface_name))
    return std::numeric_limits<float>::max();

  float dist = computeDistance(move_group_->getCurrentPose().pose, move_group_->getPoseTarget().pose);
  ROS_INFO_STREAM("Reaching distance to the target = " << dist);

  /*moveit_msgs::PickupGoal goal;
  collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));

std::cout << "attach_object_msg.link_name  " << attach_object_msg.link_name << std::endl;
std::cout << "attach_object_msg.object.id " << attach_object_msg.object.id << std::endl;
std::cout << "attach_object_msg.object.operation " << attach_object_msg.object.operation << std::endl;
std::cout << "attach_object_msg.touch_links.size() " << attach_object_msg.touch_links.size() << std::endl;

  // we are allowed to touch the target object
  approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);*/

  if (dist < 0.2)
  //if (dist > grasp_data_.approach_retreat_min_dist_)
    posture.poseHandClose(end_eff);

  return dist;
}

void findBestNMatches( uint nBest,
                       const cv::Mat &descriptor,
                       const cv::Mat &candidateDescriptors,
                       const uchar *mask,
                       std::list<cv::DMatch> &bestNMatches )
{
    bestNMatches.clear();

    double minDistance = -1.0L;

    for (int i = 0; i < candidateDescriptors.rows; ++i)
    {
        if (!mask || (mask && mask[i]))
        {
            double distance = computeDistance(descriptor, candidateDescriptors, i);

            if (distance < minDistance || bestNMatches.size() < 2)
            {
                minDistance = minDistance < 0 ? distance : MIN(minDistance, distance);
                bestNMatches.push_front(cv::DMatch(0, i, static_cast<float>(distance)));

                if (bestNMatches.size() > nBest)
                {
                    bestNMatches.pop_back();
                }
            }
        }
    }
}

/**
* CCamera::setCamera
* date Modified March 13, 2006
*/
void CCamera::setCamera(const D3DXVECTOR3 &pos, const D3DXVECTOR3 &targ, D3DXVECTOR3 &vPTwoPos)
{
	// do this
	if(vPTwoPos == D3DXVECTOR3(0,0,0))
		vPTwoPos = targ;
	// store the position of the camera
	m_Position = pos;
	// store the target position of the camera
	m_Target = (targ+vPTwoPos)*0.5f;
	// store the vector from the target to the position
	m_TargToPos = m_Position - m_Target;
	D3DXVec3Normalize(&m_UnitTargPos, &(m_Target - m_Position)); 

	// set the up vector based on the world's up
	m_UpVector = D3DXVECTOR3(0,1,0);
	// compute the at and right of the camera
	m_AtVector = m_Target - m_Position;
	m_AtVector.y = 0;
	D3DXVec3Normalize(NULL, &m_AtVector, &m_AtVector);
	D3DXVec3Cross(&m_RightVector, &m_UpVector, &m_AtVector);
	D3DXVec3Normalize(NULL, &m_RightVector, &m_RightVector);

	// set the initial rotation of the camera
	D3DXVECTOR3 vec = -m_AtVector;
	m_fRotation = acosf(D3DXVec3Dot(&vec, &D3DXVECTOR3(0,0,1)));
	if(D3DXVec3Dot(&vec, &D3DXVECTOR3(1,0,0)) < 0) m_fRotation = -m_fRotation;

	if (targ == vPTwoPos)
		m_fInitDist = 0.0f;
	else
		// compute the initial distance between the characters
		m_fInitDist = computeDistance(targ, vPTwoPos);
}

void kmeans::startClustering() {

	kcenter = new double[k];
	for(int i = 0; i < k; i++) kcenter[i] = 0;
	
	center = new average[k];
	for(int i = 0; i < k; i++) {
		center[i].setx(0);
		center[i].sety(0);
	}
	
	imageArray = new int*[numRow];
	for(int i = 0; i < numRow; ++i) {
    	imageArray[i] = new int[numCol];
	}
	
	for(int i = 0; i < numRow; i++) {
		for(int j = 0; j < numCol; j++) {
			imageArray[i][j] = 0;
		}
	}

	labelChecker = true;
		
	while (labelChecker) {
		labelChecker = false;
		computeCenter();
		computeDistance();	
	}

	listHead.putonImage(imageArray);	
	
}

  //--------------------------------------------- IS IN WEAPON RANGE -----------------------------------------
  bool UnitImpl::isInWeaponRange(Unit *target) const
  {
    // Preliminary checks
    if ( !exists() || !target || !target->exists() || this == target )
      return false;

    // Store the types as locals
    UnitType thisType = getType();
    UnitType targType = target->getType();

    // Obtain the weapon type
    WeaponType wpn = thisType.groundWeapon();
    if ( targType.isFlyer() || target->isLifted() )
      wpn = thisType.airWeapon();

    // Return if there is no weapon type
    if ( wpn == WeaponTypes::None || wpn == WeaponTypes::Unknown )
      return false;

    // Retrieve the min and max weapon ranges
    int minRange = wpn.minRange();
    int maxRange = getPlayer()->weaponMaxRange(wpn);

    // Check if the distance to the unit is within the weapon range
    int distance = computeDistance(this, target);
    return (minRange ? minRange < distance : true) && maxRange >= distance;
  }

void Shooter::run () {
    while(true) {
        computeTurn();
        // for the speed of the shooter
        if(getoShooterSpeed != -1)
            pid->SetSetpoint(getoShooterSpeed/-60.);
        else {

            //pid->SetSetpoint(0);
            pid->SetSetpoint(computeSpeed(computeDistance())/-60.);
            //printf("encoder %f %f\n", encoder.GetRate()*60, distance.GetVoltage()/.0098);
        }
        cout << encoder.GetRate() << endl;
        /*double intPart;
        turret->Set(modf(TurretLocation, &intPart));
        if(LimitTurret.Get() == 1) {
        	// I am rezeroing the turret because it might slip on the turning and this will reset it as it should be
        	turret->EnableControl(0);
        	// I think that this has to get reset to the jaguar after it is enabled
        	turret->ConfigEncoderCodesPerRev((int)(500 * config->GetValue("turretMotorTurns")));
        	turret->SetPID(config->GetValue("turretP"), config->GetValue("turretI"), config->GetValue("turretD"));
        }*/

        Wait(.05);
    }
}

 static inline float minusSimilarity(
     const int distancefunction,
     const Mat& points1, int idx1,
     const Mat& points2, int idx2)
 {
     return -computeDistance(distancefunction, points1, idx1, points2, idx2);
 }

Math::Matrix
AAKR::estimate(Math::Matrix query, double variance)
{
    Math::Matrix mean;
    Math::Matrix std;

    normalize(mean, std);

    // Use normalized query vector.
    computeDistance((query - mean) / std);
    computeWeights(variance);

    double s = sum(m_weights);

    // Avoid division by zero.
    if (!s)
        return query;

    // Combine with weights.
    Math::Matrix result = ((transpose(m_weights) * m_norm) / s);

    // Normalize.
    for (unsigned i = 0; i < sampleSize(); i++)
        result(i) = result(i) * std(i) + mean(i);

    return result;
}

/**
* CCamera::updateCameraMP
* date Modified March 13, 2006
*/
void CCamera::updateCameraMP(const D3DXVECTOR3 &one, const D3DXVECTOR3 &two)
{
	// check whether to update or not
	if(!m_bUpdateMP)
		return;

	// set the target to the point b/w the two players'
	D3DXVec3Scale(&m_Target, &(one + two), 0.5f);

	// compute the distance b/w the characters
	float fDist = computeDistance(one, two);

	// move the camera forward/back based on the change in distance b/w characters
	m_Position = m_Target + m_TargToPos;
	m_Position += m_UnitTargPos * ((m_fInitDist - fDist)/m_fInitDist*m_fMoveDist);
	m_TargToPos = m_Position - m_Target;
	m_fInitDist = fDist;

	// if the camera moves too far away, stop it
	if((fDist = D3DXVec3Length(&m_TargToPos)) > 150.0f)
	{
		m_TargToPos *= (1.0f/fDist);
		m_TargToPos *= 150.0f;
	}
}

/*
 * The inexact GiST distance method for geometric types that store bounding
 * boxes.
 *
 * Compute lossy distance from point to index entries.  The result is inexact
 * because index entries are bounding boxes, not the exact shapes of the
 * indexed geometric types.  We use distance from point to MBR of index entry.
 * This is a lower bound estimate of distance from point to indexed geometric
 * type.
 */
Datum
gist_bbox_distance(PG_FUNCTION_ARGS)
{
	GISTENTRY  *entry = (GISTENTRY *) PG_GETARG_POINTER(0);
	StrategyNumber strategy = (StrategyNumber) PG_GETARG_UINT16(2);
	bool	   *recheck = (bool *) PG_GETARG_POINTER(4);
	double		distance;
	StrategyNumber strategyGroup = strategy / GeoStrategyNumberOffset;

	/* Bounding box distance is always inexact. */
	*recheck = true;

	switch (strategyGroup)
	{
		case PointStrategyNumberGroup:
			distance = computeDistance(false,
									   DatumGetBoxP(entry->key),
									   PG_GETARG_POINT_P(1));
			break;
		default:
			elog(ERROR, "unknown strategy number: %d", strategy);
			distance = 0.0;		/* keep compiler quiet */
	}

	PG_RETURN_FLOAT8(distance);
}

 static inline float heuristicSimilarity(
     const int distancefunction,
     const float alpha,
     const Mat& points1, int idx1,
     const Mat& points2, int idx2)
 {
     return 1 / (alpha + computeDistance(distancefunction, points1, idx1, points2, idx2));
 }

 void CostMap2D::enqueue(unsigned int source, unsigned int mx, unsigned int my){
   // If the cell is not marked for cost propagation
   unsigned int ind = MC_IND(mx, my);
   if(!marked(ind)){
     QueueElement* c = new QueueElement(computeDistance(source, ind), source, ind);
     queue_.push(c);
     mark(ind);
   }
 }

 static inline float gaussianSimilarity(
     const int distancefunction,
     const float alpha,
     const Mat& points1, int idx1,
     const Mat& points2, int idx2)
 {
     float distance = computeDistance(distancefunction, points1, idx1, points2, idx2);
     return exp(-alpha * distance * distance);
 }

int point_proche(int16 table[][2]) {
	int x1, y1, i, x, y, p;
	int d1 = 1000;

	_vm->_polyStructs = &_vm->_polyStructNorm;

	if (nclick_noeud == 1) {
		x = x_mouse;
		y = y_mouse;
		x1 = table_ptselect[0][0];
		y1 = table_ptselect[0][1];

		_vm->_polyStructs = &_vm->_polyStructExp;

		getPixel(x, y);

		if (!flag_obstacle) {
			_vm->_polyStructs = &_vm->_polyStructNorm;

			getPixel(x, y);

			if (flag_obstacle) {
				polydroite(x1, y1, x, y);
			}
			_vm->_polyStructs = &_vm->_polyStructExp;
		}
		if (!flag_obstacle) {	/* dans flag_obstacle --> couleur du point */
			x1 = table_ptselect[0][0];
			y1 = table_ptselect[0][1];

			poly2(x, y, x1, y1);

			x_mouse = X;
			y_mouse = Y;
		}
	}
	_vm->_polyStructs = &_vm->_polyStructNorm;

	p = -1;
	for (i = 0; i < ctp_routeCoordCount; i++) {
		x = table[i][0];
		y = table[i][1];

		int pointDistance = computeDistance(x_mouse, y_mouse, x, y);
		if (pointDistance < d1) {
			polydroite(x_mouse, y_mouse, x, y);

			if (!flag_obstacle && ctp_routes[i][0] > 0) {
				d1 = pointDistance;
				p = i;
			}
		}
	}

	return (p);
}

bool QgsLayoutItemPolyline::_addNode( const int indexPoint,
                                      QPointF newPoint,
                                      const double radius )
{
  const double distStart = computeDistance( newPoint, mPolygon[0] );
  const double distEnd = computeDistance( newPoint, mPolygon[mPolygon.size() - 1] );

  if ( indexPoint == ( mPolygon.size() - 1 ) )
  {
    if ( distEnd < radius )
      mPolygon.append( newPoint );
    else if ( distStart < radius )
      mPolygon.insert( 0, newPoint );
  }
  else
    mPolygon.insert( indexPoint + 1, newPoint );

  return true;
}

int EuclidDistHeuristic::GetGoalHeuristic(int state_id)
{
    if (state_id == planningSpace()->getGoalStateID()) {
        return 0;
    }

    if (m_pose_ext) {
        Affine3 p;
        if (!m_pose_ext->projectToPose(state_id, p)) {
            return 0;
        }

        auto& goal_pose = planningSpace()->goal().pose;

        const double dist = computeDistance(p, goal_pose);

        const int h = FIXED_POINT_RATIO * dist;

        double Y, P, R;
        angles::get_euler_zyx(p.rotation(), Y, P, R);
        SMPL_DEBUG_NAMED(LOG, "h(%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f) = %d", p.translation()[0], p.translation()[1], p.translation()[2], Y, P, R, h);

        return h;
    } else if (m_point_ext) {
        Vector3 p;
        if (!m_point_ext->projectToPoint(state_id, p)) {
            return 0;
        }

        auto& goal_pose = planningSpace()->goal().pose;
        Vector3 gp(goal_pose.translation());

        double dist = computeDistance(p, gp);

        const int h = FIXED_POINT_RATIO * dist;
        SMPL_DEBUG_NAMED(LOG, "h(%d) = %d", state_id, h);
        return h;
    } else {
        return 0;
    }
}