%Make me a cubeang, strong and tall
vertices = [0 0 0; 1 0 0; 1 1 0; 0 1 0; 0 0 1; 1 0 1; 1 1 1; 0 1 1;];
faces = [1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8; 1 2 3 4; 5 6 7 8;];
str = [{' '}; {'Xang    Yang    Zang    Xacc Yacc  Zacc'}; {' '}; {' '}; {' '}; {' '}; {' '}; {' '}; {' '}; {' '}];

h = axes('Position',[0 0 1 1],'Visible','off');
b = axes('Position',[.4 0 .7 .8]);
patch('Vertices', vertices, 'Faces', faces,  'FaceVertexCData',hsv(6),'FaceColor','flat');
camzoom(0.75);
axis equal;
axis vis3d;
axis off;

drawnow;

%NOTE: If you need to close open serial ports first, run the following:
%fclose(instrfind);
%You might need that if this program crashes and leaves the serial port
%Open

%Open Serial Port
sp = serial('/dev/ttyS0', 'BaudRate', 115200);
%And open it up
fopen(sp);

%Roll
ro = 0;
dro = 0;
%Pan
pn = 0;
dpn = 0;
%Azimuth
az = 0;
daz = 0;

strinit = {'Initilizing....'};
f = text(.025,.6,strinit,'FontSize',12);

while 1 == 1

    %Get line.. format is [anglex angley anglez accelx accely accelz]
    dataline = fscanf(sp, '%d %d %d %d %d %d');
   
    dangle = [dataline(1) dataline(2) dataline(3)];
    accel = [dataline(4) dataline(5) dataline(6)];
    
    %+/-2g in 4096..... one g = 9.81 m/s/s
    accel = accel * (4 / 4096) * 9.81;
    
    %+/-75 deg/s in 2048
    dangle = dangle * (150 / 2048) / (8 * 70.3125);
    %dangle change is in degrees now, accel is in m/s/s
    
    %Two possible states: moving quickly, and moving slowly or not really
    %moving
    
    sumdangle = sum(abs(dangle));
    
    absaccel = sqrt(dot(accel, accel));
    
    %We aren't doing much so the state is 'stable'
    if (sumdangle < 2) && (absaccel > 8.5) && (absaccel < 10.5)
        stable = true;
        str(1) = {'System Stable'};
    else
        stable = false;
        str(1) = {'System moving'};
    end;
    
    str(10) = str(9);
    str(9) = str(8);
    str(8) = str(7);
    str(7) = str(6);
    str(6) = str(5);
    str(5) = str(4);
    str(4) = str(3);
    s = sprintf('%6.3f %6.3f %6.3f %5.1f %5.1f %5.1f', [dangle accel]);
    str(3) = cellstr(s);
    set(gcf,'CurrentAxes',h);
    delete(f);
    f = text(.025,.6,str,'FontSize',12);
    set(gcf,'CurrentAxes',b);
    
    %We need to change everything to camroll(ro), camorbit(pn, az)
    
    %If state is stable we figure out up/down from the acceleration vector
    if (stable == true)
        
        %Step #1: Project vector onto XY plane (Z is down)
        proxy = accel * [1 0 0; 0 1 0; 0 0 0];
        
        %Step #2: Get angle between xy plane and vector... this will be
        %one angle
        newro = acos((dot(proxy, accel) / (norm(proxy) * norm(accel) )));
        
        if (accel(3) > 0)
            newro = newro * -1;
        end;
        
        %Step #3: Get angle between the xy projection and x axis... this
        %one be other angle
        newaz = acos(dot(proxy, [1 0 0]) / (norm(proxy) * norm([1 0 0])));
        
        %Convert to degrees
        newro = newro * (180 / pi);
        newaz = -newaz * (180 / pi);
        
        dro = newro - ro + 0.0001;
        daz = newaz - az + 0.0001;
       
    else   
        %uuhhh... system is moving. Just go with gyro readings
        dro = dangle(1);
        daz = -dangle(2);      
    end;

    %ALWAYS get pan from gyro
    dpn = dangle(3);
    
    %Apply the rolls and orbits to the camera
    camroll(dro)
    camorbit(dpn, daz,'camera')
    drawnow
    
    ro = ro + dro;
    pn = pn + dpn;
    az = az + daz;
    
end;
    
fclose(sp);