Finish 2010-11-17 12:00:00 UTC

allways_wp4a

by Seth Kosowsky

Status: Passed
Results: 24642 (cyc: 53, node: 4341)
CPU Time: 46.231
Score: 4976.41
Submitted at: 2010-11-12 07:56:46 UTC
Scored at: 2010-11-12 07:57:58 UTC

Current Rank: 2166th (Highest: 58th )

Comments
Please login or create a profile.
Code
function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
    % current opposition
    
    % Copyright 2010 The MathWorks, Inc.
    
    thrustRow = [];
    thrustCol = [];
    flagVisualize = 0;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %  try several routes, take the best
    
    %at a,b pts
    rtei = 0;
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 4);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 5);

    %buffer region
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 6);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 7);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 8);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 9);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 10);

    %max thr
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 101);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 102);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 103);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 104);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 105);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 106);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 107);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 108);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 109);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 110);

%     %drift
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 201);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 202);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 203);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 204);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 205);

    %miss size 2
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+1);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+2);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+3);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+4);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+5);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+6);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+7);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+8);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+9);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+10);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+101);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+102);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+103);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+104);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+105);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+106);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+107);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+108);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+109);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+110);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+201);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+202);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+203);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+204);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+205);
    %miss size 3
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+1);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+2);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+3);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+4);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+5);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+6);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+7);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+8);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+9);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+10);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+101);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+102);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+103);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+104);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+105);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+106);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+107);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+108);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+109);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+110);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+201);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+202);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+203);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+204);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+205);
    %miss size 4
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+1);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+2);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+3);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+4);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+5);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+6);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+7);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+8);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+9);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+10);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+101);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+102);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+103);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+104);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+105);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+106);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+107);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+108);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+109);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+110);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+201);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+202);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+203);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+204);
    rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+205);
    
    
    
    
    
    
    
    [tmp, ii] = min(sscore);
    fi = ii(1);
    
    thrustRow = thRow{fi};
    thrustCol = thCol{fi};
    
end

function [rscore, thrustRow, thrustCol] = routesolve(chart, aIndex, bIndex, maxThrottle, rflag)
    % current opposition
    
    rscore = Inf;
    
    thrustRow = [];
    thrustCol = [];
    flagVisualize = 0;
    
    
    pathr = [];
    pathc = [];
    
    rwind = chart(:,:,1);
    cwind = chart(:,:,2);
    [maxr, maxc, tmp] = size(chart);
    
    %starting r,c
    [ra, ca] = ind2sub(size(chart(:,:,1)),aIndex);
    [rb, cb] = ind2sub(size(chart(:,:,1)),bIndex);
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    maxerr = 7;
    maxN = 999;
    buff = floor(maxThrottle / 2);
    
    if maxThrottle >= 9,
        speedgoal = floor(maxThrottle / 3);
    elseif maxThrottle >= 6,
        speedgoal = 4;
    elseif maxThrottle >= 4,
        speedgoal = 2;
    else
        speedgoal = maxThrottle;
    end
    rs = 0;
    cs = 0;
    driftfl = 0;
    
    %current pos
    k = aIndex;
    r = ra;
    c = ca;
    pathr = [r];
    pathc = [c];
    
    if (rflag > 1000) & (rflag < 2000),
        rflag = rflag - 1000;
        maxerr = 5;
    end
    if (rflag > 2000) & (rflag < 3000),
        rflag = rflag - 2000;
        maxerr = 4;
    end
    if (rflag > 3000) & (rflag < 4000),
        rflag = rflag - 3000;
        maxerr = 2;
    end
    if (rflag > 100) & (rflag < 200),
        %         max throt
        rflag = rflag - 100;
        speedgoal = maxThrottle;
    end
    if (rflag > 200) & (rflag < 300),
        rflag = rflag - 200;
        driftfl = 1;
    end
    switch rflag,
        case 1,
            % directed 
            wayr = [rb; ra];
            wayc = [cb; ca];
        case 2,
            %keep row corner
            wayr = [ra; rb; ra; ra];
            wayc = [cb; cb; cb; ca];
        case 3,
            %keep col corner
            wayr = [rb; rb; rb; ra];
            wayc = [ca; cb; ca; ca];
        case 4
            %circulate
            wayr = [ra; rb; rb; ra];
            wayc = [cb; cb; ca; ca];
        case 5
            %countercirculate
            wayr = [rb; rb; ra; ra];
            wayc = [ca; cb; cb; ca];
        case 6,
            % directed 
            wayr = [rb-buff; ra];
            wayc = [cb-buff; ca];
        case 7,
            %keep row corner
            if ra > maxr/2,
                rm = ra - buff;
            else
                rm = ra + buff;
            end
            if cb > maxc/2,
                cm = cb - buff;
            else
                cm = cb + buff;
            end
            wayr = [rm ; rb; rm; ra];
            wayc = [cm; cb; cm; ca];
        case 8,
            %keep col corner
            if rb > maxr/2,
                rm = rb - buff;
            else
                rm = rb + buff;
            end
            if ca > maxc/2,
                cm = ca - buff;
            else
                cm = ca  + buff;
            end
            wayr = [rm; rb; rm; ra];
            wayc = [cm; cb; cm; ca];
        case 9
            %circulate
            if ra > maxr/2,
                rm1 = ra - buff;
            else
                rm1 = ra + buff;
            end
            if cb > maxc/2,
                cm1 = cb - buff;
            else
                cm1 = cb + buff;
            end
            if rb > maxr/2,
                rm2 = rb - buff;
            else
                rm2 = rb + buff;
            end
            if ca > maxc/2,
                cm2 = ca - buff;
            else
                cm2 = ca  + buff;
            end
            wayr = [rm1; rb; rm2; ra];
            wayc = [cm1; cb; cm2; ca];
        case 10
            %countercirculate
            if ra > maxr/2,
                rm1 = ra - buff;
            else
                rm1 = ra + buff;
            end
            if cb > maxc/2,
                cm1 = cb - buff;
            else
                cm1 = cb + buff;
            end
            if rb > maxr/2,
                rm2 = rb - buff;
            else
                rm2 = rb + buff;
            end
            if ca > maxc/2,
                cm2 = ca - buff;
            else
                cm2 = ca  + buff;
            end
            wayr = [rm2; rb; rm1; ra];
            wayc = [cm2; cb; cm1; ca];
    end
    
    %proceed through each waypoint
    
    wi = 0;
    wdone = 0;
    n = 0;
    while wdone == 0,
        wi = wi + 1;
        goalr = wayr(wi);
        goalc = wayc(wi);
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Head to next pt
        done = 0;
        while (done == 0 & n < maxN);
            n = n + 1;
            %delta to b point
            totd = (abs(goalr - r) + abs(goalc - c));
            dr = (goalr-r) ;
            dc = (goalc-c) ;
            
            speedgoalr = fix(speedgoal * dr / totd);
            speedgoalc = fix(speedgoal * dc / totd);
            %new speeds from winds
            rs1 = rs + rwind(r,c);
            cs1 = cs + cwind(r,c);
            
            %correct to nominal or not
            if driftfl == 1,
                tmpr = r + rs1;
                tmpc = c + cs1;
                nomhead = atan2((goalr - r),(goalc - c + eps));
                newhead = atan2((tmpr - r) , (tmpc - c + eps));
                if (abs(nomhead - newhead) < 30 * pi/180) & (tmpr >= 1  & tmpc >= 1 & tmpr <= maxr & tmpc <= maxc),
                    thr0 = 0;
                    thc0 = 0;
                else            
                    thr0 = speedgoalr - rs1;
                    thc0 = speedgoalc - cs1;
                end
            else
                thr0 = speedgoalr - rs1;
                thc0 = speedgoalc - cs1;
            end
            if abs(thr0) + abs(thc0) > maxThrottle,
                thr = fix(thr0 * maxThrottle / ( abs(thr0) + abs(thc0)));
                thc = fix(thc0 * maxThrottle / ( abs(thr0) + abs(thc0)));
%                 %check if limited thrust puts us at risk 
%                 tmpr = r + rs1 + thr;
%                 tmpc = c + rs1 + thc;
%                 if (tmpr < 1 | tmpc < 1 | tmpr > maxr | tmpc > maxc),
%                     %at risk
%                     if (tmpr < 1 | tmpr > maxr),
%                         thr = thr0;
%                         thc = sign(thc0) * min([abs(thc0) maxThrottle - abs(thr0)]);
%                     else
%                         thc = thc0;
%                         thr = sign(thr0) * min([abs(thr0) maxThrottle - abs(thc0)]);
%                     end
%                 end
            else
                thr = thr0;
                thc = thc0;
            end
            
            
            thrustRow = [thrustRow; thr];
            thrustCol = [thrustCol; thc];
            
            %new speeds
            rs = rs1 + thr;
            cs = cs1 + thc;
            %new point
            r = r + rs;
            c = c + cs;
            pathr = [pathr r];
            pathc = [pathc c];
            
            if (r < 1 | c < 1 | r > maxr | c > maxc),
                % call it here
                thrustRow = thrustRow(1:end-1);
                thrustCol = thrustCol(1:end-1);
                done = 1;
                wdone = 1;
            end
            
            if (abs(r-goalr) + abs(c-goalc)) <= maxerr,
                done = 1;
            end
            if n > 999,
                done = 1;
            end
        end
        if (wi == length(wayr)),
            wdone = 1;
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % score the result
    
    [dA,dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex, flagVisualize);
    rscore = scoresolution(dA,dB,thrustRow,thrustCol);
    
    
end



function [thrustRow, thrustCol] = validatesolution(thrustRow, thrustCol, maxThrottle)
    % VALIDATESOLUTION Validates the solution vectors given by the solver
    
    % Ensure column vectors and reals
    thrustRow = real(thrustRow(:));
    thrustCol = real(thrustCol(:));
    
    % Check maximum number of moves
    mnmoves = 1000;
    if numel(thrustRow) > mnmoves
        thrustRow = thrustRow(1:mnmoves);
    end
    if numel(thrustCol) > mnmoves
        thrustCol = thrustCol(1:mnmoves);
    end
    
    % Ensure integers
    if any(rem(thrustRow,1))
        thrustRow = round(thrustRow);
    end
    if any(rem(thrustCol,1))
        thrustCol = round(thrustCol);
    end
    
    % Ensure same length
    ntR = numel(thrustRow);
    ntC = numel(thrustCol);
    if ntR~=ntC
        n = min(ntR,ntC);
        thrustRow = thrustRow(1:n);
        thrustCol = thrustCol(1:n);
    end
    
    % Check maximum throttle
    h = (abs(thrustRow) + abs(thrustCol)) > maxThrottle;
    if any(h)
        % Motor is blown for every turn the throttle exceeds the motor capacity!
        thrustRow(h) = 0;
        thrustCol(h) = 0;
    end
    
end

function [dA, dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex, flagVisualize)
    % RUNSOLUTION Simulates the navigation trajectory given the winds and the
    % motor thrust.
    
    rowWind = chart(:,:,1);
    colWind = chart(:,:,2);
    [nR,nC] = size(rowWind);
    [AR,AC] = ind2sub([nR,nC],aIndex);
    [BR,BC] = ind2sub([nR,nC],bIndex);
    
    % Initialize variables at start point (A)
    fR = AR; fC =AC;
    fvR = 0; fvC = 0;
    dB = (fR-BR)^2 + (fC-BC)^2;
    
    for i = 1:numel(thrustRow)
        ivR = fvR + thrustRow(i) + rowWind(fR,fC);
        ivC = fvC + thrustCol(i) + colWind(fR,fC);
        iR = fR + ivR;
        iC = fC + ivC;
        if iR>nR || iR<1 || iC>nC || iC<1
            break % out of bounds
        end
        fR = iR;
        fC = iC;
        fvR = ivR;
        fvC = ivC;
        % Verify if this is the closest point to B
        if ((fR-BR)^2 + (fC-BC)^2) < dB
            dB = (fR-BR)^2 + (fC-BC)^2;
        end
    end
    dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
end

function score = scoresolution(dA,dB,thrustRow,thrustCol)
    % SCORESOLUTION Calculates the score for the solution.
    score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end