Add required methods, core method implementation without exceptions.

Took a bit to figure out how swift arrays/Data works to simplify things down...

Sources/SwiftRPLidar/SwiftRPLidar.swift

@@ -1,29 +1,30 @@
-import foundation
+import Foundation
+import SwiftSerial
 
 struct Constants{
-    static let SYNC = 0xA5
-    static let SYNC2 = 0x5A
-
-    static let GET_INFO = 0x50
-    static let GET_HEALTH = 0x52
-
-    static let STOP = 0x25
-    static let RESET = 0x40
-
-    static let SCAN = 0x20
-    static let FORCE_SCAN = 0x21
-
+    static let SYNC: UInt8 = 0xA5
+    static let SYNC2: UInt8 = 0x5A
+    
+    static let GET_INFO: UInt8 = 0x50
+    static let GET_HEALTH: UInt8 = 0x52
+    
+    static let STOP: UInt8 = 0x25
+    static let RESET: UInt8 = 0x40
+    
+    static let SCAN: UInt8 = 0x20
+    static let FORCE_SCAN: UInt8 = 0x21
+    
     static let DESCRIPTOR_LEN = 7
     static let INFO_LEN = 20
     static let HEALTH_LEN = 3
-
-    static let INFO_TYPE = 4
-    static let HEALTH_TYPE = 6
-    static let SCAN_TYPE = 129
-
+    
+    static let INFO_TYPE: UInt8 = 4
+    static let HEALTH_TYPE: UInt8 = 6
+    static let SCAN_TYPE: UInt8 = 129
+    
+    static let SET_PWM_BYTE: UInt8 = 0xF0
     static let MAX_MOTOR_PWM = 1023
     static let DEFAULT_MOTOR_PWM = 660
-    static let SET_PWM_BYTE = 0xF0
 }
 
 enum HEALTH_STATUSES {
@@ -34,18 +35,177 @@ struct SwiftRPLidar {
     var text = "Hello, World!"
 }
 
+func processScan(raw: Data){
+    
+}
+
+struct LidarScan{
+    var quality: Float
+    var angle: Float
+    var distance: Float
+}
+
+
+
+typealias MeasurementHandler = (_ scan: LidarScan) -> Void
+typealias ScanHandler = (_ scans: [LidarScan]) -> Void
+
 class RPLidar{
-    private var serialPort: String
-    private var timeout
-    private var motor
-    private var baudrate
-
-    init?(port: String, baudrate: Int = 115200, timeout: Int = 1){
-        self.serialPort = port
-        self.baudrate = baudrate
-        self.timeout = timeout
+    private var motor: Bool = false
+    private var serialPort: SerialPort? = nil
+    private var motorRunning = false
+    
+    
+    init(serialPort: SerialPort) throws {
+        self.serialPort = serialPort
     }
-
-    func connect(){
+    
+    deinit {
+        if(serialPort != nil){
+            serialPort?.closePort()
+        }
     }
-}
+    
+    public func connect() throws {
+        disconnect()
+        try serialPort!.openPort()
+    }
+    
+    public func disconnect(){
+        if(serialPort != nil){
+            // Need to close, SwiftSerial is blocking.
+            serialPort!.closePort()
+        }
+    }
+    
+    public func startMotor() throws{
+        // A1
+        serialPort?.dtr = true
+        // A2
+        try self.setPwm(Constants.DEFAULT_MOTOR_PWM)
+        self.motorRunning = true
+    }
+    
+    public func stopMotor() throws{
+        try setPwm(0)
+        serialPort?.dtr = false
+        motorRunning = false
+    }
+    
+    public func setPwm(_ pwm: Int) throws{
+        assert(0 <= pwm && pwm <= Constants.MAX_MOTOR_PWM)
+        try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData())
+    }
+    
+    public func getInfo() throws -> (UInt8, UInt8, UInt8, String){
+        try sendCommand(Data([Constants.GET_HEALTH]))
+        let (dataSize, isSingle, dataType) = try readDescriptor()!
+        if (dataSize != Constants.INFO_LEN){
+            
+        }
+        if(!isSingle){
+            
+        }
+        if(dataType != Constants.INFO_TYPE){
+            
+        }
+        
+        let raw = try readResponse(dataSize)
+        let serialNumber = String(bytes: raw[4...], encoding: .ascii)!
+        return (raw[0], raw[2], raw[3], serialNumber)
+    }
+    
+    public func getHealth(){
+        
+    }
+    
+    public func clearInput(){
+        
+    }
+    
+    public func stop(){
+        
+    }
+    
+    public func reset(){
+        
+    }
+    
+    public func iterMeasurements(maxBufferMeasurements: Int = 500, _ onMeasure: MeasurementHandler) {
+        
+    }
+    
+    public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler){
+        
+    }
+    
+    private func sendPayloadCommand(_ cmd: Int, payload: Data) throws{
+        let size = UInt8(payload.count)
+        var req = Constants.SYNC.asData() + cmd.asData() + size.asData() + payload
+        let checksum = calcChecksum(req)
+        req += checksum.asData()
+        try serialPort?.writeData(req)
+    }
+    
+    private func sendPayloadCommand(_ cmd: UInt8, payload: Data) throws {
+        return try sendPayloadCommand(Int(cmd), payload: payload)
+    }
+    
+    private func calcChecksum(_ data: Data) -> Int{
+        var checksum = 0;
+        data.forEach{ body in
+            checksum ^= Int(body)
+        }
+        return checksum
+    }
+    
+    private func sendCommand(_ command: Data) throws{
+        var req = Constants.SYNC.asData()
+        req.append(command)
+        try serialPort?.writeData(req)
+    }
+    
+    private func readDescriptor() throws -> (UInt8, Bool, UInt8)?{
+        guard let descriptor = try serialPort?.readData(ofLength: Constants.DESCRIPTOR_LEN) else {
+            return nil
+        }
+        if (descriptor.count != Constants.DESCRIPTOR_LEN){
+            
+        }
+        else if (descriptor[0...1] != Data([Constants.SYNC, Constants.SYNC2])){
+            
+        }
+        let isSingle = descriptor[descriptor.count - 2] == 0
+        return (descriptor[2], isSingle, descriptor[descriptor.count - 1])
+    }
+    
+    private func readResponse(_ dataSize: Int) throws -> Data {
+        guard let data = try serialPort?.readData(ofLength: dataSize) else{
+            return Data()
+        }
+        if(data.count != dataSize){
+            
+        }
+        return data
+    }
+    
+    private func readResponse(_ dataSize: UInt8) throws -> Data {
+        return try readResponse(Int(dataSize))
+    }
+    
+    
+}
+
+extension Int{
+    func asData() -> Data {
+        var int = self
+        return Data(bytes: &int, count: int / Int(UINT8_MAX))
+    }
+}
+
+extension UInt8 {
+    func asData() -> Data {
+        return Data([self])
+    }
+}
+