import Foundation
import SwiftSerial

struct Constants{
    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: 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
}

enum HEALTH_STATUSES {
    case GOOD, WARNING, ERROR
}

struct SwiftRPLidar {
    var text = "Hello, World!"
}

func processScan(raw: Data) throws -> LidarScan {
    let newScan = raw[0] & 0b1
    let inversedNewScan = (raw[0] >> 1) & 0b1
    let quality = raw[0] >> 2
    if (newScan == inversedNewScan){

    }
    if ((raw[1] & 0b1) != 1) {

    }
    let angle = (raw[1] >> 1) + (raw[2] << 7) / 64
    let distance = raw[3] + (raw[4] << 8) / 4
    return LidarScan(newScan: newScan, quality: quality, angle: angle, distance: distance)
}

struct LidarScan{
    var newScan: Bool
    var quality: Float
    var angle: Float
    var distance: Float
}



typealias MeasurementHandler = (_ scan: LidarScan) -> Void
typealias ScanHandler = (_ scans: [LidarScan]) -> Void

class RPLidar{
    private var motor: Bool = false
    private var serialPort: SerialPort? = nil
    private var motorRunning = false
    
    
    init(serialPort: SerialPort) throws {
        self.serialPort = serialPort
    }
    
    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])
    }
}