#if os(macOS)
import Darwin
#elseif os(Linux)
import Glibc
#elseif os(Windows)
import ucrt
#endif


@main
struct A_Star {
    static func main() {
        print("\u{1B}[2J", terminator: "")
        print("\u{1B}[H", terminator: "")
        // From https://patorjk.com/software/taag
        print(#"""
 $$$$$$\                  $$$$$$\                            $$\ $$\                           
$$  __$$\  $$\$$\        $$  __$$\                           $$ |$$ |                          
$$ /  $$ | \$$$  |       $$ /  \__| $$$$$$\  $$$$$$$\   $$$$$$$ |$$$$$$$\   $$$$$$\  $$\   $$\ 
$$$$$$$$ |$$$$$$$\       \$$$$$$\   \____$$\ $$  __$$\ $$  __$$ |$$  __$$\ $$  __$$\ \$$\ $$  |
$$  __$$ |\_$$$ __|       \____$$\  $$$$$$$ |$$ |  $$ |$$ /  $$ |$$ |  $$ |$$ /  $$ | \$$$$  / 
$$ |  $$ | $$ $$\        $$\   $$ |$$  __$$ |$$ |  $$ |$$ |  $$ |$$ |  $$ |$$ |  $$ | $$  $$<  
$$ |  $$ | \__\__|       \$$$$$$  |\$$$$$$$ |$$ |  $$ |\$$$$$$$ |$$$$$$$  |\$$$$$$  |$$  /\$$\ 
\__|  \__|                \______/  \_______|\__|  \__| \_______|\_______/  \______/ \__/  \__|

Welcome to the A* Sandbox, an interactive terminal-based visualizer for the A* graph traversal
algorithm. You can enter a width and height for the grid, then use the following controls to
experiment with the algorithm. The optimal path will refresh every time you make a change to
the grid.

Controls:
- Cursor movement: WASD, HJKL, or Arrow Keys
- Wall placement: Space
- Origin movement: [
- Target movement: ]

"""#)
        print("Enter the grid width: ", terminator: "")
        let width: Int = Int(readLine() ?? "") ?? 16
        print("Enter the grid height: ", terminator: "")
        let height: Int = Int(readLine() ?? "") ?? 8
        print("\u{1B}[?25l")

        Terminal.enableRawMode()

        var cursorPosition = Vector2(x: 0, y: 0)

        defer {
            quit()
        }

        var grid: [[Int]] = Array(repeating: Array(repeating: 0, count: width), count: height)
        var start: Vector2 = Vector2(x: 0, y: 0)
        var goal: Vector2 = Vector2(x: width - 1, y: 0)

        var aStar = A_Star()

        var path = aStar.pathfind(grid: grid, start: start, goal: goal)

        while true {
            aStar.display(path, on: grid, cursorPosition: cursorPosition)
            let key = Terminal.readKey()
            switch key {
            case .Left:
                cursorPosition += Vector2(x: -1, y: 0)
            case .Right:
                cursorPosition += Vector2(x: 1, y: 0)
            case .Up:
                cursorPosition += Vector2(x: 0, y: -1)
            case .Down:
                cursorPosition += Vector2(x: 0, y: 1)
            default:
                switch key {
                case .Start:
                    start = cursorPosition
                case .Finish:
                    goal = cursorPosition
                case .Wall:
                    if cursorPosition != goal && cursorPosition != start {
                        grid[cursorPosition.y][cursorPosition.x] = grid[cursorPosition.y][cursorPosition.x] == 1 ? 0 : 1
                    }
                case .NW:
                    aStar.directions[0] = !aStar.directions[0]
                case .N:
                    aStar.directions[1] = !aStar.directions[1]
                case .NE:
                    aStar.directions[2] = !aStar.directions[2]
                case .W:
                    aStar.directions[3] = !aStar.directions[3]
                case .E:
                    aStar.directions[4] = !aStar.directions[4]
                case .SW:
                    aStar.directions[5] = !aStar.directions[5]
                case .S:
                    aStar.directions[6] = !aStar.directions[6]
                case .SE:
                    aStar.directions[7] = !aStar.directions[7]
                default:
                    break
                }

                path = aStar.pathfind(grid: grid, start: start, goal: goal)
                continue
            }

            if cursorPosition.x < 0 { cursorPosition.x = 0 }
            if cursorPosition.y < 0 { cursorPosition.y = 0 }
            if cursorPosition.x > grid[0].count - 1 { cursorPosition.x = grid[0].count - 1 }
            if cursorPosition.y > grid.count - 1 { cursorPosition.y = grid.count - 1 }
        }
    }

    var directions: [Bool] = Array(repeating: true, count: 8)

    func pathfind(grid: [[Int]], start: Vector2, goal: Vector2) -> [Vector2] {
        var openSet: [Vector2] = [start]

        var cameFrom: Dictionary<Vector2, Vector2> = [:]
        
        var gScore: Dictionary<Vector2, Float> = [:]
        gScore[start] = 0

        var fScore: Dictionary<Vector2, Float> = [:]
        fScore[start] = h(current: start, goal: goal)

        while openSet.count != 0 {
            var current: Vector2 = getLowest(openSet, map: fScore)

            if current == goal {
                var totalPath: [Vector2] = [current]
                while cameFrom.keys.contains(current) {
                    current = cameFrom[current]!
                    totalPath.append(current)
                }
                totalPath.reverse()
                return totalPath
            }

            openSet.remove(at: openSet.firstIndex(of: current)!)

            let neighbors: [Vector2] = [
                Vector2(x: current.x - 1, y: current.y - 1),
                Vector2(x: current.x, y: current.y - 1),
                Vector2(x: current.x + 1, y: current.y - 1),

                Vector2(x: current.x - 1, y: current.y),
                Vector2(x: current.x + 1, y: current.y),

                Vector2(x: current.x - 1, y: current.y + 1),
                Vector2(x: current.x, y: current.y + 1),
                Vector2(x: current.x + 1, y: current.y + 1),
            ]

            for i: Int in 0..<neighbors.count {
                // Enforce direction whitelist
                if !directions[i] { 
                    continue
                }
                let neighbor: Vector2 = neighbors[i]
                if neighbor.x >= grid[0].count ||
                    neighbor.x < 0 ||
                    neighbor.y >= grid.count ||
                    neighbor.y < 0 {
                        continue
                    }

                if grid[neighbor.y][neighbor.x] == 1 {
                    continue
                }

                let tentative_gScore: Float = gScore[current]! + d(current: current, neighbor: neighbor)

                if tentative_gScore < gScore[neighbor, default: Float.infinity] {
                    cameFrom[neighbor] = current
                    gScore[neighbor] = tentative_gScore
                    fScore[neighbor] = tentative_gScore + h(current: neighbor, goal: goal)
                    if !openSet.contains(neighbor) {
                        openSet.append(neighbor)
                    }
                }
            }
        }

        return [start, goal]
    }

    func h(current: Vector2, goal: Vector2) -> Float {
        let x = Float(abs(current.x - goal.x))
        let y = Float(abs(current.y - goal.y))
        return abs(x - y) + 1.4142 * min(x, y)
    }

    func d(current: Vector2, neighbor: Vector2) -> Float {
        if current.x == neighbor.x || current.y == neighbor.y {
            return 1
        }
        return 1.4142
    }

    func getLowest(_ set: [Vector2], map: Dictionary<Vector2, Float>) -> Vector2 {
        var tentativeLowest = set[0]
        for item in set {
            if map[item]! < map[tentativeLowest]! {
                tentativeLowest = item
            }
        }
        return tentativeLowest
    }

    func display(_ path: [Vector2], on: [[Int]], cursorPosition: Vector2) {
        let draw: Dictionary<String, String> = [
            "nw": "┏",
            "ne": "┓",
            "sw": "┗",
            "se": "┛",
            "n": "━",
            "e": "┃",
            "s": "━",
            "w": "┃",
            "start": "S",
            "goal": "F",
            "path": "*",
            "empty": "·",
            "wall": "▓",
            "clear": "\u{1B}[2J",
            "clearLine": "\u{1B}[2K",
            "home": "\u{1B}[H",
            "hide": "\u{1B}[?25l",
            "show": "\u{1B}[?25h",
        ]

        var output: String = ""
        output += draw["nw"] ?? "?"
        output += String(repeating: draw["n"] ?? "?", count: on[0].count)
        output += draw["ne"] ?? "?"
        output += "\n"

        for y: Int in 0..<on.count {
            output += draw["w"] ?? "?"
            for x: Int in 0..<on[0].count {
                output += cursorPosition == Vector2(x: x, y: y) ? "\u{1B}[7m" : "\u{1B}[27m"
                let position: Vector2 = Vector2(x: x, y: y)
                if position == path.first {
                    output += draw["start"] ?? "?"
                    continue
                }
                if position == path.last {
                    output += draw["goal"] ?? "?"
                    continue
                }
                if path.contains(position) {
                    let nodeIndex: Int = path.firstIndex(of: position) ?? 0
                    let positionAfter = path[nodeIndex + 1]
                    let deltaX = positionAfter.x - position.x
                    let deltaY = positionAfter.y - position.y
                    var arrow = ""
                    switch (deltaX, deltaY) {
                    case (-1, -1):
                        arrow = "↖"
                    case (0, -1):
                        arrow = "↑"
                    case (1, -1):
                        arrow = "↗"
                    case (-1, 0):
                        arrow = "←"

                    case (1, 0):
                        arrow = "→"
                    case (-1, 1):
                        arrow = "↙"
                    case (0, 1):
                        arrow = "↓"
                    case (1, 1):
                        arrow = "↘"
                    default:
                        arrow = "?"
                    }
                    output += arrow
                    continue
                }
                output += on[y][x] == 1 ? draw["wall"] ?? "?" : draw["empty"] ?? "?"
            }
            output += "\u{1B}[27m"
            output += draw["e"] ?? "?"
            output += "\n"
        }

        output += draw["sw"] ?? "?"
        output += String(repeating: draw["s"] ?? "?", count: on[0].count)
        output += draw["se"] ?? "?"

        output += "\n\n"

        output += directions[0] ? " ↖" : "  "
        output += directions[1] ? " ↑" : "  "
        output += directions[2] ? " ↗" : "  "
        output += "\n"
        output += directions[3] ? " ←" : "  "
        output += "  "
        output += directions[4] ? " →" : "  "
        output += "\n"
        output += directions[5] ? " ↙" : "  "
        output += directions[6] ? " ↓" : "  "
        output += directions[7] ? " ↘" : "  "

        print(draw["clear"] ?? "?", terminator: "")
        print(draw["home"] ?? "?", terminator: "")
        print(output)
    }

    static func clamp(_ value: Int, low: Int, high: Int) -> Int {
        if value < low {
            return low
        }
        if value > high {
            return high
        }
        return value
    }
}

enum Key {
    case Up
    case Down
    case Left
    case Right
    case Wall
    case Start
    case Finish

    case NW
    case N
    case NE
    case E
    case SE
    case S
    case SW
    case W

    case Unknown
}

struct Terminal {
    nonisolated(unsafe) static var originalTerm = termios()

    static func readKey() -> Key {
        var byte: UInt8 = 0
#if os(Windows)
        let ch = _getch()
        if ch == 224 || ch == 0 {
            let nextCh = _getch()
            switch nextch {
            case 72: return .Up
            case 80: return .Dow
            case 75: return .Left
            case 77: return .Right
            default: return .Unknown
            }
        }
        byte = UInt8(ch)
#else
        read(STDIN_FILENO, &byte, 1)

        if byte == 27 {
            var seq1: UInt8 = 0
            var seq2: UInt8 = 0

            read(STDIN_FILENO, &seq1, 1)
            read(STDIN_FILENO, &seq2, 1)

            if seq1 == 91 {
                switch seq2 {
                case 65: return .Up
                case 66: return .Down
                case 67: return .Right
                case 68: return .Left
                default: return .Unknown
                }
            }
        }
#endif
        let character = String(UnicodeScalar(byte))
        switch character {
        case "w":
            return .Up
        case "k":
            return .Up
        case "a":
            return .Left
        case "h":
            return .Left
        case "s":
            return .Down
        case "j":
            return .Down
        case "d":
            return .Right
        case "l":
            return .Right
        case " ":
            return .Wall
        case "[":
            return .Start
        case "]":
            return .Finish
        case "7":
            return .NW
        case "8":
            return .N
        case "9":
            return .NE
        case "4":
            return .W
        case "6":
            return .E
        case "1":
            return .SW
        case "2":
            return .S
        case "3":
            return .SE
        default:
            return .Unknown
        }
    }

    static func enableRawMode() {
#if os(macOS) || os(Linux)
        tcgetattr(STDIN_FILENO, &originalTerm)

        var rawTerm = originalTerm
        rawTerm.c_lflag &= ~tcflag_t(ICANON | ECHO)
        tcsetattr(STDIN_FILENO, TCSANOW, &rawTerm)

        signal(SIGINT, quit)
#endif
    }

    static func disableRawMode() {
#if os(macOS) || os(Linux)
        tcsetattr(STDIN_FILENO, TCSANOW, &originalTerm)
#endif
    }
}

func quit(_ signal: Int32 = 0) {
    Terminal.disableRawMode()
    // Show cursor
    print("Exit")
    print("\u{1B}[?25h", terminator: "")
    print("\u{1B}[2J", terminator: "")
    print("\u{1B}[H", terminator: "")
    exit(0)
}

struct Vector2: Hashable {
    var x: Int
    var y: Int

    static func == (lhs: Vector2, rhs: Vector2) -> Bool {
        return lhs.x == rhs.x && lhs.y == rhs.y
    }

    static func += (lhs: inout Vector2, rhs: Vector2) {
        lhs = Vector2(x: lhs.x + rhs.x, y: lhs.y + rhs.y)
    }

    func hash(into hasher: inout Hasher) {
        hasher.combine(x)
        hasher.combine(y)
    }
}