//
//  GMFGMapARViewControllerUtilities.swift
//  MVM5G
//
//  Created by Muthulingam, Muthuraj on 01/06/20.
//  Copyright © 2020 Kyle. All rights reserved.
//

import Foundation
import SceneKit

public extension CLLocation {
  func bearingToLocationRadian(_ destinationLocation: CLLocation) -> CGFloat {
    
    let lat1 = self.coordinate.latitude.degreesToRadians
    let lon1 = self.coordinate.longitude.degreesToRadians
    
    let lat2 = destinationLocation.coordinate.latitude.degreesToRadians
    let lon2 = destinationLocation.coordinate.longitude.degreesToRadians
    
    let dLon = lon2 - lon1
    
    let y = sin(dLon) * cos(lat2)
    let x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon)
    let radiansBearing = atan2(y, x)
    
    return CGFloat(radiansBearing)
  }
  
  func bearingToLocationDegrees(destinationLocation: CLLocation) -> CGFloat {
    return bearingToLocationRadian(destinationLocation).radiansToDegrees
  }
    
 func bearingAngle(to location: CLLocation) -> Double {

        let latA = coordinate.latitude.degreesToRadians
        let lonA = coordinate.longitude.degreesToRadians

        let latB = location.coordinate.latitude.degreesToRadians
        let lonB = location.coordinate.longitude.degreesToRadians

        let longitudeDiff = lonB - lonA

        let y = sin(longitudeDiff) * cos(latB)
        let x = cos(latA) * sin(latB) - sin(latA) * cos(latB) * cos(longitudeDiff)

        return atan2(y, x)
    }
}

public extension CGFloat {
  var degreesToRadians: CGFloat { return self * .pi / 180 }
  var radiansToDegrees: CGFloat { return self * 180 / .pi }
}

private extension Double {
  var degreesToRadians: Double { return Double(CGFloat(self).degreesToRadians) }
  var radiansToDegrees: Double { return Double(CGFloat(self).radiansToDegrees) }
}

public final class GMFGMapARViewControllerUtilities {

    private init() {}

    static func positionFromTransform(_ transform: matrix_float4x4, reversed: Bool = false) -> SCNVector3 {

        if reversed {
            return SCNVector3Make(-transform.columns.3.x, -transform.columns.3.y, -transform.columns.3.z)
        }

        return SCNVector3Make(transform.columns.3.x, transform.columns.3.y, transform.columns.3.z)
    }

    static func position(model modelNode: SCNNode, byDistance distance: Float, fromUserLocation userLocation: CLLocation, toLocation location: CLLocation) {
        modelNode.position = translateNodeFrom(userLocation: userLocation, toLocation: location, distance: distance)
        modelNode.scale = scaleNode(distance, location: location)
    }

    private static func translateNodeFrom(userLocation: CLLocation, toLocation location: CLLocation, distance: Float) -> SCNVector3 {
        let locationTransform = transformMatrix(matrix: matrix_identity_float4x4, originLocation: userLocation, targetLocation: location, distance: distance)
        return positionFromTransform(locationTransform)
    }

    private static func scaleNode(_ distance: Float, location: CLLocation) -> SCNVector3 {
        let scale = max( min( Float(1000/distance), 1.5 ), 3 )
        return SCNVector3(x: scale, y: scale, z: scale)
    }

    private static func transformMatrix(matrix: simd_float4x4, originLocation: CLLocation, targetLocation: CLLocation, distance: Float) -> simd_float4x4 {
        let bearingAngle =  originLocation.bearingAngle(to: targetLocation)
        let rotationMatrix = rotateAroundY(matrix_identity_float4x4, Float(bearingAngle))

        var position = vector_float4(0.0, 0.0, -distance, 0.0)

        if distance > 1000.0 {
            position = vector_float4(0.0, 0.0, -25.0, 0.0)
        }

        let translationMatrix = getTranslationMatrix(matrix_identity_float4x4, position)

        let transformMatrix = simd_mul(rotationMatrix, translationMatrix)

        return simd_mul(matrix, transformMatrix)
    }

    private static func getTranslationMatrix(_ matrix: simd_float4x4, _ translation : vector_float4) -> simd_float4x4 {
        var matrix = matrix
        matrix.columns.3 = translation
        return matrix
    }

    private static func rotateAroundY(_ matrix: simd_float4x4, _ degrees: Float) -> simd_float4x4 {
        var matrix = matrix

        matrix.columns.0.x = cos(degrees)
        matrix.columns.0.z = -sin(degrees)

        matrix.columns.2.x = sin(degrees)
        matrix.columns.2.z = cos(degrees)

        return matrix.inverse
    }
}
