classdef L3G < arduinoio.LibraryBase & matlab.mixin.CustomDisplay
    % L3G Create a L3G device object to interface with L3GD20H, L3GD20, and L3G4200D gyros
    %   
    % sensor = addon(a, 'Pololu/L3G', pin) creates a L3G device object.
    
    % Copyright 2016-2017 The MathWorks, Inc.

    % References:
    % L3GD20H  https://www.adafruit.com/datasheets/L3GD20H.pdf
    % L3G4200D https://www.st.com/web/en/resource/technical/document/datasheet/CD00265057.pdf
    % L3GD20   https://www.pololu.com/file/0J563/L3GD20.pdf

    properties(Access = private, Constant = true)
        CREATE_L3G               = hex2dec('01')
        DELETE_L3G               = hex2dec('02')
        INIT_L3G                 = hex2dec('03')
        SET_TIMEOUT              = hex2dec('04')
        ENABLE_DEFAULT           = hex2dec('05')
        READ_GYRO                = hex2dec('06')
    end

    properties(Access = protected, Constant = true)
        LibraryName = 'Pololu/L3G'
        DependentLibraries = {'I2C'}
        ArduinoLibraryHeaderFiles = {'L3G/L3G.h'}
        CppHeaderFile = fullfile(arduinoio.FilePath(mfilename('fullpath')), 'src', 'L3GML.h')
        CppClassName = 'L3GML'
    end
    
    properties(Access = private)
        ResourceOwner = '';    % Resource owner of I2C devices are Arduino, e.g ''
    end
    
    properties(SetAccess = private)
        Bus
        DeviceType 
        SA0PinState
        I2CAddress
    end
    
    properties(Access = private, Constant = true)
        DEVICE_AUTO  = 1
        DEVICE_4200D = 2
        DEVICE_D20   = 3
        DEVICE_D20H  = 4
        
        SA0_AUTO     = 1
        SA0_LOW      = 2
        SA0_HIGH     = 3
    end
        
    methods(Hidden, Access = public)
        function obj = L3G(parentObj, varargin)
            %   Connect to a L3G device connected to Arduino hardware
            %
            %   Syntax:
            %   sensor = addon(a, 'Pololu/L3G')
            %   sensor = addon(a, 'Pololu/L3G', Name, Value)
            %
            %   Description:
            %   sensor = addon(a, 'Pololu/L3G', Name, Value)	Connects to a L3G device 
            %
            %   Example:
            %       a = arduino('COM3','Uno','libraries','Pololu/L3G');
            %       sensor = addon(a, 'Pololu/L3G')
            %
            %   Example:
            %       a = arduino('COM3','Uno','libraries','Pololu/L3G');
            %       sensor = addon(a, 'Pololu/L3G')
            %
            %   Input Arguments:
            %   parentObj  - Arduino
            %
            %   Name-Value Pair:
            %   'DeviceType'  - Type of the LSM303 device, specified as character vector ('4200D' 'D20' 'D20H' 'auto')
            %   'SA0PinState' - The state of SA0 pin on LSM303 board, specified as character vector ('high' 'low' 'auto')
            %
            %   Output Arguments:
            %   obj - L3G object
            
            narginchk(1, 7);
            obj.Parent = parentObj;
            
            try
                % Validate inputs           
                p = inputParser;
                addParameter(p, 'Bus', 0);
                addParameter(p, 'DeviceType', 'auto');
                addParameter(p, 'SA0PinState', 'auto');
                parse(p, varargin{:});
                
                % Configure I2C pins
                obj.Bus = p.Results.Bus;
                
                % Create L3G object
                createL3G(obj);
                
                % Initialize device type and SA0 pin state
                obj.DeviceType    = p.Results.DeviceType;
                obj.SA0PinState   = p.Results.SA0PinState;
                
                % Pre-check I2C resources conflicts
                try
                    i2cAddresses = getSharedResourceProperty(parentObj, obj.ResourceOwner, 'i2cAddresses');
                catch
                    i2cAddresses = [];
                end
                output = scanI2CBus(parentObj);
                address = zeros(1, numel(output));
                for i = 1:numel(output)
                    temp = output{i};
                    address(i) = hex2dec(temp(3:end));
                end
                address = intersect(address, [...
                        bin2dec('1101011'), bin2dec('1101010'), ...   % L3GD20H
                        bin2dec('1101000'), bin2dec('1101001'), ...   % L3G4200D
                        bin2dec('1101010'), bin2dec('1101011')]);     % L3GD20
                if ismember(address, i2cAddresses)
                	obj.localizedError('MATLAB:arduinoio:general:conflictI2CAddress', num2str(address), dec2hex(address));
                end
                init(obj);
                i2cAddresses = [i2cAddresses address];
                setSharedResourceProperty(parentObj, obj.ResourceOwner, 'i2cAddresses', i2cAddresses);
                obj.I2CAddress = address;

                % Enable library
                enableDefault(obj);  
            
                setSharedResourceProperty(parentObj, 'I2C', 'I2CIsUsed', true);
            catch e
                throwAsCaller(e);
            end
        end
    end
    
    methods(Access = protected)
        function delete(obj)
            try
                parentObj = obj.Parent;
                i2cAddresses = getSharedResourceProperty(parentObj, obj.ResourceOwner, 'i2cAddresses');
                if ~isempty(i2cAddresses)
                    % obj.I2CAddress can be empty if failed during construction
                    if ~isempty(obj.I2CAddress) 
                        i2cAddresses = setxor(i2cAddresses, obj.I2CAddress);
                    end
                end
                setSharedResourceProperty(parentObj, obj.ResourceOwner, 'i2cAddresses', i2cAddresses);
                
                deleteL3G(obj);
            catch
                % Do not throw errors on destroy.
                % This may result from an incomplete construction.
            end
        end  
    end
        
    methods(Access = private)
        function createL3G(obj)
            try 
                sendCommand(obj, obj.LibraryName, obj.CREATE_L3G, obj.Bus);
            catch e
                throwAsCaller(e);
            end
        end
        
        function deleteL3G(obj)
            try
                sendCommand(obj, obj.LibraryName, obj.DELETE_L3G, []);
            catch e
                throwAsCaller(e);
            end
        end

        function configureI2C(obj, bus)
            parentObj = obj.Parent;
            I2CTerminals = parentObj.getI2CTerminals();
            
            if bus ~= 1 % not Due board
                resourceOwner = '';
                sda = parentObj.getPinsFromTerminals(I2CTerminals(bus*2+1)); sda = sda{1};
                configurePinResource(parentObj, sda, resourceOwner, 'I2C', false);
                scl = parentObj.getPinsFromTerminals(I2CTerminals(bus*2+2)); scl = scl{1};
                configurePinResource(parentObj, scl, resourceOwner, 'I2C', false);
                obj.Pins = {sda scl};
            else
                obj.Pins = {'SDA1', 'SCL1'};
            end
        end
    end
    
    % Set properties methods
    methods
        function set.Bus(obj, bus)
            try
                maxBus = 0;
                if strcmp(obj.Parent.Board, 'Due')
                    maxBus = 1;
                end
                bus = arduinoio.internal.validateIntParameterRanged('I2C Bus', bus, 0, maxBus);
            catch
                buses = sprintf('%d, ', 0:maxBus); % 0, 1,
                buses = buses(1:end-2); % 0, 1
                obj.localizedError('MATLAB:arduinoio:general:invalidI2CBusNumber', obj.Parent.Board, buses);
            end
            configureI2C(obj, bus);
            obj.Bus = bus;
        end
        
        function set.DeviceType(obj, type)
            typeValues = {'auto', '4200D', 'D20H', 'D20'};
            try
                type = validatestring(type, typeValues);
                
            catch
                error('Pololu:L3G:invalidDeviceType', 'Device type must be one of the following: %s', strjoin(typeValues, ', '));
            end
            obj.DeviceType= type;
        end
        
        function set.SA0PinState(obj, state)
            stateValues = {'auto', 'low', 'high'};
            try
                state = validatestring(state, stateValues);
            catch
                error('Pololu:L3G:invalidSA0PinState', 'SA0 pin state must be one of the following: %s', strjoin(stateValues, ', '));
            end
            obj.SA0PinState= state;
        end
        
        function init(obj)
        % Initialize the library with the device being used and the state
        % of the SA0 pin
            cmdID = obj.INIT_L3G;
            data = [0 0];
            switch obj.DeviceType
                case 'auto'
                    data(1) = obj.DEVICE_AUTO;
                case '4200D'
                    data(1) = obj.DEVICE_4200D;
                case 'D20H'
                    data(1) = obj.DEVICE_D20H;
                case 'D20'
                    data(1) = obj.DEVICE_D20;
            end
            
            switch obj.SA0PinState
                case 'auto'
                    data(2) = obj.SA0_AUTO;
                case 'low'
                    data(2) = obj.SA0_LOW;
                case 'high'
                    data(2) = obj.SA0_HIGH;
            end
            
            val = sendCommand(obj, obj.LibraryName, cmdID, data);
            if val(1) == 0
                if strcmpi(obj.DeviceType, 'auto') && strcmpi(obj.SA0PinState, 'auto')
                    error('Pololu:L3G:failedAutoDetect1', 'Cannot auto-detect the device type and SA0 pin state. If using an Arduino Due or an Arduino clone, specify both device type and pin state');
                elseif strcmpi(obj.DeviceType, 'auto')
                    error('Pololu:L3G:failedAutoDetect2', 'Cannot auto-detect the device type. Specify it as one of the following:\nD20, D20H, 4200D');
                elseif strcmpi(obj.SA0PinState, 'auto')
                    error('Pololu:L3G:failedAutoDetect3', 'Cannot auto-detect the SA0 pin state. Specify it as one of the following:\nlow, high');
                end
            elseif strcmpi(obj.DeviceType, 'auto')
                switch val(2) % detected type
                    case 0
                        obj.DeviceType = '4200D';
                    case 1
                        obj.DeviceType = 'D20';
                    case 2
                        obj.DeviceType = 'D20H';
                end
            end
        end
        
        function enableDefault(obj)
        % 1. Turn on the L3G's gyro
        % 2. Set gyro full scale (gain) to default power-on value of +/- 250 dps (+/- 245 dps for L3GD20H).
        % 3. Select output data rate 200 Hz for L3G4200D, 189.4 Hz for L3GD20H and 190 Hz for L3GD20.
        %
            sendCommand(obj, obj.LibraryName, obj.ENABLE_DEFAULT, []);
        end
    end
    
    methods(Access = public)  
        function val = readAngularVelocity(obj)
            %   Measure angular velocity in rad/s
            %
            %   Syntax:
            %   readAngularVelocity(sensor)
            %
            %   Description:
            %   Get the angular velocity reading from device
            %
            %   Example:
            %       a = arduino('COM3','Uno','libraries','Pololu/L3G');
            %       sensor = addon(a,'Pololu/L3G');
            %       value = readAngularVelocity(sensor)
            %
            %   Input Arguments:
            %   obj - L3G device
            %
            %   Output Arguments:
            %   val - Returned angular velocity reading (rad/s) 
            
            cmdID = obj.READ_GYRO;
            
            try
                output = sendCommand(obj, obj.LibraryName, cmdID, []);
                val = zeros(1,3);
                for i = 1:3
                    val(i) = typecast(uint8(output( (i*2-1):(i*2) )), 'int16');
                end
                val = double(val);
                % 8.75 mdps/digit, 1 degree = pi/180 radian
                val = val*8.75/1000*(pi/180);
            catch e
                throwAsCaller(e);
            end
        end
    end
    
    methods (Access = protected)
        function displayScalarObject(obj)
            header = getHeader(obj);
            disp(header);
                        
            % Display main options
            fprintf('               Pins: ''%s''(SDA), ''%s''(SCL)\n', obj.Pins{1}, obj.Pins{2});
            fprintf('                Bus: %d\n', obj.Bus);
            fprintf('         I2CAddress: %d (0x%02s)\n', obj.I2CAddress, dec2hex(obj.I2CAddress));
            fprintf('         DeviceType: ''%s''\n', obj.DeviceType);
            fprintf('        SA0PinState: ''%s''\n', obj.SA0PinState);
            fprintf('\n');
                  
            % Allow for the possibility of a footer.
            footer = getFooter(obj);
            if ~isempty(footer)
                disp(footer);
            end
        end
    end
end