; ----------------------------------------------------------------------------------------
; X = unit index
calculateRoute:
{
            lda SrcPosX
            cmp DestPosX
            bne notAtTarget

            lda SrcPosY
            cmp DestPosY
            bne notAtTarget

            lda #$00
            sta OBJ_ATTR(Unit, TargetMoveLo),x
            sta OBJ_ATTR(Unit, TargetMoveHi),x

            jmp addEndMarkerToRoute

notAtTarget:

            lda OBJ_ATTR(Unit, RouteIndex),x
            cmp #(ROUTE_BUFFER_SIZE-1)
            bne routeBufferHasRoom

            jmp addEndMarkerToRoute

routeBufferHasRoom:

            lda RouteBufferForUnitLo,x
            sta RouteBuffer
            lda RouteBufferForUnitHi,x
            sta RouteBuffer+1

            ; get general direction
            jsr getDirectionFromSrcToDst
            sta Direction

            ; offset position into direction of target
tryMoveIntoDirection:
            lsr
            lsr
            lsr
            lsr
            lsr
            tay
            jsr moveIntoDirection

            jsr canMoveToPosition
            bne isValidMove

            ; restore packed position to current route position
            lda SrcPosX
            sta SaveSrcPosX
            ldy SrcPosY
            sty SaveSrcPosY
            jsr packPosition

            ; try next direction clockwise
            lda #<RouteBuffer1
            sta RouteBufferPtrLo
            lda #>RouteBuffer1
            sta RouteBufferPtrHi

            lda Direction
            ldy #$20
            jsr tryAlternativeRoute
            sta FirstRouteLength

            ; restore position before alternative route
SaveSrcPosX = *+1
            lda #$00
            sta SrcPosX
SaveSrcPosY = *+1
            ldy #$00
            sty SrcPosY
            jsr packPosition

            ; try next direction counterclockwise
            lda #<RouteBuffer2
            sta RouteBufferPtrLo
            lda #>RouteBuffer2
            sta RouteBufferPtrHi

            lda Direction
            ldy #$e0
            jsr tryAlternativeRoute

            ; A = second route length

FirstRouteLength = *+1
            cmp #$00
            bcc secondRouteShorter

            lda #<RouteBuffer1
            sta RouteBufferPtrLo
            lda #>RouteBuffer1
            sta RouteBufferPtrHi
            lda FirstRouteLength

secondRouteShorter:

            sta RouteCopySize
            beq noRouteToCopy

            lda RouteBufferForUnitLo,x
            clc
            adc OBJ_ATTR(Unit, RouteIndex),x
            sta RouteBufferDest
            lda RouteBufferForUnitHi,x
            adc #$00
            sta RouteBufferDest+1

            ldy #$00
            {
                lda (RouteBufferPtrLo),y
.global RouteBufferDest = *+1
                sta $0123,y
                inc OBJ_ATTR(Unit, RouteIndex),x
                lda OBJ_ATTR(Unit, RouteIndex),x
                cmp #(ROUTE_BUFFER_SIZE-1)
                beq _break

                iny
.global RouteCopySize = *+1
                cpy #$00
                bne _cont
            }
noRouteToCopy:
            jmp addEndMarkerToRoute

isValidMove:

            ; store direction from src to dest in route buffer
            lda OBJ_ATTR(Unit, RouteIndex),x
            tay

Direction = *+1
            lda #$00
RouteBuffer = *+1
            sta $0123,y

            inc OBJ_ATTR(Unit, RouteIndex),x

            ; store packed position in SrcPos
            jsr unpackPosition
            sta SrcPosX
            sty SrcPosY
            jmp calculateRoute

}


; ----------------------------------------------------------------------------------------
; input: X = unit index, A = direction, Y = searchDirection ($20 = clockwise, $e0 = counterclockwise)
tryAlternativeRoute:
{
            sty SearchDirection
            clc
SearchDirection = *+1
            adc #$00
            sta Direction

            ldy #$00
            sty RouteIndex

tryMoveIntoDirection:
            lsr
            lsr
            lsr
            lsr
            lsr
            tay
            jsr moveIntoDirection

            jsr canMoveToPosition
            bne isValidMove

            ; restore packed position to current route position
            lda SrcPosX
            ldy SrcPosY
            jsr packPosition

            ; try next direction
            lda Direction
            clc
            adc SearchDirection
            sta Direction
            jmp tryMoveIntoDirection

isValidMove:

Direction = *+1
            lda #$00

RouteIndex = *+1
            ldy #$00
            sta (RouteBufferPtrLo),y
            inc RouteIndex

            ; store packed position in SrcPos
            jsr unpackPosition
            sta SrcPosX
            sty SrcPosY

            lda SrcPosX
            cmp DestPosX
            bne notAtTarget

            lda SrcPosY
            cmp DestPosY
            bne notAtTarget

            lda RouteIndex
            rts

notAtTarget:

            lda RouteIndex
            cmp #ROUTEBUFFER_SIZE
            bne routeBufferHasRoom

            rts

routeBufferHasRoom:
            jsr getDirectionFromSrcToDst
            sta Direction

            ; offset position into direction of target
            jmp tryMoveIntoDirection
}


; ----------------------------------------------------------------------------------------
; input: X = unit index
deleteRoute:
{
            lda #$00
            sta OBJ_ATTR(Unit, RouteIndex),x

            lda RouteBufferForUnitLo,x
            sta RouteBuffer
            lda RouteBufferForUnitHi,x
            sta RouteBuffer+1

            lda #$ff
RouteBuffer = *+1
            sta $0123
            rts
}


; ----------------------------------------------------------------------------------------
; input: X = unit index
; modifies: Y
addEndMarkerToRoute:
{
            lda OBJ_ATTR(Unit, RouteIndex),x
            tay

            lda RouteBufferForUnitLo,x
            sta RouteBuffer
            lda RouteBufferForUnitHi,x
            sta RouteBuffer+1

            lda #$ff
RouteBuffer = *+1
            sta $0123,y

            lda #$00
            sta OBJ_ATTR(Unit, RouteIndex),x
            rts
}


; ----------------------------------------------------------------------------------------
; input: PackedPositionLo/Hi, Y = direction
; output: PackedPositionLo/Hi
moveIntoDirection:
{
            lda DirectionToMapOffsetSign,y
            bmi subtract
            lda PackedPositionLo
            clc
            adc DirectionToMapOffset,y
            sta PackedPositionLo
            lda PackedPositionHi
            adc #$00
            sta PackedPositionHi
            rts
subtract:
            lda PackedPositionLo
            sec
            sbc DirectionToMapOffset,y
            sta PackedPositionLo
            lda PackedPositionHi
            sbc #$00
            sta PackedPositionHi
            rts

DirectionToMapOffset:
            .byte 64        ; N
            .byte 63        ; NE
            .byte 1         ; E
            .byte 65        ; SE
            .byte 64        ; S
            .byte 63        ; SW
            .byte 1         ; W
            .byte 65        ; NW

DirectionToMapOffsetSign:
            .byte $80       ; N
            .byte $80       ; NE
            .byte $00       ; E
            .byte $00       ; SE
            .byte $00       ; S
            .byte $00       ; SW
            .byte $80       ; W
            .byte $80       ; NW
}


; ----------------------------------------------------------------------------------------
; A = tile id
canMoveIntoTile:
{
            tay

            lda TileIdToStructureId,y
            cmp #STRUCTURE_REFINERY
            bne notRefinery
            lda OBJ_ATTR(Unit, Type),x
            cmp #UNIT_HARVESTER
            bne notHarvester
            lda #$01
            rts
notRefinery:
notHarvester:
            lda TileIdToMovementInfo,y
            and #MOVEMENT_INFO_CAN_MOVE
            rts
}


; ----------------------------------------------------------------------------------------
; input: PackedPosition
; output: A
canMoveToPosition:
{
            lda PackedPositionLo
            sta TileMapPtrLo
            sta TileInfoMapPtrLo
            lda PackedPositionHi
            clc
            adc #>TileMap
            sta TileMapPtrHi
            lda PackedPositionHi
            clc
            adc #>TileInfoMap
            sta TileInfoMapPtrHi

            ldy #$00
            lda (TileMapPtrLo),y
            jsr canMoveIntoTile
            beq isInvalidMove

            ldy #$00
            lda (TileInfoMapPtrLo),y
            and #TILE_INFO_IS_STRUCTURE
            bne isValidMove

            lda (TileInfoMapPtrLo),y
            and #TILE_INFO_INDEX_MASK
            cmp #TILE_INFO_NO_INDEX
            beq isValidMove

isInvalidMove:
            lda #$00
            rts

isValidMove:
            lda #$01
            rts
}