My super move is not so super after all.

[Quartofel]

Code:

extern void object::Main()
{
    move(5);
    smove(5);
}

void object::smove(int dist)
{
    point b = chkpos(dist), pos = this.position;
    //while(distance2d(b, this.position)>1) //Y U NO WORK
       while(true)
    {
        point c = chkpos(20);
        if(topo(c)>this.position.z){motor(0.1,0.1); adjalt(7,8);}
        else{motor(1,1);adjalt(4,5);}
    }
}

void object::adjalt(int min, int max)
{
    int alt = this.altitude;
    if(alt<min){while(alt<min){jet(1);alt = this.altitude;}}
    if(alt>max){while(alt>max){jet(-0.65);alt = this.altitude;}}
    if(alt<1 && alt<min){while(alt>min){motor(0.2, 0.2); jet(1);alt = this.altitude;}}
    jet(0);
}

point object::chkpos(int c) //winny bledow przy przelotach w okolicy wysokich struktur
{
    int a, b, ori = this.orientation;
    point t = this.position, p;
    if(0 <= ori && ori <= 90){ori = 90 - ori; a = c*cos(ori); b = c*sin(ori); p.x=(t.x+a); p.y=(t.y+b);} //I
    if(90 <= ori && ori <= 180){ori = 180 - ori;a = c*cos(ori); b = c*sin(ori); p.x=(t.x-a); p.y=(t.y+b);} // II
    if(180 <= ori && ori <= 270){ori = 270 - ori;a = c*cos(ori); b = c*sin(ori); p.x=(t.x-a); p.y=(t.y-b);} //III
    if(270 <= ori && ori <= 360){ori = 360 - ori;a = c*cos(ori); b = c*sin(ori); p.x=(t.x+a); p.y=(t.y-b);} //IV
    p.z = topo(p);
    return p;
}

I've tried everything I could. It should slow down and ascend higher if encountered terrain higher than it's altitude. Written for wasp. Help, I don't know what's wrong ;-;

It won't go for set distance, and sometimes it climbs obstacles properly, sometimes don't. Don't know why.