clc;
close all;
clear all;

comPort = 'COM5';

if (~exist('serialFlag','var'))
    [gyroConnection.s,serialFlag] = setupSerial(comPort);
end

if(~exist('figureHandle','var') || ~ishandle(figureHandle))
    figureHandle = figure(1);
end

if(~exist('stopButton','var'))
    stopButton = uicontrol('Style','togglebutton','String',...
        'Stop & Close Serial Port',...
        'pos',[0 0 200 25],'parent',figureHandle);
end

if(~exist('axisSwitchX','var'))
    axisSwitchX = uicontrol('Style','checkbox','String','X axis',...
        'pos',[300 0 50 25],'parent',figureHandle);
end

if(~exist('axisSwitchY','var'))
    axisSwitchY = uicontrol('Style','checkbox','String','Y axis',...
        'pos',[350 0 50 25],'parent',figureHandle);
end

if(~exist('axisSwitchZ','var'))
    axisSwitchZ = uicontrol('Style','checkbox','String','Z axis',...
        'pos',[400 0 50 25],'parent',figureHandle);
end



if(~exist('degreeLabelX','var'))
    degreeLabelX = uicontrol('Style','text','String','X: 0 degrees',...
        'pos',[450 120 100 25],'parent',figureHandle);
end

if(~exist('degreeLabelY','var'))
    degreeLabelY = uicontrol('Style','text','String','Y: 0 degrees',...
        'pos',[450 95 100 25],'parent',figureHandle);
end

if(~exist('degreeLabelZ','var'))
    degreeLabelZ = uicontrol('Style','text','String','Z: 0 degrees',...
        'pos',[450 70 100 25],'parent',figureHandle);
end


if(~exist('resetRadioButton','var'))
    resetRadioButton = uicontrol('Style','radiobutton','String','Reset',...
        'pos',[500 0 50 25],'parent',figureHandle);
end

%graphAxes = axes('Xlim', [-2 2], 'Ylim', [-2 2], 'Zlim', [-2 2]);
graphAxes = axes('Xlim', [-2 2], 'Ylim', [-2 2]);
view(2);

%axis off;
axis equal;
%axis normal;


[cylX, cylY, cylZ] = cylinder(0.5);
[conX, conY, conZ] = cylinder([1,0]);

hgTransformArray(1) = surface(cylX, cylY, cylZ);
hgTransformArray(2) = surface(conX, conY, conZ + 1);

hgTransform = hgtransform('Parent', graphAxes);

set(hgTransformArray, 'Parent', hgTransform);
drawnow();
pause(0.25);


[ gainx, gainy, gainz ] = calibrateGyro(gyroConnection);
%  gainx=-0.91;
%  gainy=-0.94;
%  gainz=-0.93;

displayAngleX = 0;
displayAngleY = 0;
displayAngleZ = 0;

angleX = 0;
angleY = 0;
angleZ = 0;

while (get(stopButton,'Value') == 0)
    
    [newAngleX, newAngleY, newAngleZ ] = ...
        getAngles(gyroConnection, gainx, gainy, gainz);
    
    if get(axisSwitchX, 'Value') == 1
        
        deltaAngleX = angleX - newAngleX;
        
        displayAngleX = displayAngleX + deltaAngleX;
        
        angleX = newAngleX;
        
        set(degreeLabelX,'String', ['X: ' num2str(round(displayAngleX)) 'degrees'])
    end
    
    if get(axisSwitchY, 'Value') == 1
        
        deltaAngleY = angleY - newAngleY;
        
        displayAngleY = displayAngleY + deltaAngleY;
        
        angleY = newAngleY;
        
        set(degreeLabelY,'String', ['Y: ' num2str(round(displayAngleY)) 'degrees'])
    end
    
    if get(axisSwitchZ, 'Value') == 1
        
        deltaAngleZ = angleZ - newAngleZ;
        
        displayAngleZ = displayAngleZ + deltaAngleZ;
        
        angleZ = newAngleZ;
        
        set(degreeLabelZ,'String', ['Z: ' num2str(round(displayAngleZ)) 'degrees'])
    end
    
    R = makehgtform('xrotate',(round(displayAngleX)*pi/180),...
        'yrotate',(round(displayAngleY)*pi/180),...
        'zrotate',(round(displayAngleZ)*pi/180));
    
    set(hgTransform,'Matrix',R)
    
    if (get(resetRadioButton,'Value')==1)
        
        displayAngleX=0;
        displayAngleY=0;
        displayAngleZ=0;
        
        set(resetRadioButton,'Value',0);
    end
    
    drawnow;
end
closeSerial();
    

