void MapOverview::ShowSamples(const QList<Robot::DetectedSample> &samples)
{
    static const QFont font("Times", 3, QFont::Bold);

    qDeleteAll(mSampleDetections->childItems());

    for(auto& sample : samples)
    {
        Vector2d loc = sample.location;
        auto circle = new QGraphicsEllipseItem(loc.x()-0.2, loc.y()-0.2, 0.4, 0.4, mSampleDetections);
        //circle->setPos(circle->pos() + mRobotInstance->pos());
        circle->setPen(QPen(Qt::red, 0));

        //mSampleDetections->addToGroup(circle);

        //std::cout << "Sample at " << loc.transpose() << "\n";

        /*
        auto text = new QGraphicsTextItem(sample.name);
        text->setPos(loc.x(), loc.y());
        text->setFont(font);
        text->setDefaultTextColor(Qt::red);

        mTagDetections->addToGroup(text);
        */
    }

}

float ComponentAIBomber::distanceFromThePath()
{
	Vector2d start  = path->getStart();
	Vector2d end = path->getEnd();
	Vector2d normalPoint = Vector2d::getNormalPoint(parent->position, start, end);

	//COmprobamos que el punto esta entra el principio y el fin, si no es cogemos el final
	if (normalPoint.x < Math::min_(start.x,end.x) || normalPoint.x > Math::max_(start.x,end.x) ) 
	{
		if(start.getDistanceFrom(normalPoint) < end.getDistanceFrom(normalPoint))
		{
			normalPoint = start;
		}else
		{
			normalPoint = end;
		}
	}

	//Dibujamos linea
	/*GraphicsEngine* graphicsEngine = GameManager::getInstance()->getGraphicsEngine();
	if(graphicsEngine != NULL)
	{
		if(GameManager::getInstance()->getDebugTools()->isShowingDebug())
		{				
			//	graphicsEngine->drawDebugLine(start.asVector3d(), end.asVector3d());
			graphicsEngine->drawDebugLine(normalPoint.asVector3d(), parent->position.asVector3d());
		}
	}*/

	float distance = parent->position.getDistanceFrom(normalPoint);

	return distance;

}

//  Ellipse Collisions
bool PhysicsProcessor::collidedRectangleEllipse(Vector2d<float> pos1, Vector2d<float> dims,Vector2d<float> pos2, float r1, float r2)
{
       //  If the bounding boxes don't collide then the circle can't collide.
  /*  lastCollisionA.ticks = timer->getTicks();
    lastCollisionA.hasCollided = false;
    lastCollisionB.ticks = lastCollisionA.ticks;
    lastCollisionB.hasCollided = false;*/
        //  1st we create bounding boxes for the circle and do a Rectangle test against the Rectangle.
    if(collidedRectangleRectangle(pos1, dims,pos2, Vector2d<float>(2*r1,2*r2)))
    {
        //  Check the tallest extremites
        if(collidedLineRectangle(pos2 - Vector2d<float>(0.0f,r2),pos2 + Vector2d<float>(0.0f,r2),pos1,dims )
           || collidedLineRectangle(pos2 - Vector2d<float>(r1,0.0f),pos2 + Vector2d<float>(r1,0.0f),pos1,dims)
           )
            return true;
        /// NOTE we are cheating the result for now.
        float meanR = r1 + r2 * 0.5f;

        //  If the boxes overlap do a radial distance check
        Vector2d<float> diff = pos2 - pos1;
        float meanDim = dims.x + dims.y * 0.5f;
        if(diff.lengthSquared() <= (meanDim*meanDim) + (r2*r2))
        {
            //lastCollision.hasCollided = true;
            return true;
        }
        //return true;
    }
    return false;
}

// Obtiene el objeto en una posicion indicada. Solo objetos registrados con collider
GameObject* CollisionManager::getGameObjectIn( Vector2d position )
{
	for(std::size_t i = 0; i < colliderAlliesList.size(); i++)
	{
		Vector2d direction = position - colliderAlliesList[i]->getGameObject()->position;
		float distance = direction.getSqrLength();

		if(distance <= colliderAlliesList[i]->getSqrCollisionRadius())
		{
			return colliderAlliesList[i]->getGameObject();
		}
	}

	for(std::size_t i = 0; i < colliderEnemyList.size(); i++)
	{
		Vector2d direction = position - colliderEnemyList[i]->getGameObject()->position;
		float distance = direction.getSqrLength();

		if(distance <= colliderEnemyList[i]->getSqrCollisionRadius())
		{
			return colliderEnemyList[i]->getGameObject();
		}
	}
	return NULL;
}

double Polyline::project(const Vector2d &point) const
{
    double bestS = 0.;
    double minDistSq = (point - _pts[0]).squaredNorm();

    for(int i = 0; i < _pts.endIdx(1); ++i)
    {
        double len = (_lengths[i + 1] - _lengths[i]);
        double invLen = 1. / len;
        Vector2d der = (_pts[i + 1] - _pts[i]) * invLen;
        double dot = der.dot(point - _pts[i]);        
        if(dot < 0.)
            dot = 0.;
        if(dot > len)
            dot = len;

        Vector2d ptOnLine = _pts[i] + (_pts[i + 1] - _pts[i]) * (dot * invLen);
        double distSq = (ptOnLine - point).squaredNorm();
        if(distSq < minDistSq)
        {
            minDistSq = distSq;
            bestS = _lengths[i] + dot;
        }
    }

    return bestS;
}

//----------------------------------------------------------------------------
int BspTree2::CoPointLocation (const BspPolygon2& polygon,
    const Vector2d& vertex) const
{
    // For numerical round-off error handling.
    const double epsilon = 0.00001;

    const int numEdges = (int)mCoincident.size();
    for (int i = 0; i < numEdges; ++i)
    {
        Vector2d end0 = polygon.mVArray[mCoincident[i].I0];
        Vector2d end1 = polygon.mVArray[mCoincident[i].I1];
        Vector2d dir = end1 - end0;
        Vector2d diff = vertex - end0;
        double tmax = dir.Dot(dir);
        double t = dir.Dot(diff);

        if (-epsilon <= t && t <= tmax + epsilon)
        {
            return 0;
        }
    }

    // Does not matter which subtree you use.
    if (mPosChild)
    {
        return mPosChild->PointLocation(polygon, vertex);
    }

    if (mNegChild)
    {
        return mNegChild->PointLocation(polygon, vertex);
    }

    return 0;
}

// Comprueba colision entre Vision y Collider. Un objeto entra en la vision de otro
void CollisionManager::checkVisionBetween (ComponentVision* visionCollider, ComponentCollider* targetCollider)
{
	//Optimizaciones de memoria
	GameObject *targetGameObject = targetCollider->getGameObject();

	if(targetGameObject->isDead())
	{
		return;
	}

	// Distancia entre los objetos
	Vector2d direction = targetGameObject->position - visionCollider->getGameObject()->position;
	float distance = direction.getSqrLength();

	float sqrVisionRadius = visionCollider->getSqrVisionRadius();
	float sqrTargetRadius = targetCollider->getSqrCollisionRadius();

	// Dentro de vision
	if(distance <= sqrVisionRadius + sqrTargetRadius)
	{
		Collision collision;
		collision.collider = targetGameObject;
		collision.direction = direction;
		visionCollider->onVision(collision);
	}
}

void glDrawCairoSurface(const Cairo::RefPtr<Cairo::ImageSurface> surface,
			const Vector2d &min, const Vector2d &max,
			const double z)
{
  if (surface==0) return;
  int w = surface->get_width();
  int h = surface->get_height();
  unsigned char * data = surface->get_data();

  GLuint texture; glGenTextures( 1, &texture );
  glBindTexture( GL_TEXTURE_2D, texture );

  // http://www.nullterminator.net/gltexture.html
  // select modulate to mix texture with color for shading
  glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
  // when texture area is small, bilinear filter the closest mipmap
  glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
		   GL_LINEAR_MIPMAP_NEAREST );
  // when texture area is large, bilinear filter the original
  glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );

  // build our texture mipmaps
  gluBuild2DMipmaps( GL_TEXTURE_2D, GL_ALPHA, w, h,
		     GL_ALPHA, GL_UNSIGNED_BYTE, data );

  glEnable(GL_TEXTURE_2D);
  glBegin(GL_QUADS);
  glTexCoord2d(0.0,0.0); glVertex3d(min.x(),min.y(),z);
  glTexCoord2d(1.0,0.0); glVertex3d(max.x(),min.y(),z);
  glTexCoord2d(1.0,1.0); glVertex3d(max.x(),max.y(),z);
  glTexCoord2d(0.0,1.0); glVertex3d(min.x(),max.y(),z);
  glEnd();
  glDisable(GL_TEXTURE_2D);
  glDeleteTextures( 1, &texture );
}

Vector2d s_func(
    double &xi, double &eta, const Vector4d &x, const Vector4d &y) {

    double xip = 1 + xi;
    double xim = 1 - xi;
    double etap = 1 + eta;
    double etam = 1 - eta;

    Vector4d shp;
    shp(0) = xim * etam / 4.0;
    shp(1) = xip * etam / 4.0;
    shp(2) = xim * etap / 4.0;
    shp(3) = xip * etap / 4.0;

    Vector2d map;
    map.setZero();
    for (size_t i=0; i<4; i++) {

        map(0) += shp(i) * x(i);
        map(1) += shp(i) * y(i);

    }

    return map;

}

    bool contains(const Vector& x) const
    {
      // Deal with the null vector.
      if (x.squaredNorm() < _eps*_eps)
        return (_type & Blunt) != 0;

      // Otherwise decompose x w.r.t. to the basis.
      Vector2d theta;
      theta = _basis.fullPivLu().solve(x);
      double relError = (_basis*theta - x).squaredNorm() / x.squaredNorm();
      if (relError > _eps)
        return false;

      // Deal with the degenerate cases (Pointed cone).
      if (_type & Pointed)
      {
        if (_type & PositiveCosine && _type & Blunt)
          return theta.minCoeff() > -_eps;
        return (_type & Blunt) != 0;
      }

      // Generic case.
      double min_coeff = theta.minCoeff();
      if (_type & Convex)
        return min_coeff > _eps;
      return min_coeff > -_eps;
    }

void model::rotate(const Matrix2x2& M)
{
    Vector2d tmp;

    //rotate vertices
    for(uint i=0;i<v_size;i++){
        tmp.set(vertices[i].bk_p[0],vertices[i].bk_p[1]);
        tmp=M*tmp;
        vertices[i].p.set(tmp[0],tmp[1]);
    }
    
    //rotate edges
    for(uint i=0;i<e_size;i++){
        for(int j=0;j<2;j++){
            tmp.set(edges[i].bk_in_n[j][0],edges[i].bk_in_n[j][1]);
            tmp=M*tmp;
            edges[i].in_n[j].set(tmp[0],tmp[1]);
        }
        
        tmp.set(edges[i].v[0],edges[i].v[1]);
        tmp=M*tmp;
        edges[i].v.set(tmp[0],tmp[1]);
    }

    //rotate facets
    for(uint i=0;i<t_size;i++){
        tmp.set(tris[i].n[0],tris[i].n[1]);
        tmp=M*tmp;
        tris[i].n.set(tmp[0],tmp[1]);
    }
}

//------------------------------------------------------------------------------
void Projectile::updateState()
{
  Vector2d v = getVelocity();
  if( isEqual( v.x(), 0.0 ) )
    setState( sVertical );
  else
    setState( sHorizontal );
  
  switch( getType() )
  {
  case ptBasic:
  	if( getRemainingLife() <= 0 )
  	{ setState( sExploding ); }
  break;
  case ptBullet:
  {
    if( hasIntersections() || getRemainingLife() <= 0)
  	{ setState( sExploding ); }
  }
  break;
  case ptGrenade:
  {
  	if( getRemainingLife() <= 0 )
    {
      setState( sExploding );      
      Physics& p = mpEngine->getPhysics();
      p.explode( getPosition(), 4 * 32, getExplosionDamage(), *mpEngine );
    }
  }break;
  default: break;
  }
}

   bool operator()(const Edge& lhs, const Edge& rhs) const throw()
   {
      Vector2d lhv = m_center - Vector2d(lhs.start.x, lhs.start.y);
      Vector2d rhv = m_center - Vector2d(rhs.start.x, rhs.start.y);

      return lhv.PolarAngle() < rhv.PolarAngle();
   }

	double Vector2d::angle(Vector2d& other)
	{
		double value = m_x * other.getX() + m_y * other.getY();
		double radians = acos(value);
		double degrees = radians * (180 / 3.14159265);
		return degrees;
	}

void addPoint() {
    Vector2d v;
    string m1,m2; 
    string s = a[2];
    double x,y;
    int pos;
    pos=s.find(',',0);
    if(pos!=std::string::npos) {
        m1 = s.substr(0,pos);
        m2 = s.substr(pos+1,s.size()-pos);
        x = stringToNum<double>(m1);
        y = stringToNum<double>(m2);
     } else {
         cout<<"direction error please write like this"<<endl;
         cout<<"p2 1 5,6"<<endl;
         exit(0); 
     }
     v<<x,y;
     
    point[pcnt].setPoint(v);
    point[pcnt].setName(a[0]);
    pcnt++;
    cout<<"add a dot :"<<a[0]<<endl;
    cout<<"location is:"<<v.transpose()<<endl; 
    
}

double signedDistanceInsideConvexHull(
    const Ref<const Matrix<double, 2, Dynamic>> &pts,
    const Ref<const Vector2d> &q) {
  std::vector<Point> hull_pts = convexHull(eigenToPoints(pts));
  double d_star = std::numeric_limits<double>::infinity();

  Matrix2d R;
  R << 0, 1, -1, 0;

  for (int i = 0; i < (static_cast<int>(hull_pts.size()) - 1); ++i) {
    Vector2d ai = R * Vector2d(hull_pts[i + 1].x - hull_pts[i].x,
                               hull_pts[i + 1].y - hull_pts[i].y);
    double b = ai.transpose() * Vector2d(hull_pts[i].x, hull_pts[i].y);
    double n = ai.norm();
    if (std::isnormal(n)) {
      ai = ai.array() / n;
      b = b / n;
      double d = b - ai.transpose() * q;
      // std::cout << "pt0: " << hull_pts[i].x << " " << hull_pts[i].y <<
      // std::endl;
      // std::cout << "pt1: " << hull_pts[i+1].x << " " << hull_pts[i+1].y <<
      // std::endl;
      // std::cout << "ai: " << ai.transpose() << std::endl;
      // std::cout << "b: " << b << std::endl;
      // std::cout << "d: " << d << std::endl;
      if (d < d_star) {
        d_star = d;
      }
    }
  }
  return d_star;
}

static int AngleToBezier(Point2d* pts, float radius)
{
    const Vector2d vec1 (pts[1] - pts[0]);      // 第一条边
    const Vector2d vec2 (pts[2] - pts[1]);      // 第二条边

    const float dHalfAngle = 0.5f * fabs(vec1.angleTo2(vec2));  // 夹角的一半
    if (dHalfAngle < 1e-4f || fabs(dHalfAngle - _M_PI_2) < 1e-4f)  // 两条边平行
        return 0;

    const float dDist1 = 0.5f * vec1.length();
    const float dDist2 = 0.5f * vec2.length();
    float dArc = radius / tan(dHalfAngle);    // 圆弧在边上的投影长度
    if (dArc > dDist1 || dArc > dDist2)
    {
        float dArcOld = dArc;
        dArc = mgMin(dDist1, dDist2);
        if (dArc < dArcOld * 0.5f)
            return 3;
    }

    int count = 0;
    Point2d ptCenter, ptStart, ptEnd;
    float startAngle, sweepAngle;

    ptStart = pts[1].rulerPoint(pts[0], dArc, 0);
    ptEnd = pts[1].rulerPoint(pts[2], dArc, 0);
    if (mgArcTan(ptStart, ptEnd, pts[1] - ptStart, 
        ptCenter, radius, &startAngle, &sweepAngle))
    {
        count = mgAngleArcToBezier(
            pts, ptCenter, radius, radius, startAngle, sweepAngle);
    }

    return count;
}

/*
 * Iterate the vertices, and calculate whether this
 * shape is a hole or enclosed, based on whether the
 * segment normals point outward (a hole) or inward
 * (enclosing)
 */
void Poly::calcHole() const // hole is mutable
{
  	if(vertices.size() < 3)
	  return;	// hole is undefined
	Vector2d p(-INFTY, -INFTY);
	int v=0;
	center = Vector2d(0,0);
	Vector2d q;
	for(size_t vert=0;vert<vertices.size();vert++)
	{
	  q = vertices[vert];
	  center += q;
	  if(q.x() > p.x())
	    {
	      p = q;
	      v=vert;
	    }
	  else if(q.x() == p.x() && q.y() > p.y())
	    {
	      p.y() = q.y();
	      v=vert;
	    }
	}
	center /= vertices.size();

	// we have the x-most vertex (with the highest y if there was a contest), v
	Vector2d V1 = getVertexCircular(v-1);
	Vector2d V2 = getVertexCircular(v);
	Vector2d V3 = getVertexCircular(v+1);

	// Vector2d Va=V2-V1;
	// Vector2d Vb=V3-V2;
	hole = isleftof(V2, V3, V1); //cross(Vb,Va) > 0;
	holecalculated = true;
}

void StringElement::buildParticlesAndSprings() {
	Vector2d xAxis = getEndPosition() - getStartPosition();
	double length = xAxis.norm();
	int requiredMasses = static_cast<int>(ceil(length / stdMaxMassDistance) + 1.0);
	double massDistance = length / requiredMasses;

	xAxis.normalize();

	for (int i = 0; i < requiredMasses; i++) {
		Vector2d massPosition = getStartPosition() + (xAxis * (i * massDistance));
		particles.push_back(createParticle(stdMassValue, stdMassRadius, massPosition));
	}

	for (int i = 0; i < requiredMasses - 1; i++) {
		springs.push_back(new Spring(*particles[i], *particles[i + 1], stdK));
	}

	if (getFromConnectionPoint().isConnected()) {
		ConnectionPoint::particle_list connectionParticles = getFromConnectionPoint().getConnectionParticles();
		for (ConnectionPoint::particle_list::iterator it = connectionParticles.begin(); it != connectionParticles.end(); ++it) {
			springs.push_back(new Spring(*particles[0], **it, stdK));
		}
	}

	if (getToConnectionPoint().isConnected()) {
		ConnectionPoint::particle_list connectionParticles = getToConnectionPoint().getConnectionParticles();
		for (ConnectionPoint::particle_list::iterator it = connectionParticles.begin(); it != connectionParticles.end(); ++it) {
			springs.push_back(new Spring(*particles[particles.size() - 1], **it, stdK));
		}
	}

}

// 求本矢量投影到矢量xAxis上的垂直距离
// 在xAxis的逆时针方向时返回正值，顺时针则返回负值
float Vector2d::distanceToVector(const Vector2d& xAxis) const
{
    float len = xAxis.length();
    if (len < _MGZERO)
        return length();
    return xAxis.crossProduct(*this) / len;
}