//
//  RTABMap.swift
//  RTABMapApp
//
//  Created by Mathieu Labbe on 2020-12-31.
//

import Foundation
import ARKit

class RTABMap {
    var native_rtabmap: UnsafeMutableRawPointer
    
    struct Observation {
        weak var observer: RTABMapObserver?
    }
    private var observations = [ObjectIdentifier : Observation]()
    
    func addObserver(_ observer: RTABMapObserver) {
        let id = ObjectIdentifier(observer)
        observations[id] = Observation(observer: observer)
    }

    func removeObserver(_ observer: RTABMapObserver) {
        let id = ObjectIdentifier(observer)
        observations.removeValue(forKey: id)
    }
    
    init() {
        native_rtabmap = UnsafeMutableRawPointer(mutating: createNativeApplication())
    }
    
    func setupCallbacksWithCPP()
    {
        setupCallbacksNative(native_rtabmap,
             UnsafeMutableRawPointer(Unmanaged.passUnretained(self).toOpaque()),
             //progressCallback
             {(observer, count, max) -> Void in
                         // Extract pointer to `self` from void pointer:
                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
                         // Call instance method:
                         //mySelf.TestMethod();
                for (id, observation) in mySelf.observations {
                    // If the observer is no longer in memory, we
                    // can clean up the observation for its ID
                    guard let observer = observation.observer else {
                        mySelf.observations.removeValue(forKey: id)
                        continue
                    }

                    observer.progressUpdated(mySelf, count: Int(count), max: Int(max))
                }
             },
             //initCallback
             {(observer, status, msg) -> Void in
                         // Extract pointer to `self` from void pointer:
                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
                         // Call instance method:
                         //mySelf.TestMethod();
                for (id, observation) in mySelf.observations {
                    // If the observer is no longer in memory, we
                    // can clean up the observation for its ID
                    guard let observer = observation.observer else {
                        mySelf.observations.removeValue(forKey: id)
                        continue
                    }

                    let str = String(cString: msg!)
                    observer.initEventReceived(mySelf, status: Int(status), msg: str)
                }
             },
             //statsUpdatedCallback
             {(observer, nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, matches, featuresExtracted, hypothesis, nodesDrawn, fps, rejected, rehearsalValue, optimizationMaxError, optimizationMaxErrorRatio, distanceTravelled, fastMovement, landmarkDetected, x, y, z, roll, pitch, yaw) -> Void in
                         // Extract pointer to `self` from void pointer:
                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
                         // Call instance method:
                         //mySelf.TestMethod();
                for (id, observation) in mySelf.observations {
                    // If the observer is no longer in memory, we
                    // can clean up the observation for its ID
                    guard let observer = observation.observer else {
                        mySelf.observations.removeValue(forKey: id)
                        continue
                    }

                    observer.statsUpdated(mySelf, nodes: Int(nodes), words: Int(words), points: Int(points), polygons: Int(polygons), updateTime: updateTime, loopClosureId: Int(loopClosureId), highestHypId: Int(highestHypId), databaseMemoryUsed: Int(databaseMemoryUsed), inliers: Int(inliers), matches: Int(matches), featuresExtracted: Int(featuresExtracted), hypothesis: hypothesis, nodesDrawn: Int(nodesDrawn), fps: fps, rejected: Int(rejected), rehearsalValue: rehearsalValue, optimizationMaxError: optimizationMaxError, optimizationMaxErrorRatio: optimizationMaxErrorRatio, distanceTravelled: distanceTravelled, fastMovement: Int(fastMovement), landmarkDetected: Int(landmarkDetected), x: x, y: y, z: z, roll: roll, pitch: pitch, yaw: yaw)
                }
             },
             //cameraInfoEventCallback
             {(observer, type, key, value) -> Void in
                         // Extract pointer to `self` from void pointer:
                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
                         // Call instance method:
                         //mySelf.TestMethod();
                for (id, observation) in mySelf.observations {
                    // If the observer is no longer in memory, we
                    // can clean up the observation for its ID
                    guard let observer = observation.observer else {
                        mySelf.observations.removeValue(forKey: id)
                        continue
                    }
					
                    let strKey = String(cString: key!)
                    let strValue = String(cString: value!)
                    observer.cameraInfoEventReceived(mySelf, type: Int(type), key: strKey, value: strValue)
                }
             })
    }
    
    deinit {
        destroyNativeApplication(native_rtabmap)
    }
    
    func initGlContent() {
        initGlContentNative(native_rtabmap)
    }
    
    func setupGraphic(size: CGSize, orientation: UIInterfaceOrientation) {
        var rotation: Int32
        switch orientation.rawValue {
        case 4:
            rotation=2
        case 1:
            rotation=3
        case 2:
            rotation=1
        default:
            rotation=0
        }
        //NSLog("Orientation: ios %d android %d, view=%dx%d", orientation.rawValue, rotation, Int32(size.width), Int32(size.height))
        setScreenRotationNative(native_rtabmap, rotation)
        setupGraphicNative(native_rtabmap, Int32(size.width), Int32(size.height));
    }
    
    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool, clearDatabase: Bool) -> Int {
        databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize, clearDatabase))
        }
    }
    
    func save(databasePath:String) {
        databasePath.utf8CString.withUnsafeBufferPointer { buffer in
            saveNative(native_rtabmap, databasePath)
        }
    }
    
    func recover(from: String, to: String) -> Bool {
        from.utf8CString.withUnsafeBufferPointer { bufferFrom -> Bool in
            to.utf8CString.withUnsafeBufferPointer { bufferTo -> Bool in
                return recoverNative(native_rtabmap, bufferFrom.baseAddress, bufferTo.baseAddress)
            }
        }
    }
    
    func removeMeasure() {
        removeMeasureNative(native_rtabmap)
    }
    func addMeasureButtonClicked() {
        addMeasureNative(native_rtabmap)
    }
    func teleportButtonClicked() {
        teleportNative(native_rtabmap)
    }
    func setMeasuringMode(_ mode: Int) {
        setMeasuringModeNative(native_rtabmap, Int32(mode))
    }
    func setMetricSystem(_ enabled: Bool) {
        setMetricSystemNative(native_rtabmap, enabled)
    }
    func setMeasuringTextSize(_ size: Float32) {
        setMeasuringTextSizeNative(native_rtabmap, size)
    }
    func clearMeasures() {
        clearMeasuresNative(native_rtabmap)
    }
    func cancelProcessing() {
        cancelProcessingNative(native_rtabmap);
    }
    
    func postProcessing(approach: Int) -> Int {
        return Int(postProcessingNative(native_rtabmap, Int32(approach)))
    }
    
    func exportMesh(
        cloudVoxelSize: Float,
        regenerateCloud: Bool,
        meshing: Bool,
        textureSize: Int,
        textureCount: Int,
        normalK: Int,
        optimized: Bool,
        optimizedVoxelSize: Float,
        optimizedDepth: Int,
        optimizedMaxPolygons: Int,
        optimizedColorRadius: Float,
        optimizedCleanWhitePolygons: Bool,
        optimizedMinClusterSize: Int,
        optimizedMaxTextureDistance: Float,
        optimizedMinTextureClusterSize: Int,
        textureVertexColorPolicy: Int,
        blockRendering: Bool) -> Bool
    {
       return exportMeshNative(native_rtabmap,
                                    cloudVoxelSize,
                                    regenerateCloud,
                                    meshing,
                                    Int32(textureSize),
                                    Int32(textureCount),
                                    Int32(normalK),
                                    optimized,
                                    optimizedVoxelSize,
                                    Int32(optimizedDepth),
                                    Int32(optimizedMaxPolygons),
                                    optimizedColorRadius,
                                    optimizedCleanWhitePolygons,
                                    Int32(optimizedMinClusterSize),
                                    optimizedMaxTextureDistance,
                                    Int32(optimizedMinTextureClusterSize),
                            		Int32(textureVertexColorPolicy),
                                    blockRendering)
    }
    
    func postExportation(visualize: Bool) -> Bool
    {
        return postExportationNative(native_rtabmap, visualize)
    }
    
    func writeExportedMesh(directory: String, name: String) -> Bool
    {
        directory.utf8CString.withUnsafeBufferPointer { bufferDir in
            name.utf8CString.withUnsafeBufferPointer { bufferName in
                return writeExportedMeshNative(native_rtabmap, bufferDir.baseAddress, bufferName.baseAddress)
            }
        }
    }
    
    func onTouchEvent(touch_count: Int, event: Int, x0: Float, y0: Float, x1: Float, y1: Float) {
        onTouchEventNative(native_rtabmap, Int32(touch_count), Int32(event), x0, y0, x1, y1)
    }
    
    func setPausedMapping(paused: Bool) {
        setPausedMappingNative(native_rtabmap, paused)
    }
    
    func render() -> Int {
        return Int(renderNative(native_rtabmap))
    }
    
    func startCamera(imageOverlayInFirstPerson: Bool = true) -> Bool {
        return startCameraNative(native_rtabmap)
    }
    
    func stopCamera() {
        stopCameraNative(native_rtabmap)
    }
    
    func setCamera(type: Int) {
        setCameraNative(native_rtabmap, Int32(type))
    }

    func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) {
        let pose = frame.camera.transform   // ViewMatrix
        let rotation = GLKMatrix3(
            m: (pose[0,0], pose[0,1], pose[0,2],
            pose[1,0], pose[1,1], pose[1,2],
            pose[2,0], pose[2,1], pose[2,2]))
        
        let quat = GLKQuaternionMakeWithMatrix3(rotation)
                
        let confMap = frame.sceneDepth?.confidenceMap
        let depthMap = frame.sceneDepth?.depthMap
        let points = frame.rawFeaturePoints?.points

        if points != nil && (depthMap != nil || points!.count>0)
        {
            let v = frame.camera.viewMatrix(for: orientation)
            let p = frame.camera.projectionMatrix(for: orientation, viewportSize: viewport, zNear: 0.5, zFar: 50.0)
            
            let rotation = GLKMatrix3(
                m: (v[0,0], v[0,1], v[0,2],
                    v[1,0], v[1,1], v[1,2],
                    v[2,0], v[2,1], v[2,2]))
            
            let quatv = GLKQuaternionMakeWithMatrix3(rotation)
            
            let texX1 = (1-(2*frame.camera.intrinsics[0,0]/Float(frame.camera.imageResolution.width)) / p[0,0])/2
            let texY1 = (1-(2*frame.camera.intrinsics[1,1]/Float(frame.camera.imageResolution.height)) / p[1,1])/2
            
            let texX2 = (1-(2*frame.camera.intrinsics[0,0]/Float(frame.camera.imageResolution.width)) / p[1,1])/2
            let texY2 = (1-(2*frame.camera.intrinsics[1,1]/Float(frame.camera.imageResolution.height)) / p[0,0])/2
            
            //11 10 01 00 // portrait
            //01 11 00 10 // right
            //10 00 11 01 // left
            //00 01 10 11 // down
            var texCoord: [Float]
            switch orientation {
            case .portrait:
                texCoord = [1-texX2, 1-texY2, 1-texX2,texY2, texX2, 1-texY2, texX2, texY2]
            case .landscapeRight:
                texCoord = [texX1, 1-texY1, 1-texX1, 1-texY1, texX1, texY1, 1-texX1, texY1]
            case .landscapeLeft:
                texCoord = [1-texX1, texY1, texX1, texY1, 1-texX1, 1-texY1, texX1, 1-texY1]
            default: // down
                texCoord = [texX2, texY2, texX2, 1-texY2, 1-texX2, texY2, 1-texX2, 1-texY2]
            }
            
            frame.rawFeaturePoints?.points.withUnsafeBufferPointer { bufferPoints in
                
                CVPixelBufferLockBaseAddress(frame.capturedImage, CVPixelBufferLockFlags.readOnly)
                var depthDataPtr: UnsafeMutableRawPointer?
                var depthSize: Int32 = 0
                var depthWidth: Int32 = 0
                var depthHeight: Int32 = 0
                var depthFormat: Int32 = 0
                if depthMap != nil {
                    CVPixelBufferLockBaseAddress(depthMap!, CVPixelBufferLockFlags.readOnly)
                    depthDataPtr = CVPixelBufferGetBaseAddress(depthMap!)!
                    depthSize = Int32(CVPixelBufferGetDataSize(depthMap!))
                    depthWidth = Int32(CVPixelBufferGetWidth(depthMap!))
                    depthHeight = Int32(CVPixelBufferGetHeight(depthMap!))
                    depthFormat = Int32(CVPixelBufferGetPixelFormatType(depthMap!))
                }
                
                var confDataPtr: UnsafeMutableRawPointer?
                var confSize: Int32 = 0
                var confWidth: Int32 = 0
                var confHeight: Int32 = 0
                var confFormat: Int32 = 0
                if confMap != nil {
                    CVPixelBufferLockBaseAddress(confMap!, CVPixelBufferLockFlags.readOnly)
                    confDataPtr = CVPixelBufferGetBaseAddress(confMap!)!
                    confSize = Int32(CVPixelBufferGetDataSize(confMap!))
                    confWidth = Int32(CVPixelBufferGetWidth(confMap!))
                    confHeight = Int32(CVPixelBufferGetHeight(confMap!))
                    confFormat = Int32(CVPixelBufferGetPixelFormatType(confMap!))
                }
                
                if(frame.lightEstimate != nil) {
                    addEnvSensor(type: 4, value: Float(frame.lightEstimate!.ambientIntensity))
                }
                
                var lost = false
                switch frame.camera.trackingState {
                case .normal:
                    lost = false
                case .limited(.excessiveMotion):
                    lost = false
                case .limited(.insufficientFeatures):
                    lost = false
                default:
                    lost = true
                }
                // Notify lost with pose=null

                postOdometryEventNative(native_rtabmap,
                                        !lost ? pose[3,0]:0, !lost ? pose[3,1]:0, !lost ? pose[3,2]:0, !lost ? quat.x:0, !lost ? quat.y:0, !lost ? quat.z:0, !lost ? quat.w:0,
                                        frame.camera.intrinsics[0,0], // fx
                                        frame.camera.intrinsics[1,1], // fy
                                        frame.camera.intrinsics[2,0], // cx
                                        frame.camera.intrinsics[2,1], // cy
                                        frame.timestamp,
                                        CVPixelBufferGetBaseAddressOfPlane(frame.capturedImage, 0),  // y plane pointer
                                        nil,                                                         // u plane pointer
                                        CVPixelBufferGetBaseAddressOfPlane(frame.capturedImage, 1),  // v plane pointer
                                        Int32(CVPixelBufferGetBytesPerRowOfPlane(frame.capturedImage, 0)) * Int32(CVPixelBufferGetHeightOfPlane(frame.capturedImage, 0)),  // yPlaneLen
                                        Int32(CVPixelBufferGetWidth(frame.capturedImage)),           // rgb width
                                        Int32(CVPixelBufferGetHeight(frame.capturedImage)),          // rgb height
                                        Int32(CVPixelBufferGetPixelFormatType(frame.capturedImage)), // rgb format
                                        depthDataPtr, // depth pointer
                                        depthSize,    // depth size
                                        depthWidth,   // depth width
                                        depthHeight,  // depth height
                                        depthFormat,  // depth format
                                        confDataPtr, // conf pointer
                                        confSize,    // conf size
                                        confWidth,   // conf width
                                        confHeight,  // conf height
                                        confFormat,  // conf format
                                        bufferPoints.baseAddress, Int32(frame.rawFeaturePoints!.points.count), 4,
                                        v[3,0], v[3,1], v[3,2], quatv.x, quatv.y, quatv.z, quatv.w,
                                        p[0,0], p[1,1], p[2,0], p[2,1], p[2,2], p[2,3], p[3,2],
                                        texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7])
                if depthMap != nil {
                    CVPixelBufferUnlockBaseAddress(depthMap!, CVPixelBufferLockFlags.readOnly)
                }
                if confMap != nil {
                    CVPixelBufferUnlockBaseAddress(confMap!, CVPixelBufferLockFlags.readOnly)
                }
                CVPixelBufferUnlockBaseAddress(frame.capturedImage, CVPixelBufferLockFlags.readOnly)
            }
        }
    }
        
    // Parameters
    func setOnlineBlending(enabled: Bool) {
        setOnlineBlendingNative(native_rtabmap, enabled)
    }
    func setMapCloudShown(shown: Bool) {
        setMapCloudShownNative(native_rtabmap, shown)
    }
    func setOdomCloudShown(shown: Bool) {
        setOdomCloudShownNative(native_rtabmap, shown)
    }
    func setMeshRendering(enabled: Bool, withTexture: Bool) {
        setMeshRenderingNative(native_rtabmap, enabled, withTexture)
    }
    func setPointSize(value: Float) {
        setPointSizeNative(native_rtabmap, value)
    }
    func setFOV(angle: Float) {
        setFOVNative(native_rtabmap, angle)
    }
    func setOrthoCropFactor(value: Float) {
        setOrthoCropFactorNative(native_rtabmap, value)
    }
    func setGridRotation(value: Float) {
        setGridRotationNative(native_rtabmap, value)
    }
    func setLighting(enabled: Bool) {
        setLightingNative(native_rtabmap, enabled)
    }
    func setBackfaceCulling(enabled: Bool) {
        setBackfaceCullingNative(native_rtabmap, enabled)
    }
    func setWireframe(enabled: Bool) {
        setWireframeNative(native_rtabmap, enabled)
    }
    func setTextureColorSeamsHidden(hidden: Bool) {
        setTextureColorSeamsHiddenNative(native_rtabmap, hidden)
    }
    func setLocalizationMode(enabled: Bool) {
        setLocalizationModeNative(native_rtabmap, enabled)
    }
    func setDataRecorderMode(enabled: Bool) {
        setDataRecorderModeNative(native_rtabmap, enabled)
    }
    func setGraphOptimization(enabled: Bool) {
        setGraphOptimizationNative(native_rtabmap, enabled)
    }
    func setNodesFiltering(enabled: Bool) {
        setNodesFilteringNative(native_rtabmap, enabled)
    }
    func setGraphVisible(visible: Bool) {
        setGraphVisibleNative(native_rtabmap, visible)
    }
    func setGridVisible(visible: Bool) {
        setGridVisibleNative(native_rtabmap, visible)
    }
    func setFullResolution(enabled: Bool) {
        setFullResolutionNative(native_rtabmap, enabled)
    }
    func setSmoothing(enabled: Bool) {
        setSmoothingNative(native_rtabmap, enabled)
    }
    func setDepthBleedingError(value: Float) {
        setDepthBleedingErrorNative(native_rtabmap, value)
    }
    func setAppendMode(enabled: Bool) {
        setAppendModeNative(native_rtabmap, enabled)
    }
    func setUpstreamRelocalizationAccThr(value: Float) {
        setUpstreamRelocalizationAccThrNative(native_rtabmap, value)
    }
    func setMaxCloudDepth(value: Float) {
        setMaxCloudDepthNative(native_rtabmap, value)
    }
    func setMinCloudDepth(value: Float) {
        setMinCloudDepthNative(native_rtabmap, value)
    }
    func setCloudDensityLevel(value: Int) {
        setCloudDensityLevelNative(native_rtabmap, Int32(value))
    }
    func setMeshAngleTolerance(value: Float) {
        setMeshAngleToleranceNative(native_rtabmap, value)
    }
    func setMeshDecimationFactor(value: Float) {
        setMeshDecimationFactorNative(native_rtabmap, value)
    }
    func setMeshTriangleSize(value: Int) {
        setMeshTriangleSizeNative(native_rtabmap, Int32(value))
    }
    func setClusterRatio(value: Float) {
        setClusterRatioNative(native_rtabmap, value)
    }
    func setMaxGainRadius(value: Float) {
        setMaxGainRadiusNative(native_rtabmap, value)
    }
    func setRenderingTextureDecimation(value: Int) {
        setRenderingTextureDecimationNative(native_rtabmap, Int32(value))
    }
    func setBackgroundColor(gray: Float) {
        setBackgroundColorNative(native_rtabmap, gray)
    }
    func setDepthConfidence(value: Int) {
        setDepthConfidenceNative(native_rtabmap, Int32(value))
    }
    func setExportPointCloudFormat(format: String) {
        format.utf8CString.withUnsafeBufferPointer { bufferFormat in
            return setExportPointCloudFormatNative(native_rtabmap, bufferFormat.baseAddress)
        }
    }
    func setMappingParameter(key: String, value: String) {
        key.utf8CString.withUnsafeBufferPointer { bufferKey in
            value.utf8CString.withUnsafeBufferPointer { bufferValue in
                setMappingParameterNative(native_rtabmap, bufferKey.baseAddress, bufferValue.baseAddress)
            }
        }
    }
    func setGPS(location: CLLocation)
    {
        setGPSNative(native_rtabmap, location.timestamp.timeIntervalSince1970, location.coordinate.longitude, location.coordinate.latitude, location.altitude, location.horizontalAccuracy, location.course < 0.0 ? 0.0 : location.course)
    }
    func addEnvSensor(type: Int, value: Float)
    {
        addEnvSensorNative(native_rtabmap, Int32(type), value)
    }
    func setOrthoCropFactor(_ value: Float)
    {
        setOrthoCropFactorNative(native_rtabmap, value)
    }
    func setGridRotation(_ value: Float)
    {
        setGridRotationNative(native_rtabmap, value)
    }
}

func getPreviewImage(databasePath: String) -> UIImage?
{
    let imageOut = databasePath.utf8CString.withUnsafeBufferPointer { buffer -> UIImage? in
        var image = ImageNative()
        image = getPreviewImageNative(buffer.baseAddress)
        if let dataPtr = UnsafeRawPointer(image.data)
        {
            // copy data in swift
            let data = Data(bytes: dataPtr, count: Int(image.width*image.height*image.channels))
            // release memory
            releasePreviewImageNative(image)
            // Create bitmap image
            let bitmap = CIImage(bitmapData: data, bytesPerRow: Int(image.width*image.channels), size: CGSize(width: Int(image.width), height: Int(image.height)), format: CIFormat.BGRA8, colorSpace: nil)
            return UIImage(ciImage: bitmap)
        }
        return UIImage(named: "RTAB-Map1024")
    }
    return imageOut
}

protocol RTABMapObserver: class {
    func progressUpdated(_ rtabmap: RTABMap, count: Int, max: Int)
    func initEventReceived(_ rtabmap: RTABMap, status: Int, msg: String)
    func statsUpdated(_ rtabmap: RTABMap,
                      nodes: Int,
                      words: Int,
                      points: Int,
                      polygons: Int,
                      updateTime: Float,
                      loopClosureId: Int,
                      highestHypId: Int,
                      databaseMemoryUsed: Int,
                      inliers: Int,
                      matches: Int,
                      featuresExtracted: Int,
                      hypothesis: Float,
                      nodesDrawn: Int,
                      fps: Float,
                      rejected: Int,
                      rehearsalValue: Float,
                      optimizationMaxError: Float,
                      optimizationMaxErrorRatio: Float,
                      distanceTravelled: Float,
                      fastMovement: Int,
                      landmarkDetected: Int,
                      x: Float,
                      y: Float,
                      z: Float,
                      roll: Float,
                      pitch: Float,
                      yaw: Float)
    func cameraInfoEventReceived(_ rtabmap: RTABMap, type: Int, key: String, value: String)
}

extension String {

  func toPointer() -> UnsafePointer<UInt8>? {
    guard let data = self.data(using: String.Encoding.utf8) else { return nil }

    let buffer = UnsafeMutablePointer<UInt8>.allocate(capacity: data.count)
    let stream = OutputStream(toBuffer: buffer, capacity: data.count)

    stream.open()
    data.withUnsafeBytes({ (p: UnsafePointer<UInt8>) -> Void in
      stream.write(p, maxLength: data.count)
    })

    stream.close()

    return UnsafePointer<UInt8>(buffer)
  }
}