%   Copyright 2010 The MathWorks, Inc.

function BluetoothRead(port)
% Byte structure of Bluetooth data packet for ECRobot NXT
% Byte 1, number of bytes in the message, message starts on 3rd byte
% Byte 2, 0x00 format byte, never changes
% Byte 3-34, actual data from/to NXT. Size of data packet is 32 bytes
NUM_OF_CYCLE = 1000;
PERIOD = 0.1;
PACKET_SIZE = 34;  % should not be changed
TIMES = PERIOD:PERIOD:NUM_OF_CYCLE*PERIOD;

try
    s = w32serial(port, 'BaudRate', 128000);
    fopen(s);
catch
    fclose(s);
    disp(['Failed to connect with: ' port]);
end

MotorADAQ = zeros(1, NUM_OF_CYCLE);
MotorBDAQ = zeros(1, NUM_OF_CYCLE);
REF_A_DAQ = zeros(1, NUM_OF_CYCLE);
REF_B_DAQ = zeros(1, NUM_OF_CYCLE);
UltrasonicSensorDAQ = zeros(1, NUM_OF_CYCLE);
disp('Ready for DAQ via Bluetooth. Run your NXT!');
figure;
for i = 1:NUM_OF_CYCLE
    packet = fread(s, PACKET_SIZE, '*uint8');

    if length(packet) == PACKET_SIZE
        msb = uint16(packet(4));
        lsb = uint16(packet(3));
        MotorADAQ(i) = typecast(msb*uint16(256)+lsb,'int16');
       
        msb = uint16(packet(6));
        lsb = uint16(packet(5));
        MotorBDAQ(i) = typecast(msb*uint16(256)+lsb,'int16');
        
        msb = uint16(packet(8));
        lsb = uint16(packet(7));
        REF_A_DAQ(i) = typecast(msb*uint16(256)+lsb,'int16');
        
        msb = uint16(packet(10));
        lsb = uint16(packet(9));
        REF_B_DAQ(i) = typecast(msb*uint16(256)+lsb,'int16');
        
        state = [deg2rad(MotorADAQ(i))+pi/2 deg2rad(MotorBDAQ(i))+pi/2];
        joint_positions = snakePose(state, 1);
    end
    
    clf
    p = plot(joint_positions(:,1), joint_positions(:,2), '-o');
    axis([-3 3 -3 3]);
    drawnow;
    
    if (mod(i,10) == 0)
        i 
    end
end

fclose(s);
disp('DAQ is finished.');
 
figure;
assignin('base', 'MotorADAQ', MotorADAQ);
assignin('base', 'MotorBDAQ', MotorBDAQ);
assignin('base', 'REF_A_DAQ', REF_A_DAQ);
assignin('base', 'REF_B_DAQ', REF_B_DAQ);
plot(TIMES, MotorADAQ(1:NUM_OF_CYCLE),'r');
hold on;
plot(TIMES, MotorBDAQ(1:NUM_OF_CYCLE),'b');
plot(TIMES, REF_A_DAQ(1:NUM_OF_CYCLE),'k--');
plot(TIMES, REF_B_DAQ(1:NUM_OF_CYCLE),'k--');
title('Motor Angles'); 
legend('A','B','A Ref', 'B Ref');

end

function r = deg2rad(x)
    r = x * pi / 180;
end

function pos = snakePose(state, l)
    x = 0;
    y = 0;
    ang = 0;
    
    N = numel(state);
    
    %N*2 matrix to store the position of each joint
    pos = zeros(N + 1, 2);
    pos(1,:) = [x y];
    
    for i=1:N
        ang = ang + state(i);
        x = x + l * sin(ang);
        y = y + l * cos(ang);
        
        pos(i+1,:) = [x y];
    end
    
end
