function state = algo_init(obj, st, vRef)
  algo_info = load("algo_info.m");
  algo = algo_info.algo;
  d = algo_info.d;
  ang = algo_info.ang;
  save_state = algo_info.save_state;
  fDeggreCosine = @(a,b,c) acosd(((a^2)+(b^2)-(c^2)) ...
                                /(2*a*b));
  switch(algo);
  case 0;
    
    d(1,1) = norm((obj.K*st(1:obj.nodeStateLength))(1:3));
    d(2,1) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
    algo = 1;
    state = st;
    al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
    
  case 1;
    set_point = [2 0 0 0 0 0]';
    state = obj.Av2(st,[1 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;
    al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
    if norm(al_state - set_point) < .1
      d(1,2) = norm((obj.K*st(1:obj.nodeStateLength))(1:3)); ... from sensor
      d(2,2) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
      ang(1) = 180 - fDeggreCosine(al_state(1),d(1,2),d(1,1));
      ang(2) = 180 - fDeggreCosine(al_state(1),d(2,2),d(2,1));
      save_state = al_state;
      printf("<algo_init>Estimate stage 1 \n");
      cal_al_state = [d(1,2)*cosd(ang(1))+al_state(1);
                      d(1,2)*sind(ang(1))+al_state(2);
                      0 ;
                      d(2,2)*cosd(ang(2))+al_state(1);
                      d(2,2)*sind(ang(2))+al_state(2);
                      0]
      algo=2;
    else
      al_state = obj.param.model.A * al_state ... 
                + obj.param.model.B ...
                * arrayfun(obj.param.model.limit_motor, ... 
                          (obj.param.model.Nst * set_point ...
                              - obj.param.model.Kst*al_state ));    
      state(1:3) = al_state(1:3);
      state(obj.nodeStateLength+1:obj.nodeStateLength+3) = al_state(4:6);
    endif   
  case 2;
    set_point = [1 1 0 0 0 0]';
    state = obj.Av2(st,[1 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;
    al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
    if norm(al_state - set_point) < .1
      printf("<algo_init>Estimate stage 2 \n");
      cal_al_state = [al_state(1:3);
                      d(1,2)*cosd(ang(1))+save_state(1);
                      d(1,2)*sind(ang(1))+save_state(2);
                      0 ;
                      d(2,2)*cosd(ang(2))+save_state(1);
                      d(2,2)*sind(ang(2))+save_state(2);
                      0];
      d(1,3) = norm((obj.K*st(1:obj.nodeStateLength))(1:3)); ... from sensor
      d(2,3) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
      _d(1) = norm((obj.K*cal_al_state(1:obj.nodeStateLength))(1:3));
      _d(2) = norm((obj.K*cal_al_state(1:obj.nodeStateLength))(4:6));
      printf("<algo_init> validating...\n");
      for i = 1:length(ang)
        if sum( (abs(_d(i) - d(i,3))) < .1 )
          printf("<algo_init> %i pass\n",i);
        else
          printf("<algo_init> inverte on %i\n",i);
          ang(i) = 180 + fDeggreCosine(save_state(1),d(i,2),d(i,1));
        endif
      endfor
      cal_al_state = [al_state(1:3);
                      d(1,2)*cosd(ang(1))+save_state(1);
                      d(1,2)*sind(ang(1))+save_state(2);
                      0 ;
                      d(2,2)*cosd(ang(2))+save_state(1);
                      d(2,2)*sind(ang(2))+save_state(2);
                      0]
      if norm(cal_al_state - st(1:9)) < .1
        printf("<algo_init> done\n");
        algo=3;
      else
        printf("<algo_init> fail\n");
        algo=9;
      endif
    else
      al_state = obj.param.model.A * al_state ... 
                + obj.param.model.B ...
                * arrayfun(obj.param.model.limit_motor, ... 
                          (obj.param.model.Nst * set_point ...
                              - obj.param.model.Kst*al_state ));    
      state(1:3) = al_state(1:3);
      state(obj.nodeStateLength+1:obj.nodeStateLength+3) = al_state(4:6);
    endif     
  case 3;
    state = obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
  case 9;
    state = obj.Av2(st,[0 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;;
  endswitch;
  save "algo_info.m" ang save_state algo d;
endfunction