//
//  LocationCalculation.swift
//  jmarine
//
//  Created by chihirohirakawa on 2020/05/20.
//  Copyright © 2020 J-MarineCloud. All rights reserved.
//

import Foundation
import CoreLocation
// import Mapbox

class LocationCalculation{
    static let nm = 1852.0                 // 国際海里 1852.0[m]
    
    /**
     * NMをkmに変換
     */
    static func nm2km(nm: Double) -> Double{
        return nm * 1.852
    }
    
    /**
     * kmをNMに変換
     */
    static func km2nm(km: Double) -> Double{
        return km / 1.852
    }
    
    static func kn2kmh(kn: Double) -> Double{
        return  kn * 1.852
    }
    
    static func deg2rad(deg: Double) -> Double {
        return (deg / 180.0) * Double.pi
    }
    
    static func rad2deg(rad: Double) -> Double {
        return (rad / Double.pi) * 180.0
    }
    
    /**
     * 地理座標の確認
     */
    static func checkPosition(pos: CLLocationCoordinate2D) -> Bool { // ENvCheckPos
        return pos.latitude >= -90.0 && pos.latitude <= 90.0 && pos.longitude >= -180.0 && pos.longitude <= 180.0
    }

    /**
     * 角度の正規化
     */
    static func rangeDegree(degree: Double) -> Double {
        var rangeDegree = degree
        while (rangeDegree >= 360.0) {
            rangeDegree -= 360.0
        }

        while (rangeDegree < 0.0) {
            rangeDegree += 360.0
        }

        if (rangeDegree == 360.0) {
            rangeDegree = 0.0
        }

        return rangeDegree
    }
    
    /// 2点間距離の算出
    /// - Parameters:
    ///   - posAlat: 地点A緯度
    ///   - posAlon: 地点A経度
    ///   - posBlat: 地点B緯度
    ///   - posBlon: 地点B経度
    /// - Returns:
    ///   単位：メートル
    static func distance(posAlat:Double, posAlon:Double, posBlat:Double, posBlon:Double) -> Double {
        //2点の緯度の平均
        let latAvg = deg2rad(deg: posAlat + ((posBlat - posAlat) / 2.0))
        //2点の緯度差
        let latDifference = deg2rad(deg: posAlat - posBlat)
        //2点の経度差
        let lonDifference = deg2rad(deg: posAlon - posBlon)

        let curRadiusTemp = 1 - 0.00669438 * pow(sin(latAvg), 2.0)
        
        //子午線曲率半径
        let meridianCurvatureRadius = 6335439.327 / sqrt(pow(curRadiusTemp, 3.0))
        //卯酉線曲率半径
        let primeVerticalCircleCurvatureRadius = 6378137 / sqrt(curRadiusTemp)
        
        //2点間距離
        let distanceTemp = pow(meridianCurvatureRadius * latDifference, 2.0) + pow(primeVerticalCircleCurvatureRadius * cos(latAvg) * lonDifference, 2.0)
        return sqrt(distanceTemp) //メートル単位
    }
    /// 2点間距離の算出
    /// - Parameters:
    ///   - posA: 地点A
    ///   - posB: 地点B
    /// - Returns:
    ///   単位：メートル
    static func distance(posA: CLLocationCoordinate2D, posB: CLLocationCoordinate2D) -> Double {
        return distance(posAlat: posA.latitude, posAlon: posA.longitude, posBlat: posB.latitude, posBlon: posB.longitude)
    }
    
    /// 地点Bから地点Aをみたときの方位角
    /// - Parameters:
    ///   - posAlat: 地点A緯度
    ///   - posAlon: 地点A経度
    ///   - posBlat: 地点B緯度
    ///   - posBlon: 地点B経度
    /// - Returns:
    ///   単位：度
    static func direction(posAlat:Double, posAlon:Double, posBlat:Double, posBlon:Double) -> Double {
        let posA_lat = deg2rad(deg: posAlat)
        let posA_lon = deg2rad(deg: posAlon)
        let posB_lat = deg2rad(deg: posBlat)
        let posB_lon = deg2rad(deg: posBlon)
        
        //経度の中心地
        let posC_lat = (posA_lat + posB_lat) / 2.0
        let dx = 6378137 * (posB_lon - posA_lon) * cos(posC_lat)
        let dy = 6378137 * (posB_lat - posA_lat)

        if(dx == dy){
            return 0.0
        }
        
        //なぜか左向きが０度、反時計回りに増える
        let deg = rad2deg(rad: atan2(dy, dx))

        //それを修正
        let deg_o = deg + 90
        let deg_p = 360 - deg_o
        
        if(deg_p >= 360) {
            return deg_p - 360
        } else{
            return deg_p
        }
    }

    
    /// 起点から指定の距離方位分移動した地点の緯度経度
    /// - Parameters:
    ///   - startPointLat: 起点緯度
    ///   - startPointLon: 起点経度
    ///   - initialBearing: 方位
    ///   - distanceKilometers: 距離（キロメートル指定）
    /// - Returns:
    ///   移動した地点の緯度経度
    static func findPointAtDistanceFrom(
        startPointLat:Double,
        startPointLon:Double,
        initialBearing:Double,
        distanceKilometers:Double) -> CLLocationCoordinate2D {
        
        let initialBearingRadians = initialBearing * 2.0 * Double.pi / 360.0
        let radiusEarthKilometers = 6371.01
        let distRatio = distanceKilometers / radiusEarthKilometers
        let distRatioSine = sin(distRatio)
        let distRatioCosine = cos(distRatio)

        let startLatRad = deg2rad(deg: startPointLat)
        let startLonRad = deg2rad(deg: startPointLon)

        let startLatCos = cos(startLatRad)
        let startLatSin = sin(startLatRad)

        let endLatRads = asin((startLatSin * distRatioCosine) + (startLatCos * distRatioSine * cos(initialBearingRadians)))

        let endLonRads = startLonRad + atan2(sin(initialBearingRadians) * distRatioSine * startLatCos, distRatioCosine - startLatSin * sin(endLatRads))

        return CLLocationCoordinate2D(latitude: rad2deg(rad: endLatRads), longitude: rad2deg(rad: endLonRads))
    }
    
    /// 地点Aと地点Bをの間の位置
    /// - Parameters:
    ///   - posAlat: 地点A緯度
    ///   - posAlon: 地点A経度
    ///   - posBlat: 地点B緯度
    ///   - posBlon: 地点B経度
    /// - Returns:
    ///   ２地点間の中間緯度経度
    static func halfPoint(posAlat:Double, posAlon:Double, posBlat:Double, posBlon:Double) -> CLLocationCoordinate2D {
        
        let lat = ( posAlat + posBlat ) / 2
        let lon = ( posAlon + posBlon ) / 2

        return CLLocationCoordinate2D(latitude: lat, longitude: lon)
    }
    
    /// Plygonの中心の位置
    /// - Parameters:
    ///   - positions: 地点配列
    /// - Returns:
    ///   Polygonの中間緯度経度
    static func CenterPoint(positions : [CLLocationCoordinate2D]) -> CLLocationCoordinate2D {
        
        var latMax : Double = 0
        var latMin : Double = 360
        var lonMax : Double = 0
        var lonMin : Double = 360
        for position in positions {
            if latMax < position.latitude{
                latMax = position.latitude
            }
            if latMin > position.latitude{
                latMin = position.latitude
            }
            if lonMax < position.longitude{
                lonMax = position.longitude
            }
            if lonMin > position.longitude{
                lonMin = position.longitude
            }
        }
        let lat = ( latMax + latMin ) / 2
        let lon = ( lonMax + lonMin ) / 2

        return CLLocationCoordinate2D(latitude: lat, longitude: lon)
    }
    
    /// 緯度を数値から文字列に変換
    /// - Parameters:
    ///   - lat: 緯度数値
    /// - Returns
    ///  緯度文字列
    static func latDegtoString(lat: Double) -> String {
        let latNS : String
        if (lat >= 0){
            latNS = "N"
        } else {
            latNS = "S"
        }
        
        let latNum : Double
        if (lat >= 0) {
            latNum = lat
        }else{
            latNum = -lat
        }
        
        let latDeg = Int32(latNum)
        let latMin = (latNum - Double(latDeg)) * 60.0

        return "\(latDeg)°\(String(format:"%06.3f", latMin))′\(latNS)"
    }

    /// 経度を数値から文字列に変換
    /// - Parameters:
    ///   - lon: 経度数値
    /// - Returns
    ///  経度文字列
    static func lonDegtoString(lon: Double) -> String {
        let lonEW : String
        if (lon >= 0){
            lonEW = "E"
        }else{
            lonEW = "W"
        }
    
        let lonNum : Double
        if (lon >= 0){
            lonNum = lon
        } else {
            lonNum = -lon
        }
        
        let lonDeg = Int32(lonNum)
        let lonMin = (lonNum - Double(lonDeg)) * 60.0

        return "\(lonDeg)°\(String(format:"%06.3f", lonMin))′\(lonEW)"
    }
    
    /**
     * 緯度の文字列を数値に変換
     * NXX°XX.XX
     */
    static func latDegtoDouble(lat: String) -> Double? {
        var number: String
        var direction: Double
        if (!lat.contains("N")) {
            number = lat.replacingOccurrences(of: "S", with: "")
            direction = -1.0
        } else {
            number = lat.replacingOccurrences(of: "N", with: "")
            direction = 1.0
        }

        let min = number.components(separatedBy: ".")
        if min.count < 1{
            return nil
        }
        let deg = min[0].components(separatedBy: "°")
        if deg.count < 2{
            return nil
        }
        if let deg0 = Double(deg[0]),
           let deg1 = Double(deg[1]),
           let min1Str = min[1].components(separatedBy: "′").first,
           let min1 = Double(min1Str)
        {
            let decimal = (deg1 + min1 / pow(10.0, Double(min1Str.count))) / 60.0
            return ((deg0 + decimal) * direction)
        }else{
            return nil
        }
        
    }

    /**
     * 経度の文字列を数値に変換
     * EXXX°XX.XX
     */
    static func lonDegtoDouble(lon: String) -> Double? {
        var number: String
        var direction: Double
        if (!lon.contains("E")) {
            number = lon.replacingOccurrences(of: "W", with: "")
            direction = -1.0
        } else {
            number = lon.replacingOccurrences(of: "E", with: "")
            direction = 1.0
        }

        let min = number.components(separatedBy: ".")
        if min.count < 1{
            return nil
        }
        let deg = min[0].components(separatedBy: "°")
        if deg.count < 2{
            return nil
        }
        if let deg0 = Double(deg[0]),
           let deg1 = Double(deg[1]),
           let min1Str = min[1].components(separatedBy: "′").first,
           let min1 = Double(min1Str)
        {
            let decimal = (deg1 + min1 / pow(10.0, Double(min1Str.count))) / 60.0
            return ((deg0 + decimal) * direction)
        }else{
            return nil
        }
    }
    
    ///時計回りか
    ///return: 1 -> 時計回り
    ///     -1 -> 反時計回り
    ///     null -> 直線or点
    static func clockWise(obj: [CLLocationCoordinate2D]) -> Int?{
        if obj.count < 3{
            //3点未満であれば直線or点
            return nil
        }else{
            var s = 0.0
            for i in 0...(obj.count - 2){
                s += ((obj[i].longitude * obj[i + 1].latitude) - (obj[i + 1].longitude * obj[i].latitude))
            }
            if s > 0 {
                return -1
            }else if s < 0{
                return 1
            }else{
                return nil
            }
        }
    }
    
    /// 地点Bから地点Aをみたときの方位角文字列
    /// - Parameters:
    ///   - posAlat: 地点A緯度
    ///   - posAlon: 地点A経度
    ///   - posBlat: 地点B緯度
    ///   - posBlon: 地点B経度
    /// - Returns:
    ///   単位：度
    static func directionStr(posAlat:Double, posAlon:Double, posBlat:Double, posBlon:Double) -> String {
        let direction = direction(posAlat: posAlat, posAlon: posAlon, posBlat: posBlat, posBlon: posBlon)
        return String(round(direction)) + "°"

    }
    
    ///距離（NM）を文字列に変換
    static func distance2Str(nm: Double) -> String{
        var rtnStr = ""
        if nm < 0.1{
            rtnStr = String(format:"%.2f", nm) + "NM"
        }else if nm < 1000{
            rtnStr = String(format:"%.1f", nm) + "NM"
        }else{
            rtnStr = String(Int(nm)) + "NM"
        }
        return rtnStr
    }
    
    static let rad45 = 0.785398163397448   // radで45°
    static let rDeg = 57.295779513082323   // radからdegに変換する乗数
    static let eEccentricity = 0.08181919  // 離心率
    static let brg = 0.001                 // 方位精度
    static let deg2NM = 60.0
    static let pidDiv2 = Double.pi / 2.0
    static let errorRange = 0.00001        //誤差範囲

    struct AreaLatLon {
        var NorthLat: Double = 0.0
        var SouthLat: Double = 0.0
        var EastLon: Double = 0.0
        var WestLon: Double = 0.0
    }

    /**
     * 包囲チェック
     * 1)直線X=CXとオブジェクトの各辺との交点を求め、交点が検索領域の中心CYより上方にあるもの
     *   下方にあるものをそれぞれカウントし、それぞれが奇数個ならば、検索対象オブジェクトが検索領域を包囲していると判断する
     * 2)始点、終点がともにX=CX上に存在する場合、つまり、X=CXと重なる垂直線の場合はカウントしない
     * 3)終点のみX=CX上にある場合はカウントし、次の直線がX=CXと重ならない場合で、終点がCXを中心にして、
     *   前の始点と同じ方向にある場合はカウントし、逆の方向にある場合はカウントしない
     *
     * MFD /src/Chart/DecisionAlgorithm.cpp CheckBeset移植
     */
    static func checkBeset(
        objPos: Array<CLLocationCoordinate2D>,
        minMax: AreaLatLon,
        exceed180: Bool)
        -> Bool
    {
        var lUpperCnt = 0
        var lLowerCnt = 0
        var lBeforeLR = 0                   //-1:Left 1:Right
        var lpSP = CLLocationCoordinate2D(latitude: 0.0, longitude: 0.0)    //180度回り込み経度換算処理後の始点
        var lpEP = CLLocationCoordinate2D(latitude: 0.0, longitude: 0.0)    //180度回り込み経度換算処理後の終点
        let cx = (minMax.EastLon + minMax.WestLon) / 2.0
        let cy = (minMax.NorthLat + minMax.SouthLat) / 2.0

        let max = objPos.count - 2
        for i in 0...max {
            //180度回り込みチェック用のワークエリアに移す
            lpSP = objPos[i]
            lpEP = objPos[(i + 1)]

            //180度回り込み経度換算処理 (TransLonの中で検査領域minMaxの180度回り込みもチェック)
            if lpSP.longitude > lpEP.longitude {
                if ((lpEP.longitude < 0.0) && (lpSP.longitude > 0.0))
                    && ((lpEP.longitude + 180.0) < lpSP.longitude) {  //Objectの直線が180度を跨っている時
                    //最大、最小を入れ替えて、経度を換算する
                    let check = transLon(max: lpEP, min: lpSP, minMax: minMax, exceed180: true, exceed180Search: exceed180)
                    lpEP = check.first
                    lpSP = check.second
                } else {
                    let check = transLon(max: lpSP, min: lpEP, minMax: minMax, exceed180: false, exceed180Search: exceed180)
                    lpSP = check.first
                    lpEP = check.second
                }
            } else {
                if ((lpSP.longitude < 0.0) && (lpEP.longitude > 0.0))
                    && ((lpSP.longitude + 180.0) < lpEP.longitude) {   //Objectの直線が180度を跨っている時
                    //最大、最小を入れ替えて、経度を換算する
                    let check = transLon(max: lpSP, min: lpEP, minMax: minMax, exceed180: true, exceed180Search: exceed180)
                    lpSP = check.first
                    lpEP = check.second
                } else {
                    let check = transLon(max: lpEP, min: lpSP, minMax: minMax, exceed180: false, exceed180Search: exceed180)
                    lpEP = check.first
                    lpSP = check.second
                }
            }

            var maxX: CLLocationDegrees
            if lpEP.longitude > lpSP.longitude {
                maxX = lpEP.longitude
            } else {
                maxX = lpSP.longitude
            }
            var minX: CLLocationDegrees
            if lpEP.longitude < lpSP.longitude {
                minX = lpEP.longitude
            } else {
                minX = lpSP.longitude
            }
            if (maxX < cx) || (minX > cx) {
                //中心線と交わらない場合
                continue
            }
            if lpEP.longitude == lpSP.longitude {
                //垂直線の場合
                continue
            }

            if lpEP.latitude == lpSP.latitude { //平行線の場合
                if lpSP.longitude == cx {  //始点が中心線上にある場合
                    //終点がその前の線分の始点と同じ方向か？
                    if (lpEP.longitude == maxX && lBeforeLR == 1)
                        || (lpEP.longitude == minX && lBeforeLR == -1) {
                        //同じならばカウントする
                        if lpSP.latitude > cy {
                            lUpperCnt += 1
                        } else {
                            lLowerCnt += 1
                        }
                        lBeforeLR = 0
                    }
                } else if lpEP.longitude == cx {   //終点が中心線上にある場合
                    //カウントする
                    if lpEP.latitude > cy {
                        lUpperCnt += 1
                    } else {
                        lLowerCnt += 1
                    }
                    if lpSP.longitude == maxX {
                        lBeforeLR = 1   //線分の始点が中心より右
                    } else {
                        lBeforeLR = -1  //線分の始点が中心より左
                    }
                } else {    //X=CXと交わる平行線なので、カウントする
                    if (lpSP.latitude > cy) {
                        lUpperCnt += 1
                    } else {
                        lLowerCnt += 1
                    }
                }
                continue
            }

            //直線S-E Y=(Ey-Sy)/Ex-Sx)*X+(Sy*Ex-Sx*Ey)/(Ex-Sx)
            //交点のY座標、Y
            let Y = (lpEP.latitude - lpSP.latitude) / (lpEP.longitude - lpSP.longitude) * cx +
                    (lpSP.latitude * lpEP.longitude - lpSP.longitude * lpEP.latitude) /
                    (lpEP.longitude - lpSP.longitude)
            if lpSP.longitude == cx {   //始点が中心線上にある場合
                //終点がその前の線分の始点と同じ方向か?
                if (lpEP.longitude == maxX && lBeforeLR == 1)
                    || (lpEP.longitude == minX && lBeforeLR == -1)
                {
                    //同じならばカウントする
                    if lpSP.latitude > cy {
                        lUpperCnt += 1
                    } else {
                        lLowerCnt += 1
                    }
                }
                lBeforeLR = 0
            } else if lpEP.longitude == cx {    //終点が中心線上にある場合
                if (lpEP.latitude > cy) {
                    lUpperCnt += 1
                } else {
                    lLowerCnt += 1
                }
                if lpSP.longitude == maxX {
                    lBeforeLR = 1   //線分の始点が中心より右
                } else {
                    lBeforeLR = -1  //線分の始点が中心より左
                }
            } else {
                //X=CXと交わる線分なので、カウントする
                if Y > cy {
                    lUpperCnt += 1
                } else {
                    lLowerCnt += 1
                }
            }
        }

        if lUpperCnt % 2 == 1 && lLowerCnt % 2 == 1{
            return true
        } else {
            return false
        }
    }

    /**
     * 180度跨りを考慮した直線の経度の変換
     * 2点を受け取り、検索領域が180度を跨っているかどうかによって2点の経度を変換する
     */
    static func transLon(
        max: CLLocationCoordinate2D,
        min: CLLocationCoordinate2D,
        minMax: AreaLatLon,
        exceed180: Bool,
        exceed180Search: Bool)
        -> (first: CLLocationCoordinate2D, second: CLLocationCoordinate2D)
    {
        var maxRt = max
        var minRt = min

        //Objectの直線が180度を跨っている時は最小、最大を入れ替え済み
        if exceed180Search {  //検索領域が180度を跨っている時
            if min.longitude < 0.0  { //最小経度が-ならば、最小、最大経度に360度をプラスする
                minRt.longitude = min.longitude + 360.0
                maxRt.longitude = max.longitude + 360.0
            } else if max.longitude < 0.0 {
                maxRt.longitude = max.longitude + 360.0
            }
        } else {    //検索領域は180度を跨っていない
            if exceed180 { //Objectの直線が180度を跨っている時
                if minMax.WestLon > 0.0 { //検索領域の経度が+
                    maxRt.longitude = max.longitude + 360.0
                } else {    //検索領域の経度が-
                    minRt.longitude = min.longitude - 360.0
                }
            }
        }
        return (maxRt, minRt)
    }

    enum Mrdc {
        case E_NV_MRDC_NORMAL
        case E_NV_MRDC_XTE_R
    }

    struct ShipPosInf {
        var retCode: Bool = false   //正常終了:true 異常(位置異常):false
        var co1: Double = 0.0       //前回位置から次回位置への方位[deg]
        var co2: Double = 0.0       //次回位置から前回位置への方位[deg]
        var dist: Double = 0.0      //前回位置から次回位置までの距離[nm]
    }

    struct DistanceInf {
        var retCode: Bool = false   //正常終了:true 異常(位置異常):false
        var dist: Double = 0.0      //前回位置から次回位置までの距離[nm]
    }

    struct BearingInf {
        var retCode: Bool = false   //正常終了:true 異常(位置異常):false
        var co1: Double = 0.0       //前回位置から次回位置への方位[deg]
        var co2: Double = 0.0       //次回位置から前回位置への方位[deg]
    }

    /**
     * 線跨ぎ確認用処理
     */
    static func checkCrossLine(
        lastPos: CLLocationCoordinate2D,    //レグ起点
        nextPos: CLLocationCoordinate2D,    //レグ終点
        shipPos: CLLocationCoordinate2D)    //自船位置
            -> (retCode: Bool, xte: Double) //正常終了:true 異常:false 距離[NM]
    {
        let deg1 = bearing(lastPos: lastPos, nextPos: nextPos)    //レグ起点 → レグ終点
        let deg2 = bearing(lastPos: lastPos, nextPos: shipPos)    //レグ起点 → 自船位置
        let deg3 = bearing(lastPos: nextPos, nextPos: shipPos)    //レグ終点 → 自船位置

        //自船を頂点とした方位が９０度以内
        if (deg1.retCode && deg2.retCode && deg3.retCode) {
            let brg1 = abs(deg1.co1 - deg2.co1)
            let brg2 = abs(deg3.co1 - deg1.co2)
            if (brg1 <= 90.0 && brg2 <= 90.0) {
                let rtn = eNvXteRL(lastPos: lastPos, nextPos: nextPos, shipPos: shipPos)
                if (rtn.retCode) {
                    return (true, rtn.xte)
                }
            }

            let brg3 = abs(deg1.co2 - deg2.co2)
            let brg4 = abs(deg3.co2 - deg1.co1)
            if (brg3 <= 90.0 && brg4 <= 90.0) {
                let rtn = eNvXteRL(lastPos: lastPos, nextPos: nextPos, shipPos: shipPos)
                if (rtn.retCode) {
                    return (true, rtn.xte)
                }
            }
        }
        return (false, 0.0)
    }

    /**
     * 線に対して直交するか確認
     */
    static func checkCross(
        lastPos: CLLocationCoordinate2D,       //レグ起点
        nextPos: CLLocationCoordinate2D,       //レグ終点
        shipPos: CLLocationCoordinate2D)       //自船位置
            -> Bool
    {
        var checkCross: Bool = false

        let deg1 = bearing(lastPos: lastPos, nextPos: nextPos)   //レグ起点 → レグ終点
        let deg2 = bearing(lastPos: lastPos, nextPos: shipPos)   //レグ起点 → 自船位置
        let deg3 = bearing(lastPos: nextPos, nextPos: shipPos)   //レグ終点 → 自船位置

        //自船を頂点とした方位が９０度以内
        if (deg1.retCode && deg2.retCode && deg3.retCode) {
            let brg1 = abs(deg1.co1 - deg2.co1)
            let brg2 = abs(deg3.co1 - deg1.co2)
            if (brg1 <= 90.0 && brg2 <= 90.0) {
                checkCross = true
            }

            let brg3 = abs(deg1.co2 - deg2.co2)
            let brg4 = abs(deg3.co2 - deg1.co1)
            if (brg3 <= 90.0 && brg4 <= 90.0) {
                checkCross = true
            }
        }
        return checkCross
    }

    /**
     * 線迄の距離
     */
    static func checkPolyline(
        objPos: Array<CLLocationCoordinate2D>,
        shipPos: CLLocationCoordinate2D)
            -> Double?
    {
        var distance: Double?
        let rtn = crossLineDistance(objPos: objPos, shipPos: shipPos)

        //直線と直行していない
        if (!rtn.retCode) {
            let rtn1 = pointsDistance(objPos: objPos, shipPos: shipPos)
            if (rtn1.retCode) {
                distance = nil
            } else {
                distance = rtn1.xte * -1
            }
        } else {
            distance = rtn.xte * -1
        }

        if let dist = distance {
            print(debug: "checkPolyline \(dist)")
        }
        return distance
    }

    /**
     * 線迄の距離
     */
    static func crossLineDistance(
        objPos: Array<CLLocationCoordinate2D>,
        shipPos: CLLocationCoordinate2D)
        -> (retCode: Bool, xte: Double)     //正常終了:true 異常:false 距離[NM]
    {
        //Loopしているか確認
        var isLoop = false
        if objPos[0].latitude == objPos[(objPos.count - 1)].latitude
            && objPos[0].longitude == objPos[(objPos.count - 1)].longitude {
            isLoop = true
        }

        var loopCntMax = objPos.count - 1
        if isLoop {
            loopCntMax = objPos.count - 2
        }

        //自船が直線に直行している場合の距離を求める
        var isOutside = false
        var isCross = false
        var distance = 1000000.0
        for cnt in 0...(loopCntMax - 1) {
            let rtn = eNvXteRL(lastPos: objPos[cnt], nextPos: objPos[(cnt + 1)], shipPos: shipPos)
            if rtn.retCode {
                //自船を頂点とした方位が９０度以内
                if checkCross(lastPos: objPos[cnt], nextPos: objPos[(cnt + 1)], shipPos: shipPos) {
                    if distance > abs(rtn.xte) {
                        isOutside = rtn.xte < 0
                        distance = abs(rtn.xte)
                        isCross = true
                    }
                }

                print(debug: "crossLineDistance \(rtn)/(isOutside)/(distance)")
            }
        }

        if isOutside {
            distance *= -1
        }
        print(debug: "crossLineDistance \(distance)")
        return (isCross, distance)
    }

    /**
     * 点迄の距離
     */
    static func pointsDistance(
        objPos: Array<CLLocationCoordinate2D>,
        shipPos: CLLocationCoordinate2D)
        -> (retCode: Bool, xte: Double)     //正常終了:true 異常:false 距離[NM]
    {
        var distance = 10000000.0
        var nearbyPoint = 0

        //Loopしているか確認
        var isLoop = false
        if objPos[0].latitude == objPos[(objPos.count - 1)].latitude
            && objPos[0].longitude == objPos[(objPos.count - 1)].longitude {
            isLoop = true
        }

        var loopCntMax = objPos.count
        if isLoop {
            loopCntMax = objPos.count - 1
        }

        //一番近いポイントを見つける
        for cnt in 0...(loopCntMax - 1) {
            let newDistance = distance2(lastPos: objPos[cnt], nextPos: shipPos, type: Mrdc.E_NV_MRDC_NORMAL)
            if newDistance.retCode {
                print(debug: "pointsDistance \(newDistance) (cnt)")
                if distance > newDistance.dist {
                    distance = newDistance.dist
                    nearbyPoint = cnt
                }
            }
        }

        //近いポイントの前のポイント
        var beforePoint = 0
        if nearbyPoint == 0 {
            if isLoop {
                beforePoint = loopCntMax - 1
            } else {
                beforePoint = 0
            }
        } else {
            beforePoint = nearbyPoint - 1
        }

        //近いポイントの次のポイント
        var nextPoint = 0
        if nearbyPoint == (loopCntMax - 1) {
            if isLoop {
                nextPoint = 0
            } else {
                nextPoint = nearbyPoint
            }
        } else {
            nextPoint = nearbyPoint + 1
        }

        var outside1 = false
        var distance1 = 0.0
        let rtn1 = eNvXteRL(lastPos: objPos[beforePoint], nextPos: objPos[nearbyPoint], shipPos: shipPos)
        if rtn1.retCode {
            print(debug: "pointsDistance \(rtn1)/(beforePoint)/(nearbyPoint)/(distance)")
            if rtn1.xte < 0 {
                outside1 = true
            }
            distance1 = rtn1.xte
        }

        var outside2 = false
        var distance2 = 0.0
        let rtn2 = eNvXteRL(lastPos: objPos[nearbyPoint], nextPos: objPos[nextPoint], shipPos: shipPos)
        if rtn2.retCode {
            print(debug: "pointsDistance \(rtn2)/(beforePoint)/(nearbyPoint)/(distance)")
            if rtn2.xte < 0 {
                outside2 = true
            }
            distance2 = rtn2.xte
        }

        var outside = false
        if distance1 < distance2 {
            outside = outside1
        } else {
            outside = outside2
        }

        //船が端の点に近く、ラインと直行していない場合はライン判定から外れていることにする
        var lineOut = false
        if nearbyPoint == 0 {
            lineOut = true
        } else if nearbyPoint == (loopCntMax - 1) {
            lineOut = true
        }

        if outside {
            distance *= -1
        }
        print(debug: "pointsDistance \(distance)")
        return (lineOut, distance)
    }

    /**
     * XTE(航路ずれ)計算(RL)
     * 航路データとNextWP番号、自船位置を与えて航路からの距離(XTE)を求める
     *
     * build/src/ENv/ENv.cpp ENvXteRL
     */
    static func eNvXteRL(
        lastPos: CLLocationCoordinate2D,    //レグ起点
        nextPos: CLLocationCoordinate2D,    //レグ終点
        shipPos: CLLocationCoordinate2D)    //自船位置
        -> (retCode: Bool, xte: Double)     //正常終了:true 異常:false 距離[NM]
    {
        let rtn1 = eNvMrdc(lastPos: lastPos, nextPos: nextPos, type: Mrdc.E_NV_MRDC_NORMAL)
        print(debug: "eNvXteRL1 \(rtn1)")
        if (!rtn1.retCode) {
            return (false, 0.0)
        }
        let rRegCo = LocationCalculation.deg2rad(deg: rtn1.co1)
        let rtn2 = eNvMrdc(lastPos: lastPos, nextPos: shipPos, type: Mrdc.E_NV_MRDC_NORMAL)
        print(debug: "eNvXteRL2 \(rtn2)")
        if (!rtn2.retCode) {
            return (false, 0.0)
        }
        let rShipCo = LocationCalculation.deg2rad(deg: rtn2.co1)
        let xte = rtn2.dist * sin(rShipCo - rRegCo)
        return (true, xte)
    }

    /**
     * 等角航法での距離・方位計算
     *
     * build/src/ENv/ENv.cpp ENvMrdc移植
     */
    static func eNvMrdc(
        lastPos: CLLocationCoordinate2D,    //前回位置
        nextPos: CLLocationCoordinate2D,    //次回位置
        type: Mrdc)                         //手法 Normal:E_NV_MRDC_NORMAL forAutoSail:E_NV_MRDC_XTE_R
        -> ShipPosInf
    {
        var bRng1 = 0.0
        var bRng2 = 0.0
        var dist = 0.0
        var ret = ShipPosInf(retCode: false, co1: bRng1, co2: bRng2, dist: dist)

        //前回位置チェック
        if (!eNvCheckPos(checkPos: lastPos)) {
            //位置が地球上にないのでエラー
            ret = ShipPosInf(retCode: false, co1: bRng1, co2: bRng2, dist: dist)
            return ret
        }

        //次回位置チェック
        if (!eNvCheckPos(checkPos: nextPos)) {
            //位置が地球上にないのでエラー
            ret = ShipPosInf(retCode: false, co1: bRng1, co2: bRng2, dist: dist)
            return ret
        }

        //経度座標の符号を合わせる
        var lastLon = lastPos.longitude
        let nextLon = nextPos.longitude
        if (abs(lastLon - nextLon) > 180.0) {
            if (lastLon < 0.0) {
                lastLon += 360.0
            } else {
                lastLon -= 360.0
            }
        }

        //同じ位置かチェック
        //『前回位置』と『次回位置』の差が0.00001'以下なら同じ位置とみなす
        if ((abs(lastPos.latitude - nextPos.latitude) <= errorRange) &&
             (abs(lastLon - nextLon) <= errorRange)) {
            ret = ShipPosInf(retCode: false, co1: 0.0, co2: 180.0, dist: 0.0)
            return ret
        }

        //度単位のデータをラジアンに変換
        let rLat0 = LocationCalculation.deg2rad(deg: lastPos.latitude)
        let rLon0 = LocationCalculation.deg2rad(deg: lastPos.longitude)
        let rLat1 = LocationCalculation.deg2rad(deg: nextPos.latitude)
        let rLon1 = LocationCalculation.deg2rad(deg: nextPos.longitude)

        //変緯・変経を求める
        let aTanA = eNvConvRad(longiture: rLon1 - rLon0)    //変経計算
        var work2 = eNvMrLat(latitude: nextPos.latitude)    //漸長緯度に変換
        var work1 = eNvMrLat(latitude: lastPos.latitude)    //漸長緯度に変換
        let aTanB = LocationCalculation.deg2rad(deg: work2 - work1) //変緯計算
        //前回位置から次回位置への方位を求める
        if ((aTanA != 0.0) || (aTanB != 0.0)) {
            work1 = atan2(aTanA, aTanB)
            bRng1 = work1
        }

        var bCalc = false
        switch type {
            case Mrdc.E_NV_MRDC_XTE_R:
            if (abs(rLat1 - rLat0) > 0.0000001) {
                bCalc = true
            }
            default:
            if (abs((abs(bRng1) - pidDiv2)) >= (Double.pi / 180.0)) {
                bCalc = true
            }
        }

        //2点間の距離を求める
        var approximation = false
        if (bCalc) {
            //漸長緯度航法で距離を求める
            work2 = cos(work1)
            dist = (rLat1 - rLat0) / work2
        } else {
            //針路が90度または270度の場合
            //中分緯度航法で距離を求める
            dist = sqrt((rLat1 - rLat0) * (rLat1 - rLat0) + cos(rLat0) * cos(rLat0) *
                        eNvConvRad(longiture: rLon1 - rLon0) * eNvConvRad(longiture: rLon1 - rLon0))
            approximation = true
            //手法によってずれ量を変える
            var dist01: Double
            switch type {
                case Mrdc.E_NV_MRDC_XTE_R:
                dist01 = 0.00000011
                default:
                //0.11度ずれた緯度の質を求める
                dist01 = (tan(LocationCalculation.deg2rad(deg: 0.11)) * abs(LocationCalculation.rad2deg(rad: dist) * deg2NM)) / 60.0
            }
            let nextPosTmp = CLLocationCoordinate2D(latitude: (nextPos.latitude + dist01), longitude: nextPos.longitude)
            let shipInf = eNvMrdc(lastPos: lastPos, nextPos: nextPosTmp, type: type)
            if (!shipInf.retCode) {
                ret = ShipPosInf(retCode: false, co1: 0.0, co2: 180.0, dist: 0.0)
                return ret
            }
            dist = shipInf.dist
        }
        bRng1 = eNvConvDeg(dDeg: LocationCalculation.rad2deg(rad: bRng1))
        bRng2 = eNvConvDeg(dDeg: bRng1 + 180.0)
        if (!approximation) {
            dist = LocationCalculation.rad2deg(rad: dist) * deg2NM
        }
        dist = abs(dist)
        return ShipPosInf(retCode: true, co1: bRng1, co2: bRng2, dist: dist)
    }

    /**
     * 等角航法での距離
     *
     * build/src/ENv/ENv.cpp ENvMrdc移植
     */
    static func distance2(
        lastPos: CLLocationCoordinate2D,    //前回位置
        nextPos: CLLocationCoordinate2D,    //次回位置
        type: Mrdc)                         //手法 Normal:E_NV_MRDC_NORMAL forAutoSail:E_NV_MRDC_XTE_R
            -> DistanceInf
    {
        var bRng1 = 0.0
        var dist = 0.0
        var ret = DistanceInf(retCode: false, dist: dist)

        //前回位置チェック
        if (!eNvCheckPos(checkPos: lastPos)) {
            //位置が地球上にないのでエラー
            ret = DistanceInf(retCode: false, dist: dist)
            return ret
        }

        //次回位置チェック
        if (!eNvCheckPos(checkPos: nextPos)) {
            //位置が地球上にないのでエラー
            ret = DistanceInf(retCode: false, dist: dist)
            return ret
        }

        //経度座標の符号を合わせる
        var lastLon = lastPos.longitude
        let nextLon = nextPos.longitude
        if (abs(lastLon - nextLon) > 180.0) {
            if (lastLon < 0.0) {
                lastLon += 360
            } else {
                lastLon -= 360
            }
        }

        //同じ位置かチェック
        //『前回位置』と『次回位置』の差が0.00001'以下なら同じ位置とみなす
        if ((abs(lastPos.latitude - nextPos.latitude) <= errorRange) &&
             (abs(lastLon - nextLon) <= errorRange)) {
            ret = DistanceInf(retCode: false,  dist: dist)
            return ret
        }

        //度単位のデータをラジアンに変換
        let rLat0 = LocationCalculation.deg2rad(deg: lastPos.latitude)
        let rLon0 = LocationCalculation.deg2rad(deg: lastPos.longitude)
        let rLat1 = LocationCalculation.deg2rad(deg: nextPos.latitude)
        let rLon1 = LocationCalculation.deg2rad(deg: nextPos.longitude)

        //変緯・変経を求める
        let aTanA = eNvConvRad(longiture: rLon1 - rLon0)    //変経計算
        var work2 = eNvMrLat(latitude: nextPos.latitude)    //漸長緯度に変換
        var work1 = eNvMrLat(latitude: lastPos.latitude)    //漸長緯度に変換
        let aTanB = LocationCalculation.deg2rad(deg: work2 - work1) //変緯計算
        //前回位置から次回位置への方位を求める
        if ((aTanA != 0.0) || (aTanB != 0.0)) {
            work1 = atan2(aTanA, aTanB)
            bRng1 = work1
        }

        var bCalc = false
        switch type {
            case Mrdc.E_NV_MRDC_XTE_R:
            if (abs(rLat1 - rLat0) > 0.0000001) {
                bCalc = true
            }
            default:
            if (abs((abs(bRng1) - pidDiv2)) >= (Double.pi / 180.0)) {
                bCalc = true
            }
        }

        //2点間の距離を求める
        var approximation = false
        if (bCalc) {
            //漸長緯度航法で距離を求める
            work2 = cos(work1)
            dist = (rLat1 - rLat0) / work2
        } else {
            //針路が90度または270度の場合
            //中分緯度航法で距離を求める
            dist = sqrt((rLat1 - rLat0) * (rLat1 - rLat0) + cos(rLat0) * cos(rLat0) *
                        eNvConvRad(longiture: rLon1 - rLon0) * eNvConvRad(longiture: rLon1 - rLon0))
            approximation = true
            //手法によってずれ量を変える
            var dist01: Double
            switch type {
                case Mrdc.E_NV_MRDC_XTE_R:
                dist01 = 0.00000011
                default:
                //0.11度ずれた緯度の質を求める
                dist01 = (tan(LocationCalculation.deg2rad(deg: 0.11)) * abs(LocationCalculation.rad2deg(rad: dist) * deg2NM)) / 60.0
            }
            let nextPosTmp = CLLocationCoordinate2D(latitude: (nextPos.latitude + dist01), longitude: nextPos.longitude)
            let shipInf = eNvMrdc(lastPos: lastPos, nextPos: nextPosTmp, type: type)
            if (!shipInf.retCode) {
                ret = DistanceInf(retCode: false, dist: 0.0)
                return ret
            }
            dist = shipInf.dist
        }
        if (!approximation) {
            dist = LocationCalculation.rad2deg(rad: dist) * deg2NM
        }
        dist = abs(dist)
        print( debug: "dist:\(dist)" )
        return DistanceInf(retCode: true, dist: dist)
    }

    /**
     * 方位計算
     *
     * build/src/ENv/ENv.cpp ENvMrdc移植
     */
    static func bearing(
        lastPos: CLLocationCoordinate2D,       //前回位置
        nextPos: CLLocationCoordinate2D)       //次回位置
            -> BearingInf
    {
        var bRng1 = 0.0
        var bRng2 = 0.0
        var ret = BearingInf(retCode: false, co1: bRng1, co2: bRng2)

        //前回位置チェック
        if (!eNvCheckPos(checkPos: lastPos)) {
            //位置が地球上にないのでエラー
            let ret = BearingInf(retCode: false, co1: 0.0, co2: 0.0)
            return ret
        }
        //次回位置チェック
        if (!eNvCheckPos(checkPos: nextPos)) {
            //位置が地球上にないのでエラー
            let ret = BearingInf(retCode: false, co1: 0.0, co2: 0.0)
            return ret
        }

        //経度座標の符号を合わせる
        var lastLon = lastPos.longitude
        let nextLon = nextPos.longitude
        if (abs(lastLon - nextLon) > 180.0) {
            if (lastLon < 0.0) {
                lastLon += 360
            } else {
                lastLon -= 360
            }
        }

        //同じ位置かチェック
        //『前回位置』と『次回位置』の差が0.00001'以下なら同じ位置とみなす
        if ((abs(lastPos.latitude - nextPos.latitude) <= errorRange) &&
             (abs(lastLon - nextLon) <= errorRange)) {
            ret = BearingInf(retCode: false, co1: 0.0, co2: 180.0)
            return ret
        }

        //度単位のデータをラジアンに変換
        let rLon0 = LocationCalculation.deg2rad(deg: lastPos.longitude)
        let rLon1 = LocationCalculation.deg2rad(deg: nextPos.longitude)

        //変緯・変経を求める
        let aTanA = eNvConvRad(longiture: rLon1 - rLon0)    //変経計算
        let work2 = eNvMrLat(latitude: nextPos.latitude)    //漸長緯度に変換
        let work1 = eNvMrLat(latitude: lastPos.latitude)    //漸長緯度に変換
        let aTanB = deg2rad(deg: work2 - work1) //変緯計算

        //前回位置から次回位置への方位を求める
        if ((aTanA != 0.0) || (aTanB != 0.0)) {
            bRng1 = atan2(aTanA, aTanB)
        } else {
            bRng1 = 0.0
        }

        bRng1 = eNvConvDeg(dDeg: LocationCalculation.rad2deg(rad: bRng1))
        bRng2 = eNvConvDeg(dDeg: bRng1 + 180.0)

        return BearingInf(retCode: true, co1: bRng1, co2: bRng2)
    }

    /**
     * 位置データチェック
     */
    static func eNvCheckPos(
        checkPos: CLLocationCoordinate2D) -> Bool
    {
        var check = false
        if (checkPos.latitude <= 90.0 &&
            checkPos.latitude >= -90.0 &&
            checkPos.longitude <= 180.0 &&
            checkPos.longitude >= -180.0) {
            check = true
        }
        return check
    }

    /**
     * ラジアンデータ変換
     * 引数が2πより大きいか-2πより小さいとき、2π〜-2πになるように丸める
     */
    static func eNvConvRad(longiture: Double) -> Double {
        var lon: Double
        if (longiture <= -Double.pi) {
            lon = longiture + Double.pi * 2.0
        } else if (longiture > Double.pi) {
            lon = longiture - Double.pi * 2.0
        } else {
            lon = longiture
        }
        return lon
    }

    /**
     * 漸長緯度計算
     * 指定緯度の漸長緯度を求める
     */
    static func eNvMrLat(latitude: Double) -> Double {   // ENvMrLat
        let rLat = abs(latitude * Double.pi / 180)
        let work1 = log(tan(rad45 + rLat / 2.0))
        let work2 = pow(eEccentricity, 2.0) * sin(rLat)
        let work3 = pow(eEccentricity, 4.0) * pow(sin(rLat), 3.0) / 3.0
        let mrLat = work1 - (work2 + work3)

        var lat: Double
        if (latitude < 0.0) {
            lat = mrLat * -1 * rDeg
        } else {
            lat = abs(mrLat) * rDeg
        }
        return lat
    }

    /**
     * 度データ変換
     * ±の度データを+0〜360度のデータに変換する
     *
     * build/src/ENv/ENv.cpp ENvConvDeg
     */
    static func eNvConvDeg(
        dDeg: Double
    ) -> Double {
        var deg = dDeg

        if (deg >= 360.0) {
            //360度以上なら360度を減算
            deg -= 360.0
        }

        if (deg < 0.0) {
            //0度より小さいなら360度を加算
            deg += 360.0
        }

        if (deg == 360.0) {
            deg = 0.0
        }

        return deg
    }

    /**
     * 等角航法での推測位置計算
     * 等角航法で始点座標とその方位から任意の距離における位置座標を求める
     *
     * build/src/ENv/ENv.cpp ENvMrnp移植
     */
     static func eNvMrnp(
        startPos: CLLocationCoordinate2D,
        course: Double,
        distance: Double
    ) -> CLLocationCoordinate2D { // ENvMrnp
        var drPos = CLLocationCoordinate2D(latitude: 0.0, longitude: 0.0)
        if (!LocationCalculation.checkPosition(pos: startPos)) {
            return drPos
        }

        let rangeCourse = LocationCalculation.rangeDegree(degree: course)
        drPos.latitude = startPos.latitude + distance * cos(rangeCourse * Double.pi / 180.0) / 60.0
        if ((abs(rangeCourse - 90.0) >= (0.1 + brg)) && (abs(rangeCourse - 270.0) >= (0.1 + brg))) {
            drPos.longitude = startPos.longitude + (eNvMrLat(latitude: drPos.latitude) - eNvMrLat(latitude: startPos.latitude)) * tan(rangeCourse * Double.pi / 180.0)
        } else {
            let courseEx: Double
            if (abs(rangeCourse - 270.0) >= (0.1 + brg)) {
                courseEx = 90.11
            } else {
                courseEx = 270.11
            }

            let drPosEx = eNvMrnp(startPos: startPos, course: courseEx, distance: distance)
            drPos.longitude = drPosEx.longitude
        }

        if (180.0 < drPos.longitude) {
            drPos.longitude -= 360.0
        }
        if (drPos.longitude < -180.0) {
            drPos.longitude += 360.0
        }

        return drPos
    }
}
