Tthresh = 30;

% Setup the background
% 1. Make Table
% 2. Make Hands
% 3. Plot Table and Hands
figure(1)
close
clf

%make a table
table=UnitCube;
table.facecolor=[222/255, 191/255, 150/255]; % A light brown wood-ish color
table.facelighting='flat';
table.edgecolor=[175/255, 124/255, 54/255]; % A darker color for outline
table=scale(table,2,2,.2);
table=translate(table,0,0,-1.2);

%make hand (namely little sticks)
cylHand = UnitCylinder(10);
L1 = 1;
L2 = 1;
radius = 0.03;
arm1 = translate(scale(cylHand,radius,radius,L1/2),-.57,0,L1/2);
arm1 = rotateX(arm1, 90);
arm1 = translate(arm1, -.25, 0, -.4);
arm1.facecolor = 'blue';
arm2 = translate(scale(cylHand,radius,radius,L2/2),.55,0,L2/2);
arm2 = rotateX(arm2, 90);
arm2 = translate(arm2, .25, 0, -.4);
arm2.facecolor = 'green';
hand = combine(arm1, arm2);

%plot table and hands
background = combine(table, hand);
renderpatch(background);
axis off;
axis([-2, 2, -2, 2, -4, 4]);
grid on
daspect([1 1 1])
light('position',[10,-10,10])
%Do a persptective transform
set(gca,'projection','perspective')
set(gca,'CameraViewAngle',6)
%The frame background color
set(gcf,'color', [183/255, 248/255, 1])
xlabel('x');ylabel('y');zlabel('z');
view(7,20)
drawnow;

disp('Initializing serial...')
initialized=0;
mcu=serial('COM8',...
    'Baudrate',9600,...
    'Stopbits',1,...
    'Parity','none',...
    'FlowControl','none');
fopen(mcu)
try
    while (true)
        disp('Getting serial input...')
        tline = fgetl(mcu);
        result = sscanf(tline, '%d');

        if numel(result)==9
            objectSelected = result(1);
            dIn = result(2);
            dOut = result(3);
            isSolid = result(4);
            temperature = result(5);
            isFull = result(6);
            objectGripped = result(7);
            objectLifted = result(8);
            height = result(9);
            
            % scale dimensions to fit on the Matlab Screen
            dIn = dIn/20;
            dOut = dOut/20;
            
            % Foreground with the object and stuff
            % 1. Get input from the MCU
            % 2. Draw object if necessary
            % 3. Move object if necessary
            % 4. Update
            % Parameters inputted: inner diameter, outer diameter, temperature, object
            % selected?  object gripped?  displacement from table, hasHandle, isSolid,
            % isFull, object lifted?
            if (objectSelected)
                res=20;

                % make a closed end cylinder
                cyl1=CSGcylinder(0,0,0,dOut,'z',res);
                cube1=CSGcube(0,0,-dOut+0.05,dOut+0.05,res);
                body=CSGintersection(cyl1,cube1);
                if (~isSolid)
                    % subtract by a smaller cylinder
                    cyl2=CSGcylinder(0,0,0,dIn,'z',res);
                    cube2=CSGcube(0,0,-dIn+0.05,dIn+0.05,res);
                    hole=CSGintersection(cyl2,cube2);
                    body=CSGsubtract(body,hole);
                end
                object = body;

                objectSurface = CSGtoSurface(object, res);
                if(temperature >= Tthresh)
                    Tgradient = [1, 1-(temperature+30)/100, 0]; % color of the object scaled to temperature
                else
                    Tgradient = [0, (temperature+20)/100, 1];
                end
                objectSurface.facecolor = Tgradient;
                
                if(isFull)
                    cylFull = UnitCylinder(2);
                    lengthFull = dOut*0.4;
                    radiusFull = dIn - 0.02;
                    inContent = translate(scale(cylFull,radiusFull,radiusFull,lengthFull-.02),.01,0,-lengthFull/1.8);
                    inContent.facecolor = [6/255, 249/255, 0];
                    objectSurface = combine(objectSurface, inContent);
                end

                if(objectGripped)
                    arm1 = translate(arm1, (-dOut - max(arm1.vertices(:,1))), 0, 0);
                    arm2 = translate(arm2, (dOut - min(arm2.vertices(:,1))),0, 0);
                    objectSurface = combine(objectSurface, arm1, arm2);

                    if(objectLifted)
                        movingObject = translate(objectSurface, 0, 0, height);
                        scene = combine(table, movingObject);
                    else
                        scene = combine(table, objectSurface);
                    end
                else
                    scene = combine(table, objectSurface);
                end

                figure(1);
                clf
                renderpatch(scene);
                axis off;
                axis([-2, 2, -2, 2, -4, 4]);
                grid on
                daspect([1 1 1])

                light('position',[10,-10,10])

                %Do a persptective transform
                set(gca,'projection','perspective')
                set(gca,'CameraViewAngle',6)

                %The frame background color
                set(gcf,'color', [183/255, 248/255, 1])
                xlabel('x');ylabel('y');zlabel('z');
                view(7,20)
                drawnow;
                
            else
                figure(1)
                clf
                %plot table and hands
                background = combine(table, hand);
                renderpatch(background);
                axis off;
                axis([-2, 2, -2, 2, -4, 4]);
                grid on
                daspect([1 1 1])
                light('position',[10,-10,10])
                %Do a persptective transform
                set(gca,'projection','perspective')
                set(gca,'CameraViewAngle',6)
                %The frame background color
                set(gcf,'color', [183/255, 248/255, 1])
                xlabel('x');ylabel('y');zlabel('z');
                view(7,20)
                drawnow;
            end % if(objectSelected)
        end % if nuel(result) == 6;
    end % while(true)
    
catch
    fclose(mcu)
    disp('Serial closed')
    disp(lasterror.message)
end