close all; clear all;
N = 25;
filename = '/Users/jon/Documents/PROJECTS/miniDowels/images/mr-dem-height-map-output-03 copy.jpeg';
%filename = '/Users/jon/Documents/PROJECTS/miniDowels/images/uransus.jpeg';
filename = '/Users/jon/Documents/PROJECTS/miniDowels/images/gradient1.jpeg';
% filename = '/Users/jon/Documents/PROJECTS/miniDowels/images/gradientMount.png';
% filename = '/Users/jon/Documents/PROJECTS/miniDowels/images/gradient2.jpeg';
% filename = '/Users/jon/Documents/PROJECTS/miniDowels/images/gradient3.jpeg';


C = imread(filename);
C = imresize(C,[N,N]);
if (size(C,3)>1)
    C = rgb2gray(C);
end
C = double(C);
C = C-min(C(:));
C = C./max(C(:));

imagesc(C); axis image;

R = length(C); % number of dowels along side
T = R^2;
dowelSize = 3.175; % mm
sideLength = dowelSize*R;

minHeight = 30;% 17.5;
maxHeight = sideLength*1.2;
A = C*(maxHeight-minHeight)+minHeight;

[X,Y] = meshgrid(linspace(-sideLength/2,sideLength/2,R),linspace(-sideLength/2,sideLength/2,R));

% figure;
% surf(X,Y,A); axis image;

%% Create structure;
figure; hold on; axis image;
view(3);
yt = 0;

for n = 1:R
        
        xt = 0;
        yt2 = yt+dowelSize;
    for m=1:R
        xt2 = xt+dowelSize;
        
        h = A(n,m);
        
        coord = [...
            xt    yt    0;
            xt2  yt    0;
            xt2  yt2  0;
            xt    yt2  0;
            xt    yt    h;
            xt2  yt    h;
            xt2  yt2  h;
            xt    yt2  h;];
        idx = [4 8 5 1 4; 1 5 6 2 1; 2 6 7 3 2; 3 7 8 4 3; 5 8 7 6 5; 1 4 3 2 1]';
        
        xc = coord(:,1);
        yc = coord(:,2);
        zc = coord(:,3);
        
        
        patch(xc(idx), yc(idx), zc(idx), 'w', 'facealpha', 0.9);
        
        xt = xt2;
        
    end
    yt = yt2;
    
    
end

title({[num2str(R),' x ' num2str(R),'  -  ',num2str(T),' dowels'],[num2str(sideLength,3),' x ',num2str(sideLength,3),' x ',num2str(sideLength,3),' mm']});

%% Create text arrays for Arduino
stepsPerMM = 99.8732;
stepperIntercept = -15378.5476;
clear b dowelLabels hStack;
dowelLabels = '{';
p=1;
for n = 1:R
        
    for m=1:R
        
        hStack(p) = round(A(m,n));
        b(p) = round(A(m,n)*stepsPerMM+stepperIntercept);
        if p<(R*R)
            dowelLabels = [dowelLabels,'''A',num2str(m),' B',num2str(n),''',  '];
        else
            dowelLabels = [dowelLabels,'''A',num2str(m),' B',num2str(n),'''}'];
        end
        p=p+1;
    end
    
end

stepperTxt=['{',num2str(b(1)),', '];
hTxt = ['{',num2str(hStack(1)),', ']; 
for p=2:length(b)-1
   stepperTxt = [stepperTxt,num2str(b(p)),', ']; 
   hTxt = [hTxt,num2str(hStack(p)),', ']; 

end
stepperTxt = [stepperTxt,num2str(b(end)),'}']; 
hTxt = [hTxt,num2str(hStack(end)),'}'];