bool c_CurveClustering::show_curves(const t_CurveSet &_curve_set,
                                    const size_t _height, const size_t _width,
                                    const std::string _file_name)
{
    Mat_<Vec3b> image = Mat_<Vec3b>::zeros(_height, _width);
    Scalar color(255, 255, 255);
    t_CurveSet::const_iterator cit = _curve_set.begin();
    t_CurveSet::const_iterator cit_end = _curve_set.end();
    for (; cit != cit_end; ++cit)
    {
        const t_PointSet &samples = (*cit)->get_samples();
        t_PointSet::const_iterator cit_p = samples.begin();
        t_PointSet::const_iterator cit_p_end = samples.end() - 1;
        for (; cit_p != cit_p_end; ++cit_p)
        {
            line(image, Point2d(cit_p->X(), cit_p->Y()),
                 Point2d((cit_p + 1)->X(), (cit_p + 1)->Y()), color);
        }
    }

    imshow(_file_name.c_str(), image);
    waitKey();
    imwrite(_file_name.c_str(), image);
    return true;
}

  bool OCVLbp::InitializeLbp() {
    options.crop.clear();
    options.write_image = false;    

    options.lbptype = LBP;
    options.cslbp_threshold = 0.0;
      
    options.nneighbors = 8;
    options.neighborhood = LBP_8NEIGHBORS;
    options.radius = 2.0;

    options.overlapping_blocks = false;

    options.mapping = LBP_MAPPING_NONE;
    lbp_map.mapsize = 0;

    options.normalize_histogram = HISTNORM_NONE;
    
    options.do_funneling = false;    
    funneling_modelfile = "";

    options.do_illnorm = false;
    options.cropfirst = true;

    spoints_min = Point2d(0.0,0.0);
    spoints_max = Point2d(0.0,0.0);

    return true;
  }

void WorldDrawer2d::DrawCircle(Object2d ** o1, float r,float g,float b){
	

	std::vector<Point2d> points;
	std::vector<int> topology;
	points.push_back(Point2d(0,0));
	for(int i = 0; i < 360; i++){
		float x = sin(3.1415f * i / 180) * 1;
		float y = cos(3.1415f * i / 180) * 1;
		points.push_back(Point2d(x,y));
	}
	topology.push_back(0);
	topology.push_back(1);
	topology.push_back(2);
	topology.push_back(2);
	topology.push_back(3);
	topology.push_back(0);
	for( int i = 3; i < points.size()-1; i++){
		topology.push_back(i);
		topology.push_back(i+1);
		topology.push_back(0);
	}
	topology.push_back(points.size()-1);
	topology.push_back(1);
	topology.push_back(0);
	(*o1) = new Object2d(points,topology);
	(*o1)->setcolor(r,g,b);
	cs1->objectAdd(*o1);


}

/* metoda care creeaza un nou joc atunci cand acesta s-au tereminat */
void Game::restart() {
	int width = glutGet(GLUT_WINDOW_WIDTH);
	int height = glutGet(GLUT_WINDOW_HEIGHT);
	vector<Point2d> points;
	vector<int> topology;
	CoordinateSystem2d *cs = new CoordinateSystem2d();

	points.push_back(Point2d(-width, -height));
	points.push_back(Point2d(-width, height));
	points.push_back(Point2d(width, -height));
	points.push_back(Point2d(width, height));
	topology.push_back(0);
	topology.push_back(1);
	topology.push_back(2);
	topology.push_back(1);
	topology.push_back(2);
	topology.push_back(3);

	Object2d *obj = new Object2d(points, topology);
	obj->setcolor(0.5, 0.4, 0.3);
	
	cs_used.clear();
	cs->objectAdd(obj);
	cs_used.push_back(cs);
}	

bool c_CurveClustering::show_clusters(const t_ClusterSet &_cluster_set,
                                      const size_t _height,
                                      const size_t _width,
                                      const std::string _file_name)
{
    Mat_<Vec3b> image = Mat_<Vec3b>::zeros(_height, _width);
    srand(int(time(0)));

    t_ClusterSet::const_iterator cit = _cluster_set.begin();
    t_ClusterSet::const_iterator cit_end = _cluster_set.end();
    for (size_t i = 0; cit != cit_end; ++cit, ++i)
    {
        Scalar color(rand() % 255, rand() % 255, rand() % 255);
        std::vector<int>::const_iterator cit_i = cit->m_i_data.begin();
        std::vector<int>::const_iterator cit_i_end = cit->m_i_data.end();
        for (; cit_i != cit_i_end; ++cit_i)
        {
            const t_PointSet &samples = (*m_p_curve_set)[*cit_i]->get_samples();
            t_PointSet::const_iterator cit_p = samples.begin();
            t_PointSet::const_iterator cit_p_end = samples.end() - 1;
            for (; cit_p != cit_p_end; ++cit_p)
            {
                line(image, Point2d(cit_p->X(), cit_p->Y()),
                     Point2d((cit_p + 1)->X(), (cit_p + 1)->Y()), color);
            }
        }
    }
#if 1
    imshow(_file_name.c_str(), image);
    waitKey();
#endif
    imwrite(_file_name.c_str(), image);

    return true;
}

Point2d CameraPinhole::Project(const Point3d& p3d)
{
#ifdef __SSE__
    if(p3d.z==1.)
    {
        __m128d xy = _mm_setr_pd(p3d.x,p3d.y);
        xy=_mm_add_pd(_mm_setr_pd(cx,cy),_mm_mul_pd(xy,(__m128d){fx,fy}));
        return *(Point2d*)&xy;
    }
    else if(p3d.z>0)
    {
        double z_inv=1./p3d.z;
        return Point2d(fx*z_inv*p3d.x+cx,fy*z_inv*p3d.y+cy);
    }
    else return Point2d(-1,-1);
#else
    if(p3d.z==1.)
    {
        return Point2d(fx*p3d.x+cx,fy*p3d.y+cy);
    }
    else if(p3d.z>0)
    {
        double z_inv=1./p3d.z;
        return Point2d(fx*z_inv*p3d.x+cx,fy*z_inv*p3d.y+cy);
    }
    else return Point2d(-1,-1);
#endif
}

bool CameraProjections::GetOnImageCordinate(const vector<Point2f> contour,
		vector<Point> &resCountour)
{
	if (contour.size() < 1)
	{
		ROS_ERROR("Error In Programming");
		return false;
	}
	vector<Point2f> resC, resCountourd, contourShiftAndScale;
	vector<Point> resCI;

	for (uint32_t i = 0; i < contour.size(); i++)
	{
		Point2f f = Point2d(contour[i].x * 100., contour[i].y * 100.);
		f = Point2d(
				f.x / params.topView.scale->get()
						+ ((params.topView.width->get()
								/ params.topView.scale->get()) / 2.),
				(f.y / params.topView.scale->get())
						+ ((params.topView.width->get()
								/ params.topView.scale->get()) / 2.));
		contourShiftAndScale.push_back(f);
	}

	perspectiveTransform(contourShiftAndScale, resC, realHomoBack);

	for (uint32_t i = 0; i < resC.size(); i++)
	{
		resCI.push_back(Point((int) resC[i].x, (int) resC[i].y));
	}
	return _distorionModel.DistortP(resCI, resCountour);

}

SpaceShip::SpaceShip() : body(Point2d(INITIAL_X, INITIAL_Y), Point2d(INITIAL_X+BODY_WIDTH, INITIAL_Y+BODY_HEIGHT)), 
						cannon(
							Point2d(INITIAL_X+(BODY_WIDTH-CANON_WIDTH)/2, INITIAL_Y+BODY_HEIGHT),
							Point2d(INITIAL_X+(BODY_WIDTH+CANON_WIDTH)/2, INITIAL_Y+BODY_HEIGHT+CANON_HEIGHT)
						),
						bullets(),
						lastTime(getMilliCount()),
						alive(true) {}

void LineHelpersTest::testPointLeftMiddleLeft()
{
	float weight = -1;
	Point2d res = getCorrespPoint(straightLine, Point2d(.5, 1), true, 1, weight, 0.01);
	double dist = norm(res - Point2d(.5, 1));
	QString err = getErrStr(Point2d(.5, 1), res, 1, weight);
	QVERIFY2((weight != 0 && dist < .1), err.toAscii().data());
}

Point2d Point2d::rulerPoint(const Point2d& dir, float yoff) const
{
    float len = distanceTo(dir);
    if (len < _MGZERO) {
        return Point2d(x, y + yoff);
    }
    yoff /= len;
    return Point2d(x - (dir.y - y) * yoff, y + (dir.x - x) * yoff);
}

void WorldDrawer2d::MakeX(float r,float g, float b,Object2d* z1,Object2d* z2,int sens){
	DrawFour(0.25f,2.0f,&z1,r,g,b);
	DrawFour(0.25,2.0f,&z2,r,g,b);
	(z1)->rotateRelativeToPoint(Point2d(0.0f,0.0f),3.1415f/4);
	(z1)->translate(16.0f,0.0f);
	(z2)->rotateRelativeToPoint(Point2d(0.0f,0.0f),-3.1415f/4);
	(z2)->translate(16.0f,0.0f);

}

void DialogRange::on_cdButton_clicked()
{
    //gp->calPlaneEquation();
    //gp->saveGroundParam();
    gp->readGroundParam();
    cout<<gp->getPointAtGround(Point2d(285,327))<<endl;
    cout<<gp->getPointAtGround(Point2d(285,294))<<endl;


}

	Arc ArcWithDirectionAndAngle(const Point2d &center, float radius, const vsr::cga2D::Vec &direction, float angle) {
		auto theta = std::atan2(direction[1], direction[0]);
		auto theta1 = theta - angle * 0.5f;
		auto theta2 = theta1 + angle;
		auto r = std::abs(radius);
		auto pt1 = center + Point2d(std::cos(theta1), std::sin(theta1)) * r;
		auto pt2 = center + Point2d(std::cos(theta2), std::sin(theta2)) * r;
		auto circle = Circle{center, radius};
		auto endpoints = std::signbit(radius) ? LineSegment{pt2, pt1} : LineSegment{pt1, pt2};
		return Arc{circle, endpoints};
	}

BasicButton::BasicButton(char* name, sf::Font &font, sf::Color textColor, sf::Color buttonColor, unsigned int size, Point2d position): 
	backgroundColor(buttonColor), buttonText (name,font,size)
{
	buttonText.setPosition(position.x,position.y);
	buttonText.setColor(textColor);
	double w = buttonText.getGlobalBounds().width;
	double h = buttonText.getGlobalBounds().height;
	Point2d textCenter = Point2d(buttonText.getGlobalBounds().left+0.5*w,buttonText.getGlobalBounds().top+0.5*h);
	hitbox.min = textCenter-Point2d(w*1.05+h*0.05,h*1.05+w*0.05);
	hitbox.max = textCenter+Point2d(w*1.05+h*0.05,h*1.05+w*0.05);
}

LineSegment LineSegment::PerpendicularLineSegment(double scale)
{
	double angle = GetRadianFromX();
	angle += M_PI / 2;
	Point2d mid = GetMiddle();
	double len = GetLength() / 2;
	len *= scale;
	double x1 = mid.x + cos(angle) * len;
	double y1 = mid.y + sin(angle) * len;
	double x2 = mid.x - cos(angle) * len;
	double y2 = mid.y - sin(angle) * len;
	return LineSegment(Point2d(x1, y1), Point2d(x2, y2));
}

void GcGraphView::draw(GiGraphics& gs)
{
    int gridType = cmdView()->getOptionInt("showGrid", 0);
    if (gridType < 1 || gridType > 2 || gs.xf().getViewScale() < 0.05f)
        return;
    
    Box2d rect(gs.xf().getWndRectW());
    GiContext ctx(0, GiColor(127, 127, 127, gridType == 2 ? 48 : 20));
    
    if (gridType == 1) {
        GiContext ctx5(0, GiColor(127, 127, 127, 48));
        float x = mgbase::roundReal(rect.xmin, -1) - 10;
        float y = mgbase::roundReal(rect.ymin, -1) - 10;
        int i = mgRound(x) / 10;
        
        for (; x < rect.xmax + 10; x += 10) {
            gs.drawLine(i++ % 5 ? &ctx : &ctx5, Point2d(x, rect.ymin), Point2d(x, rect.ymax), false);
        }
        i = mgRound(y) / 10;
        for (; y < rect.ymax + 10; y += 10) {
            gs.drawLine(i++ % 5 ? &ctx : &ctx5, Point2d(rect.xmin, y), Point2d(rect.xmax, y), false);
        }
    }
    else if (gridType == 2) {
        for (float x = rect.xmin - 10; x < rect.xmax + 10; x += 10) {
            for (float y = rect.ymin - 10; y < rect.ymax + 10; y += 10) {
                gs.drawLine(&ctx, Point2d(x, y - 0.5f), Point2d(x, y + 0.5f), false);
                gs.drawLine(&ctx, Point2d(x - 0.5f, y), Point2d(x + 0.5f, y), false);
            }
        }
    }
    
    GcBaseView::draw(gs);
}

// Generates a random position for portal
Point2d Labyrinth::generateRandomPosition()
{
	int x, y, dim = size * 2 + 1;
	bool wall;

	do
	{
		x = rand() % dim;
		y = rand() % dim;
		wall = isOnWall(Point2d(x, y));
	} while (wall == true);
	
	return Point2d(x, y);
}

void WorldDrawer2d::onKey(unsigned char key) {
    switch(key) {
    case KEY_UP:
        flag_minge = true;
        for (int i = 0; i < 12; i++)
            if (e[i] == 1)
            {
                trX = a - axa_x[i];
                trY = b - axa_y[i];
            }
        if (abs(trX) > 0.1 || abs(trY) > 0.1)
        {
            trX = trX / 30;
            trY = trY / 30;
        }
        break;
    case KEY_DOWN:
        flag_minge = true;
        for (int i = 0; i < 12; i++)
            if (e[i] == 1)
            {
                trX = a - axa_x[i];
                trY = b - axa_y[i];
            }
        if (abs(trX) > 0.1 || abs(trY) > 0.1)
        {
            trX = trX / 30;
            trY = trY / 30;
        }
        break;
    case KEY_LEFT:
        for (int i = 0; i < 12; i++)
            if (e[i] == 1)
                minge->rotateRelativeToPoint(Point2d(axa_x[i], axa_y[i]), 0.1f);
        break;
    case KEY_RIGHT:
        for (int i = 0; i < 12; i++)
            if (e[i] == 1)
            {
                minge->rotateRelativeToPoint(Point2d(axa_x[i], axa_y[i]), -0.1f);
            }
        break;
    case KEY_SPACE:
        animation=!animation;
        break;
    default:
        break;
    }
}

void mgnear::getRectHandle(const Box2d& rect, int index, Point2d& pt)
{
    switch (index)
    {
    case 0: pt = rect.leftTop(); break;
    case 1: pt = rect.rightTop(); break;
    case 2: pt = rect.rightBottom(); break;
    case 3: pt = rect.leftBottom(); break;
    case 4: pt = Point2d(rect.center().x, rect.ymax); break;
    case 5: pt = Point2d(rect.xmax, rect.center().y); break;
    case 6: pt = Point2d(rect.center().x, rect.ymin); break;
    case 7: pt = Point2d(rect.xmin, rect.center().y); break;
    default: pt = rect.center(); break;
    }
}

Point2d foot_p2line(Point2d p, Segment2d seg){
	if (abs(seg.p1.x - seg.p2.x) < EPS_HERE)
	{
		return Point2d(seg.p1.x, p.y);
	}
	if (seg.p2.y - seg.p1.y < EPS_HERE)
	{
		return Point2d(p.x, seg.p1.y);
	}

	double k = seg.getSlope();
	double foot_x = (k * k * seg.p1.x + k * (p.y - seg.p1.y) + p.x) / (k * k + 1);
	double foot_y = k * (foot_x - seg.p1.x) + seg.p1.y;
	return Point2d(foot_x, foot_y);
}