close all; clear all;
N = 22;
theta = linspace(-pi*1.3,pi*1.3,N+1);
b = sinc(theta); % function to rotate
% b = 10-b; % flipped sinc function

%%% Gaussian
% sd = 2;
% mu = 0;
% x = linspace(-5*sd,5*sd,N);
% b = 1/(2*pi*sd)*exp(-(x-mu).^2/(2*sd^2));


% b = sin(theta);
% b = cos(theta);
% b = -3*(theta);


[rx, ry] = meshgrid( -N/2:N/2 , -N/2:N/2 );
r = hypot( rx , ry );
halfb = b((length(b)-1)/2 + 1 : end );
vq = interp1( 0:(length(halfb)-1) , halfb , r(:) , 'linear' , 0 );
W=r; W(:)=vq;

W = W-min(W(:));
W = W./max(W(:));

ind1 = N/2-floor(N/2*cosd(45))+1;
ind2 = N+1-ind1+1;
C = W(ind1:ind2,ind1:ind2);
C = C-min(C(:));
C = C./max(C(:));

imagesc(C); axis image;

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

minHeight = 25;
maxHeight = 80; %sideLength*1.3;
A = C*(maxHeight-minHeight)+minHeight;

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

totalLength = sum(sum(A,[]));
lengthDowel = 914.4;

minDowels = round(totalLength/lengthDowel);


% 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)),'}'];