%   Copyright 2010 The MathWorks, Inc.

function USBLog(port)
NUM_OF_CYCLE = 1000;
PERIOD = 0.1;

%These are the values that are logged:

%Motor Positions (16 bits)
MotorAPos = zeros(1, NUM_OF_CYCLE);
MotorBPos = zeros(1, NUM_OF_CYCLE);
MotorCPos = zeros(1, NUM_OF_CYCLE);
%Sensor Readings (16 bits)
Sensor1 = zeros(1, NUM_OF_CYCLE);
Sensor2 = zeros(1, NUM_OF_CYCLE);
Sensor3 = zeros(1, NUM_OF_CYCLE);
Sensor4 = zeros(1, NUM_OF_CYCLE);
%User defined variables
UserA = zeros(1, NUM_OF_CYCLE); %8 bits
UserB = zeros(1, NUM_OF_CYCLE); %8
UserC = zeros(1, NUM_OF_CYCLE); %16
UserD = zeros(1, NUM_OF_CYCLE); %16
UserE = zeros(1, NUM_OF_CYCLE); %16
UserF = zeros(1, NUM_OF_CYCLE); %32
UserG = zeros(1, NUM_OF_CYCLE); %32
%System battery reading (in volts*1000), 16 bits
Battery = zeros(1, NUM_OF_CYCLE);
%System clock (milliseconds), 32 bits
Clock = zeros(1, NUM_OF_CYCLE);


disp('Note: nxtusb API requires LEGO MINDSTORMS NXT Driver(Fantom) to make it work.');
LightSensorDAQ = zeros(1, NUM_OF_CYCLE);
UltrasonicSensorDAQ = zeros(1, NUM_OF_CYCLE);
disp('Start the program in the NXT, wait for 10 seconds...');
nu = nxtusb;
if nu.CurrentState == 3 % NXT_CONNECT
    % NXT-USB interface has already been opened
    disp(['Connection Information: Id: ', num2str(nu.Id)]);
    disp(['Connection Information: Name: ', nu.Name]);
    for i = 1:NUM_OF_CYCLE
        [len, data] = read(nu, 'uint8', 64);
        if len == 64
            MotorAPos(i)    = Two_Bytes_To_int16(data(2), data(1));
            MotorBPos(i)    = Two_Bytes_To_int16(data(4), data(3));
            MotorCPos(i)    = Two_Bytes_To_int16(data(6), data(5));

            Sensor1(i)      = Two_Bytes_To_int16(data(8), data(7));
            Sensor2(i)      = Two_Bytes_To_int16(data(10), data(9));
            Sensor3(i)      = Two_Bytes_To_int16(data(12), data(11));
            Sensor4(i)      = Two_Bytes_To_int16(data(14), data(13));

            UserA(i)        = One_Byte_To_int8(data(15));
            UserB(i)        = One_Byte_To_int8(data(16));
            UserC(i)        = Two_Bytes_To_int16(data(18), data(17));
            UserD(i)        = Two_Bytes_To_int16(data(20), data(19));
            UserE(i)        = Two_Bytes_To_int16(data(22), data(21));
            UserF(i)        = Two_Bytes_To_int16(data(24), data(23));
            UserG(i)        = Two_Bytes_To_int16(data(26), data(25));

            Battery(i)      = Two_Bytes_To_int16(data(28), data(27));
            Clock(i)        = Four_Bytes_To_int32(data(32), data(31), data(30), data(29));
            
            if (mod(i,10) == 0)
                i / NUM_OF_CYCLE * 100
            end
        else
            delete(nu);
            error('Cannot read data from NXT.')
        end
    end
else
    disp(['CurrentState: ', num2str(nu.CurrentState)]);
    error('Cannot connect to NXT. Check the NXT-USB connection.')
end
close(nu);
delete(nu);

log.MotorAPos = MotorAPos;
log.MotorBPos = MotorBPos;
log.MotorCPos = MotorCPos;
log.Sensor1 = Sensor1;
log.Sensor2 = Sensor2;
log.Sensor3 = Sensor3;
log.Sensor4 = Sensor4;
log.UserA = UserA;
log.UserB = UserB;
log.UserC = UserC;
log.UserD = UserD;
log.UserE = UserE;
log.UserF = UserF;
log.Battery = Battery;
log.Clock = Clock;
assignin('base', 'log', log);

disp('Finished.')



end


function val = Four_Bytes_To_int32(MSB, msb, lsb, LSB)
    MSB = uint32(MSB);  %most significant byte
    msb = uint32(msb);  %2nd most significant byte
    lsb = uint32(lsb);  %3rd most significant byte
    LSB = uint32(LSB);  %4th most (least) significant byte
    
    synthesis = LSB + uint32(2^8)*lsb + uint32(2^16)*msb + uint32(2^24)*MSB;
    
    val = typecast(synthesis, 'int32');
end

function val = Two_Bytes_To_int16(msb, lsb)
    msb = uint16(msb);
    lsb = uint16(lsb);
    val = typecast(msb*uint16(256)+lsb,'int16');
end

function val = One_Byte_To_int8(msb)
    msb = uint8(msb);
    val = typecast(msb,'int8');
end

