bool Enemy::tick(){
    if(Game::enemiesRoute.size()<=_nextPositionIndex
    || _nextPositionIndex<0 || _distanceToEnd<=_velocity)
        return true;

    Vec2d p = Game::enemiesRoute[_nextPositionIndex];
    double toMove = _velocity;
    _distanceToEnd -= toMove;
    while(toMove>0.0){
        double d = _position.distance(p);
        if(d<=_velocity){
            _position = p;
            _nextPositionIndex += 1;
            /*if(Game::enemiesRoute.size()<=_nextPositionIndex){
                _nextPositionIndex = -1;
                return true;
            }*/
            toMove -= d;
            p = Game::enemiesRoute[_nextPositionIndex];
        }else{
            Vec2d t = p-_position;
            t.normalize();
            t *= _velocity;
            _position += t;
            toMove = 0.0;
        }
    }
    return false;
}

Transform* PatchSet::createPatch(const std::string& filename, PatchOptions* poptions)
{
    Patch* patch = new Patch;
    patch->setPatchSet(this);
    Vec2d ll, ur;
    poptions->getPatchExtents(ll, ur);
    Vec2d range = (ur - ll);
    ref_ptr<Patch::Data> data = new Patch::Data;
    int patchDim = _resolution + 1;
    Vec3Array* verts = new Vec3Array(patchDim * patchDim);
    for (int j = 0; j < patchDim; ++j)
        for (int i = 0; i < patchDim; ++i)
            (*verts)[patchDim * j + i]
                = Vec3((ll.x() + i * range.x()
                        / static_cast<float>(_resolution)) * 81920.0,
                       (ll.y() + j * range.y()
                        / static_cast<float>(_resolution)) * 81920.0,
                       0.0);
    data->vertexData.array = verts;
    data->vertexData.binding = Geometry::BIND_PER_VERTEX;
    Vec3Array* norms = new Vec3Array(1);
    (*norms)[0] = Vec3d(0.0, 0.0, 1.0);
    data->normalData.array = norms;
    data->normalData.binding = Geometry::BIND_OVERALL;
    Vec4Array* colors = new Vec4Array(1);
    (*colors)[0] = Vec4(1.0, 1.0, 1.0, 1.0);
    data->colorData.array = colors;
    data->colorData.binding = Geometry::BIND_OVERALL;
    patch->setData(data);
    MatrixTransform* transform = new MatrixTransform;
    transform->addChild(patch);
    return transform;
}

void	Voronoi::CheckCircle(VParabola * b){

	VParabola * lp = VParabola::GetLeftParent (b);
	VParabola * rp = VParabola::GetRightParent(b);

	VParabola * a  = VParabola::GetLeftChild (lp);
	VParabola * c  = VParabola::GetRightChild(rp);

	if(!a || !c || a->site == c->site) return;

	Vec2d * s = 0;
	s = GetEdgeIntersection(lp->edge, rp->edge);
	if(s == 0) return;

	double dx = a->site->x - s->x;
	double dy = a->site->y - s->y;

	double d = std::sqrt( (dx * dx) + (dy * dy) );

	if(s->y - d >= ly) { return;}

    Vec2d * newv = new Vec2d(); newv->set( s->x,  s->y - d );
    VEvent * e = new VEvent( newv, false);
	//VEvent * e = new VEvent( new Vec2d(s->x, s->y - d), false);
	points.push_back(e->point);
	b->cEvent = e;
	e->arch = b;
	queue.push(e);

}

//Add of Marie
bool VRWorkpieceElement::doesCollide(Vec3d position) {
    //position in argument is the lowest part of the worktool and the (0,0) point of the profile

    float py = this->position[1], px = this->position[0], pz = this->position[2];
    float sy = this->size[1], sx = this->size[0], sz = this->size[2];
    float ptooly = position[1], ptoolx = position[0], ptoolz = position[2];

    float profileLength = workpiece.cuttingProfile->getLength();

    if ((py + sy/2.0f > ptooly) && (py - sy/2.0f < ptooly + profileLength))
    {
        float maximum = workpiece.cuttingProfile->maxProfile(position, this->position, this->size);

        if ((ptoolz - maximum < pz + sz/2.0f) && (ptoolz + maximum > pz - sz/2.0) &&
            (ptoolx - maximum < px + sx/2.0f) && (ptoolx + maximum > px - sx/2.0))
        {
            float diffx = ptoolx - px, diffz = ptoolz - pz;
            if (std::abs(diffx) <= sx / 2.0f) return true;
            if (std::abs(diffz) <= sz / 2.0f) return true;

            float sgnx = sgn(diffx), sgnz = sgn(diffz);
            Vec2d vertex(px + sgnx * sx / 2.0f, pz + sgnz * sz / 2.0f);
            Vec2d toolMid(ptoolx, ptoolz);
            Vec2d distanceVertexTool = vertex - toolMid;
            if (distanceVertexTool.length() < maximum) {
                return true;
            }
        }
    }

    return false;
}

void draw() {

    //	draw Points
    glColor3f( 0.3,0.3,0.3 );
    for ( int i=0; i<n; i++ ) {
        draw_Vec2d( ps[i] );
    };
    //	draw map.points.hits
    for ( int i=0; i<map.points.capacity; i++ ) {
        draw_Segment2d( {-0.5+i*pixsz,-0.5}, { -0.5+i*pixsz, -0.5	+ map.points.hits[i] * 3*pixsz } );
    }

    Vec2d pmouse;
    pmouse.set( (mouseX-WIDTH_HALF)*pixsz, (HEIGHT_HALF-mouseY)*( pixsz*ASPECT_RATIO ) );
    int ix    = map.getIx( pmouse.x );
    int iy    = map.getIy( pmouse.y );
    int index = map.getBoxIndex( ix, iy );
    //colorHash( map.getBoxIndex( ix, iy )  );
    glColor3f( 0.9,0.7,0.8 );
    draw_Rect2d( { map.getX(ix), map.getY(iy) }, { map.getX(ix+1), map.getY(iy+1) } );
    //printf( " rect X Y %i %i %f %f \n", ix, iy, map.getX(ix), map.getY(iy) );
    //printf( " mouse X Y %i %i \n", mouseX, mouseY );
    paint_Vec2d( pmouse, 0.01 );
    int nloaded = map.loadPointsIn( pmouse );
    for ( int i=0; i<nloaded; i++ ) {
        int ii = map.tempi[i];
        Vec2d* pp = map.points.store[ ii ];
        paint_Vec2d( *pp, 0.0025 );
    };
    if( index != lastIndex ) printf( " ix iy index %i %i %i iters %i \n", ix, iy, index, map.points.DEBUG_counter  );
    lastIndex = index;

};

void NBodyWorldApp::pickParticle( Particle2D*& picked ){
    Particle2D* screenObjects[256];
    // mouse picking
    double rmax  = 2.0d;
    double r2max = rmax*rmax;
    Vec2d vmouse;
    vmouse.set( mouse_begin_x, mouse_begin_y );
    UINT mfound = world.map.getObjectsInRect( (float)(vmouse.x - rmax ), (float)(vmouse.y - rmax ), (float)(vmouse.x + rmax ), (float)(vmouse.y + rmax ), &(screenObjects[0]) );
    //printf( "mfound  %i \n", mfound );
    int imin = -1;
    double r2min = 1e+300;
    for( int i=0; i<mfound; i++ ){
        Particle2D* p = screenObjects[i];
        Vec2d d;
        d.set_sub( p->pos, vmouse );
        double r2 = d.norm2();
        if( r2 < r2max ){
            if( r2 < r2min ){  r2min = r2; imin = i; }
        }
        //printf( " r2 %3.3f r2max %3.3f r2min %3.3f imin %i \n", r2, r2max, r2min, imin );
    }
    if( imin >= 0 ){
        picked = screenObjects[imin];
    }else{
        picked = NULL;
    }
    //if( world.picked != NULL ) printf( " MOUSE picked (%f,%f) \n", world.picked->pos.x, world.picked->pos.y );
}

 bool mouseMotion(GLWindow *win, int x, int y, int buttons) {
     Vec2d pos = Vec2d(x - win->w / 2, -(y - win->h / 2)) / win->w;
     pos *= m_fieldWidth;
     m_mouseAngle = pos.getAngle();
     m_mouseIn = pos.mag() <= m_circleRad;
     m_pt = pos;
     return true;
 }

float netrule :: CalcLineError (int li, const Vec2d & v) const
{
  float dx = v.X() - linevecs.Get(li).X();
  float dy = v.Y() - linevecs.Get(li).Y();

  const threefloat * ltf = &linetolerances.Get(li);
  return ltf->f1 * dx * dx + ltf->f2 * dx * dy + ltf->f3 * dy * dy;
}

double Algebra::slope( const Vec2d &b, const Vec2d &e )
{
   Vec2d dxdy;
   dxdy.diff(e, b);
   if (absolute(dxdy.x) < EPS)
      return PI_2;
   else
      return atan((double) dxdy.y / dxdy.x);
}

double Angle (const Vec2d & v)
{
  if (v.X() == 0 && v.Y() == 0)
    return 0;
    
  double ang = atan2 (v.Y(), v.X());
  if (ang < 0) ang+= 2 * M_PI;
  return ang;
}

int MoleculeWorldApp::pickMol( const Vec2d& mpos ){
    int    imin  = -1;
    double r2min = 1e+300;
    for(int i=0; i<world.nMols; i++){
        Vec2d d; d.set_sub( mpos, world.pos[i] );
        double r2 = d.norm2();
        if (r2<r2min){ r2min=r2; imin=i; }
    }
    return imin;
}

void Enemy::reloadDistanceToEnd(){
    if(_nextPositionIndex<0 || _nextPositionIndex>=Game::enemiesRoute.size()){
        _distanceToEnd = -1;
    }else{
        double d = 0;
        Vec2d p = _position;
        for(int i=_nextPositionIndex; i<Game::enemiesRoute.size(); i++)
            d += p.distance(Game::enemiesRoute[i]);
        _distanceToEnd = d;
    }
}

bool World::collidesWithQuads(vector<Quad*>* quads) {
    for (auto quad : *quads) {
        for (int i = 0; i < quad->getVertexCount(); i++) {
            Vec2d* vertex = quad->getVertices()[i];
            if (vertex->getY() < 0 || vertex->getY() > bounds->getY() || vertex->getX() < 0 || vertex->getX() > bounds->getX()) {
                cout << "Collision with world " << vertex->toString() << endl;
                return true;
            }
        }
    }
    return false;
}

bool World::resetTet(char letter) {
    TetrominoType* type = typeFactory->getType(letter);

    Vec2d* newCoords = new Vec2d(spawnPoint);
    int spawnOffset = type->getHeight();
    newCoords->setY(newCoords->getY() - spawnOffset);
    
    activeTet = tetFactory->getTetromino(type, newCoords)->withDirection(defaultDirection);
    nextTet = tetFactory->getTetromino(type, newCoords)->withDirection(defaultDirection);
    
    return !heap->collidesWithQuads(activeTet->getQuads());
}

void	Voronoi::InsertParabola(Vec2d * p){
	if(!root){root = new VParabola(p); return;}

	if(root->isLeaf && root->site->y - p->y < 1){ // degenerovaný pripad - obì spodní místa ve stejné výšce
		Vec2d * fp = root->site;
		root->isLeaf = false;
		root->SetLeft( new VParabola(fp) );
		root->SetRight(new VParabola(p)  );
		//Vec2d * s = new Vec2d( (p->x + fp->x)/2, height ); // zaèátek hrany uprostøed míst
		Vec2d * s = new Vec2d();   s->set( (p->x + fp->x)/2, height ); // zaèátek hrany uprostøed míst
		points.push_back(s);
		if(p->x > fp->x) root->edge = new VEdge(s, fp, p); // rozhodnu, který vlevo, který vpravo
		else root->edge = new VEdge(s, p, fp);
		edges->push_back(root->edge);
		return;
	}

	VParabola * par = GetParabolaByX(p->x);

	if(par->cEvent)	{
		deleted.insert(par->cEvent);
		par->cEvent = 0;
	}

	//Vec2d * start = new Vec2d(p->x, GetY(par->site, p->x));
    Vec2d * start = new Vec2d(); start->set( p->x, GetY(par->site, p->x) );
	points.push_back(start);

	VEdge * el = new VEdge(start, par->site, p);
	VEdge * er = new VEdge(start, p, par->site);

	el->neighbour = er;
	edges->push_back(el);

	// pøestavuju strom .. vkládám novou parabolu
	par->edge = er;
	par->isLeaf = false;

	VParabola * p0 = new VParabola(par->site);
	VParabola * p1 = new VParabola(p);
	VParabola * p2 = new VParabola(par->site);

	par->SetRight(p2);
	par->SetLeft(new VParabola());
	par->Left()->edge = el;

	par->Left()->SetLeft(p0);
	par->Left()->SetRight(p1);

	CheckCircle(p0);
	CheckCircle(p2);
}

Vec2d MaiBody::resolveForce(const MaiBody &body) const
{
	if (this == &body) { return Vec2d(0, 0); } // no force is here.

	Vec2d r = (position_ - body.getPosition());
	float r_len = r.length();

	double GMm = Constants::kGaes * mass_ * body.getMass();

	return Vec2d(
		-GMm * (r.x / (r_len * r_len * r_len)),
		-GMm * (r.y / (r_len * r_len * r_len))
	);
}

void	Voronoi::FinishEdge(VParabola * n){
	if(n->isLeaf) {delete n; return;}
	double mx;
	if(n->edge->direction->x > 0.0){ mx = std::max(width, n->edge->start->x + 10 ); }
	else                           { mx = std::min(0.0,   n->edge->start->x - 10 ); }

	//Vec2d * end = new Vec2d(mx, mx * n->edge->f + n->edge->g);
	Vec2d * end = new Vec2d(); end->set( mx,  mx * n->edge->f + n->edge->g );
	n->edge->end = end;
	points.push_back(end);

	FinishEdge(n->Left() );
	FinishEdge(n->Right());
	delete n;
}

static std::vector<Vec2d> make_one_period(double width, double scaleFactor, double z_cos, double z_sin, bool vertical, bool flip)
{
    std::vector<Vec2d> points;
    double dx = M_PI_4; // very coarse spacing to begin with
    double limit = std::min(2*M_PI, width);
    for (double x = 0.; x < limit + EPSILON; x += dx) {  // so the last point is there too
        x = std::min(x, limit);
        points.emplace_back(Vec2d(x,f(x, z_sin,z_cos, vertical, flip)));
    }

    // now we will check all internal points and in case some are too far from the line connecting its neighbours,
    // we will add one more point on each side:
    const double tolerance = .1;
    for (unsigned int i=1;i<points.size()-1;++i) {
        auto& lp = points[i-1]; // left point
        auto& tp = points[i];   // this point
        Vec2d lrv = tp - lp;
        auto& rp = points[i+1]; // right point
        // calculate distance of the point to the line:
        double dist_mm = unscale<double>(scaleFactor) * std::abs(cross2(rp, lp) - cross2(rp - lp, tp)) / lrv.norm();
        if (dist_mm > tolerance) {                               // if the difference from straight line is more than this
            double x = 0.5f * (points[i-1](0) + points[i](0));
            points.emplace_back(Vec2d(x, f(x, z_sin, z_cos, vertical, flip)));
            x = 0.5f * (points[i+1](0) + points[i](0));
            points.emplace_back(Vec2d(x, f(x, z_sin, z_cos, vertical, flip)));
            // we added the points to the end, but need them all in order
            std::sort(points.begin(), points.end(), [](const Vec2d &lhs, const Vec2d &rhs){ return lhs < rhs; });
            // decrement i so we also check the first newly added point
            --i;
        }
    }
    return points;
}

 bool isInRange(Vec2d position, double minRange, double maxRange, Enemy* enemy){
     if(maxRange<0)
         return true;
     if(maxRange<minRange)
         return false;
     double d = position.distance(enemy->getPosition());
     return d<=maxRange && d>=minRange;
 }

		void Spatial::update()
		{
			double timeDelta = mTimeDelta.getElapsedTime().asSeconds();
			mSteeringForce.truncate(mMaxForce);

			Vec2d acceleration = mSteeringForce / mMass;
			mVelocity += acceleration * timeDelta;
			mVelocity.truncate(mMaxSpeed);
			mPos += mVelocity * timeDelta;

			if (mVelocity.lengthSq() > 0.00000001)
			{
				Vec2d velocityNorm = mVelocity;
				velocityNorm.normalize();
				setHeading(velocityNorm);
			}

			mTimeDelta.restart();
		}