function plotVP( Vopt, Popt, paramSet )
% plots v,p on a grid

% normalise range of v and plot on grid, setting values of obstructed cells
% to 0 (so they will show up as off-color blocks)
figure(1); clf;
colormap( gray )
v = Vopt;

v = reshape( v , paramSet.colCount, paramSet.rowCount )';
imagesc( v );
hold on;

% loop through policy and plot directions
thetaLUT = [ pi/2 -pi/2 pi 0 ];
for s = 1:( paramSet.rowCount * paramSet.colCount )
    rc = s2rc( s , paramSet.colCount );
    r = rc(1); c = rc(2);
    ang = thetaLUT( Popt( s ) );
    plot( c , r , 'co' );
    plot( [ c (c + .25*cos( ang )) ], ...
          [ r (r - .25*sin( ang )) ], ...
           'c' );
end
    
% plot start and goal
plot( paramSet.goalState(1) , paramSet.goalState(2) , 'rp', 'markersize',20 )
plot( paramSet.startState(1) , paramSet.startState(2) , 'rs', 'markersize',20 )

hold off;