library MoveSys initializer IniMove

globals
    body array mvs_Sb [1000]
    integer mvs_si = -1
endglobals

struct vector
    real x
    real y 
    real z
    static method GetFrom2Locs takes real ax,real ay,real az,real bx,real by,real bz returns vector
        local vector v = vector.create()
            set v.x = bx-ax
            set v.y = by-ay
            set v.z = bz-az
            return v
    endmethod
    method Summ takes vector a returns nothing
        set this.x = this.x + a.x
        set this.y = this.y + a.y
        set this.z = this.z + a.z
    endmethod  
    method Multiply takes real k returns nothing
        set this.x = k*this.x
        set this.y = k*this.y 
        set this.z = k*this.z
    endmethod
    method Module takes nothing returns real
        return SquareRoot(this.x*this.x+this.y*this.y+this.z*this.z)
    endmethod
    method Norm takes nothing returns nothing
    local real m = SquareRoot(this.x*this.x+this.y*this.y+this.z*this.z)
        set this.x = this.x/m
        set this.y = this.y/m
        set this.z = this.z/m
    endmethod
endstruct

struct body
    unit u
    static method Create takes unit u returns body
        local body b = body.create()
            set b.u = u
            call UnitAddAbility(u,'Amrf')
            call UnitRemoveAbility(u,'Amrf')  
        return b
    endmethod
    method Destroy takes nothing returns nothing
        call this.destroy()
    endmethod
    method Start takes nothing returns boolean
      if mvs_si < 1000 then  
        set mvs_si = mvs_si + 1
        set mvs_Sb[mvs_si] = this
        return true
      else
        return false
      endif
    endmethod
endstruct

private function Engine takes nothing returns nothing
    local integer i = 0
    local body b
    loop
        exitwhen i > mvs_si
            if mvs_Sb[i] != 0 then
                set b = mvs_Sb[i]
                call SetUnitX(b.u, GetUnitX(b.u) + b.v.x*0.025)
                call SetUnitY(b.u, GetUnitY(b.u) + b.v.y*0.025)
                call SetUnitFlyHeight(b.u, GetUnitFlyHeight(b.u) + b.v.z*0.025,0)
            else
                set mvs_Sb[i] = mvs_Sb[mvs_si]
                set mvs_Sb[mvs_si] = 0
                set mvs_si = mvs_si - 1
        set i = i - 1
            endif
        set i = i + 1
    endloop
endfunction

function IniMove takes nothing returns nothing
        call TimerStart(CreateTimer(), 0.025, true, function Engine)
endfunction

endlibrary