darkness21

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Created by Alan Chalker 11/11/10

% setup vars
[arow, acol] = ind2sub(size(chart(:,:,1)),aIndex); % start coords
[brow, bcol] = ind2sub(size(chart(:,:,1)),bIndex); % target coords
[totrow,totcol,foo]=size(chart);
%init vels
rowv = 0;
colv = 0;
%init thrust vars
thrustRow = [];
thrustCol = [];
turn=0;
maxthr=floor(maxThrottle/2);
%current coords
currow = arow;
curcol = acol;
tarrow = brow;
tarcol = bcol;


while 1
    % get current wind vels
    wrow = chart(currow, curcol, 1);
    wcol = chart(currow, curcol, 2);
    rowv=rowv+wrow;
    colv=colv+wcol;
    
    %update where wind and net vel takes us
    currow = currow + rowv;
    curcol = curcol + colv;
    
    % determine direction / distance to target
    rowdiff = tarrow - currow;
    coldiff = tarcol - curcol;
    
    %vel directions
    if rowdiff
        rowdir = rowdiff/abs(rowdiff);
    else
        rowdir=0;
    end
    if coldiff
        coldir = coldiff/abs(coldiff);
    else
        coldir=0;
    end
    
    %vel magnitudes
    rowdiff = abs(rowdiff);
    coldiff = abs(coldiff);
    
    availthr = maxthr;
    
    mrow=0;
    mcol=0;
  
    if currow <1 || currow>totrow || curcol <1 || curcol>totcol % if off the board
        
        if currow < 1
            mrow=abs(currow)+1;
        elseif currow > totrow
            mrow=totrow-currow;
        end
        if curcol < 1
            mcol=abs(curcol)+1; 
        elseif curcol > totcol
            mcol=totcol-curcol;
        end
        availthr=maxThrottle-abs(mcol)-abs(mrow);
    end
        
    if  (abs(rowv) > maxthr) && (mrow==0) %if moving too fast rowwise
        rowvdir = rowv/abs(rowv);
        if abs(rowv) <= (availthr +maxthr)
            mrow=-1*rowvdir*(abs(rowv)-availthr);
            availthr=availthr-abs(mrow);
        else
            mrow=-1*rowvdir*availthr;
            availthr=0;
        end
    end
    if (abs(colv) > maxthr) && (mcol==0) %if moving too fast colwise
        colvdir = colv/abs(colv);
        if abs(colv) <= (availthr +maxthr)
            mcol=-1*colvdir*(abs(colv)-availthr);
            availthr=availthr-abs(mcol);
        else
            mcol=-1*colvdir*availthr;
            availthr=0;
        end
        end
        
    if (availthr >0) && (mcol==0) && (mrow==0) && (rowdiff >= availthr) && (coldiff >= availthr) %if long dist to target

        mcol= coldir*(availthr-abs(mrow));
    end
    
    if mrow==0
        if (rowdiff < availthr) % if near same row as target
        mrow= rowdir*rowdiff;
        availthr = availthr-rowdiff;
        else
                  mrow= rowdir*floor(availthr/2);
                  availthr = availthr-abs(mrow);
        end
    end
    
    if mcol==0
        if (coldiff < availthr) % near same col as target
        mcol= coldir*coldiff;
        else
            mcol=coldir*availthr;
        end
    end
    
    %output motor vector
    thrustRow = [thrustRow;mrow];
    thrustCol = [thrustCol;mcol];
    turn=turn+1;
    
    %update where motor takes us
    currow = currow + mrow;
    curcol = curcol + mcol;
    
    
    %update total vels
    rowv=rowv+mrow;
    colv=colv+mcol;
    
    % stop if total turns too much
    if turn > 50
                if (tarrow == brow) && (tarcol == bcol)
            tarrow = arow;
            tarcol = acol;
            turn=0;
        else
            break
        end
    end
    
    % check if just looping in place
    if (turn > 5) && isequal([mrow; mrow; mrow; mrow],thrustRow(end-4:end-1)) && isequal([mcol; mcol; mcol; mcol],thrustCol(end-4:end-1))
             if (tarrow == brow) && (tarcol == bcol)
            tarrow = arow;
            tarcol = acol;
        else
            break
        end
    end   
    
    %if at target
    if ((currow==tarrow) && (curcol==tarcol))
        if (tarrow == brow) && (tarcol == bcol)
            tarrow = arow;
            tarcol = acol;
        else
            break
        end
    end
    
    %check if off the board and back out of step
    if currow <1 || curcol <1 || currow>totrow || curcol>totcol
        thrustRow=thrustRow(1:end-1);
        thrustCol=thrustCol(1:end-1);
        currow = currow - mrow;
        curcol = curcol - mcol;
        rowv=rowv-mrow;
        colv=colv-mcol;
        curcol = curcol - colv;
        currow = currow - rowv;
        rowv=rowv-wrow;
        colv=colv-wcol;
        if (tarrow == brow) && (tarcol == bcol)
            tarrow = arow;
            tarcol = acol;
        else
            break
        end
    end
end

end