motorboat reloaded 2

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Basic solver, does nothing

% Copyright 2010 The MathWorks, Inc.



[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)),bIndex);


maxThrottle
chart(:,:,1)
chartsize = size(chart(:,:,1))
goal = [bRow, bCol];
curPos= [aRow, aCol]

curVel = [0  0]
thrustRow = [];
thrustCol =[];

for (i = 1:165)
dVelW = [chart(curPos(1),curPos(2),1), chart(curPos(1),curPos(2),2)]

% actThrust = [ 1 1];
veldrift = dVelW + curVel
actThrust = thrustcalc( chart, veldrift, curPos, goal, maxThrottle )
% thrustRow = [thrustRow ,actThrust(1) ];
% thrustCol = [thrustCol ,actThrust(2) ];

newVel = dVelW + curVel +actThrust;
newPos = curPos+ newVel;

% avoid loop
if ( newVel(1) ==0 && newVel(2) == 0)
  disp('-------!!!  will stay in a loop! ')  
  actThrust(1) = sign(actThrust(1)) * (abs( actThrust(1))+1)
  newVel = dVelW + curVel +actThrust;
  newPos = curPos+ newVel;
end
thrustRow = [thrustRow ,actThrust(1) ];
thrustCol = [thrustCol ,actThrust(2) ];

curVel = newVel
curPos = newPos
[distv,dx,dy] = distf(curPos, goal)
distance= distv
if (distance < 1) 
    goal = [aRow, aCol] 
    [distance,dx,dy] = distf(curPos, goal)
end
if  (goal(1) == aRow && goal(2) == aCol && distance== 0 )
  fprintf ('done, finish!')
  break 
end    
outofmap(newPos, chartsize)
if outofmap(newPos, chartsize)
  disp('boat will leave the map! ')
  fprintf( 'nxet bad postion: %d ,%d', newPos)
  
  thrustRow = thrustRow(1:end-1)
  thrustCol = thrustCol(1:end-1) 
  break
end 
goal
fprintf ('-------end cycle--------')
fprintf ('i: %d, pos: %d,%d', i , curPos )
end % end for cycle 

thrustRow
thrustCol
maxThrottle

%acttrust= [1, 1]
%thrustRow = [0 0 0 0 ];
%thrustCol = [0 0 0 0 ];

%thrustRow = [thrustRow ,acttrust(1) ]
%thrustCol = [thrustCol ,acttrust(2) ]
end

function [distance, dx, dy] = distf( pos1, pos2 )
 distance = ( abs(pos2(1)- pos1(1)) + abs(pos2(2)- pos1(2)) );
 dx= pos2(1)- pos1(1);
 dy= pos2(2)- pos1(2);
end

function thrust = thrustcalc ( chart, veldrift, curPos, goal, maxThrottle )
 % here comes the algorithm
[distv,dx,dy] = distf(curPos, goal)
mt = min(abs(dx),abs(dy));
if (distv==0)
    thrust= 0
    return
elseif (distv >0 && mt==0) 
    dir = [ sign(dx) sign(dy) ]  
else
    dir = [ round(dx/mt), round(dy/mt) ]
end

if (distv > 20)
    dir = dir*3
elseif (distv > 18)
    dir = dir*2
end    

thrguess = dir-veldrift
if ( velnorm(thrguess) < maxThrottle  )
    thrust = thrguess
else 
    thrust = round (thrguess./2)
    if ( velnorm(thrust) > maxThrottle  )
      thrustn = renorm(thrust, maxThrottle) 
      thrust = thrustn
    end  
 % avoid fatal situations
   nextpos =  curPos+ veldrift + thrust
   chsize = size(chart(:,:,1))
   thrustorig = thrust
   if ( outofmap(nextpos, chsize) )       
     if ( nextpos(1) >  chsize(1)  )
         maxmove = chsize(1) - curPos(1)
         thrust(1) = -veldrift(1)+maxmove
         thrust(2) = sign(thrustorig(2))* min( abs(maxThrottle-thrust(1)), abs(thrustorig(2)) )
     elseif  ( nextpos(2) >  chsize(2)  )
         maxmove = chsize(2) - curPos(2)
         thrust(2) = -veldrift(2)+maxmove
         thrust(1) = sign(thrustorig(1))* min( abs(maxThrottle-thrust(2)), abs(thrustorig(1)) )  
      elseif  ( nextpos(1) <  1  )
         maxmove =  1 - curPos(1)
         thrust(1) = -veldrift(1)+maxmove
         thrust(2) = sign(thrustorig(2))* min( abs(maxThrottle-thrust(1)), abs(thrustorig(2)) )    
      elseif  ( nextpos(2) <  1  )
         maxmove =  1 - curPos(2)
         thrust(2) = -veldrift(2)+maxmove
         thrust(1) = sign(thrustorig(1))* min( abs(maxThrottle-thrust(2)), abs(thrustorig(1)) )  
     end    
   end    
        
end    

% avoid fatal situations

   
end

function vabs = velnorm (vel)
  vabs = abs(vel(1))+abs(vel(2))
end

function sw = outofmap (pos, chsize)
  if ( pos(1)<1 || pos(1)> chsize(1) || pos(2)<1 || pos(2)> chsize(2) )
     sw= 1
  else
      sw= 0
  end   
end

function v =renorm(thrust, maxThrottle)
  v= thrust    
  while ( velnorm(v) > maxThrottle )
       v= round (v * 0.8)
  end    
end