;AIMING_X_TO_Y : Set Obj[x]'s velocity toward Obj[y], its speed is V_AIMING_LO,V_AIMING_HI.
;ATAN_X_TO_Y : Get 8 bits wide binary degree angle toward Obj[y] from Obj[x].

;Note: These routines are NOT validated well, though these are used for 4mi...

;角度は、→x ↓yの自然なものにしてあるので注意
AIMING_X_TO_0302_0100:
    jsr ATAN_X_TO_0302_0100
    jmp SET_VELOCITY_POLAR
AIMING_X_TO_Y:
    jsr ATAN_X_TO_Y
SET_VELOCITY_POLAR:

    tya

    ;x速度のセット
    pha ;1
    tay
    jsr .my_abs_cos
    sta A_ObjVxlo,x
    pla ;1
    ;y速度のセット
    pha ;2
    tay
    jsr .my_abs_sin
    sta A_ObjVylo,x
    ;速度上位クリア
;   lda #$00
;   sta A_ObjVxhi,x
;   sta A_ObjVyhi,x
;移動方向フラグをセット
    pla ;2
    asl a
    rol <$00
    asl a
    rol <$00
    lda <$00
    and #$03
    tay
    lda .move_toward_table,y
    sta $0420,x ; Obj移動方向<？？？？上下左右>[%X]

;掛け算※xレジスタ破壊

    ;X方向
    ;00.## * 00.##
    txa
    pha ;[1]

    lda V_AIMING_LO
    sta <$00
    lda A_ObjVxlo,x
    sta <$01
    jsr $FC7B

    pla ;[1]
    tax

    ;00.00**の部分には興味が無い……四捨五入とかしなくていいよね？
    lda <$03
    pha ;[1]

    ;00.## * ##
    txa
    pha ;[2]

    lda V_AIMING_HI
    sta <$00
    lda A_ObjVxlo,x
    sta <$01
    jsr $FC7B

    pla ;[2]
    tax

    ;結果の足し合わせと書き込み
    clc
    pla ;[1]
    adc <$02
    sta A_ObjVxlo,x
    lda <$03
    adc #$00
    sta A_ObjVxhi,x

    ;Y方向
    ;00.## * 00.##
    txa
    pha ;[1]

    lda V_AIMING_LO
    sta <$00
    lda A_ObjVylo,x
    sta <$01
    jsr $FC7B

    pla ;[1]
    tax

    ;00.00**の部分には興味が無い……四捨五入とかしなくていいよね？
    lda <$03
    pha ;[1]

    ;00.## * ##
    txa
    pha ;[2]

    lda V_AIMING_HI
    sta <$00
    lda A_ObjVylo,x
    sta <$01
    jsr $FC7B

    pla ;[2]
    tax

    ;結果の足し合わせと書き込み
    clc
    pla ;[1]
    adc <$02
    sta A_ObjVylo,x
    lda <$03
    adc #$00
    sta A_ObjVyhi,x

    rts

.move_toward_table
    .db $5,$6,$A,$9


.my_abs_sin
    tya
    and #$7F

    cmp #$40
    and #$3F
    bcc .sin_not_inverse
    sta <$00
    sec
    lda #$3F
    sbc <$00
.sin_not_inverse
    tay
    lda UTIL_SIN_TABLE_64_256,y
    rts

.my_abs_cos
    tya
    clc
    adc #$40
    tay
    jmp .my_abs_sin


UTIL_ABS_SIN:
    tya
    and #$7F

    cmp #$40
    and #$3F
    bcc .sin_not_inverse
    sta <$00
    sec
    lda #$3F
    sbc <$00
.sin_not_inverse
    tay
    lda UTIL_SIN_TABLE_64_256,y
    tay

    rts

;y=sin(y)    -7F<=戻値<=7F
UTIL_SIN:
    sty <$01
    jsr UTIL_ABS_SIN
    tya
    lsr a
    asl <$01
    bcc .plus
    INV_A
.plus
    tay
    rts

UTIL_SIN_TABLE_64_256:
    .db $00,$06,$0D,$13,$19,$1F,$26,$2C,$32,$38,$3E,$44,$4A,$50,$56,$5C
    .db $62,$68,$6D,$73,$79,$7E,$84,$89,$8E,$93,$98,$9D,$A2,$A7,$AC,$B1
    .db $B5,$B9,$BE,$C2,$C6,$CA,$CE,$D1,$D5,$D8,$DC,$DF,$E2,$E5,$E7,$EA
    .db $ED,$EF,$F1,$F3,$F5,$F7,$F8,$FA,$FB,$FC,$FD,$FE,$FF,$FF,$FF,$FF ;$100,$100

ATAN_X_TO_Y:
;Δx   $03~$02
;Δy   $01~$00
;$04flag     0 0 0 0   0 yxcoodinate yinv xinv


    lda A_ObjXhi,y
    sta <$02
    lda A_ObjYhi,y
    sta <$00
ATAN_X_TO_Y_Hi:
    lda A_ObjXhe,y
    sta <$03
    lda A_ObjYhe,y
    sta <$01

ATAN_X_TO_0302_0100:
;ココから処理開始


    sec
    lda <$02
    sbc A_ObjXhi,x
    sta <$02
    lda <$03
    sbc A_ObjXhe,x
    sta <$03

    sec
    lda <$00
    sbc A_ObjYhi,x
    sta <$00
    lda <$01
    sbc A_ObjYhe,x
    sta <$01

ATAN_Delta_0302_0100:
    lda #$00
    sta <$04

;X負チェック
    lda <$03
    bpl .delta_x_isnot_minus
    lda <$02
    eor #$FF
    clc
    adc #$01
    sta <$02
    lda <$03
    eor #$FF
    adc #$00
    sta <$03

;   lda <$04 ;^
;   ora #$01 ;|
    lda #$01 ;v
    sta <$04
.delta_x_isnot_minus

;Y負チェック
    lda <$01
    bpl .delta_y_isnot_minus
    lda <$00
    eor #$FF
    clc
    adc #$01
    sta <$00
    lda <$01
    eor #$FF
    adc #$00
    sta <$01
    lda <$04
    ora #$02
    sta <$04
.delta_y_isnot_minus

;ΔｘとΔｙの大小関係をチェック

    sec
    lda <$01 ;Δyhe
    sbc <$03 ;Δxhe
    bcc .delta_x_is_greater
    bne .delta_y_is_greater
;延長戦
    ;sec
    lda <$00 ;Δyhi
    sbc <$02 ;Δxhi
    bcc .delta_x_is_greater
    beq .delta_x_is_greater ;きわどいところだが同じならｘが大きいことに
.delta_y_is_greater
    lda <$02 ;Δxhi
    pha
    lda <$00 ;Δyhi
    sta <$02 ;Δxhi
    pla
    sta <$00 ;Δyhi

    lda <$03 ;Δxhe
    pha
    lda <$01 ;Δyhe
    sta <$03 ;Δxhe
    pla
    sta <$01 ;Δyhe

    lda <$04
    ora #$04
    sta <$04

.delta_x_is_greater

;以上の処理により、フラグにより扱いを変えるほかは、
;Δｘ　＞＝　Δｙ
;となった

;フラグだけは退避しておく
    lda <$04
    pha

;除算処理に送り込む
;実はこのΔｘ、Δｙの取りかたで除算処理にちょうど送り込める
    jsr $FC3A
;$05~$04に帰ってくる

;   ;テスト（２以上にはならないはず……）
;.lock
;   lda <$05
;   cmp #$02
;   bcs .lock

    ldy #$20
    lda <$05
    bne .dx_equal_to_dy
    lda <$04
    lsr a
    tay
    lda .atan_table128,y
    tay
.dx_equal_to_dy
    sty <$00

;フラグを取り出した
    pla
    sta <$01
    ldy <$01
    lda .is_reverse,y
    beq .not_reverse_angle
    clc
    lda <$00
    eor #$FF
    adc #$01
    sta <$00
.not_reverse_angle
    ldy <$01
    lda .atan_add_angle,y
    clc
    adc <$00

    tay
    rts

.is_reverse
    .db 0,1,1,0
    .db 1,0,0,1

.atan_add_angle
    .db $00,$80,$00,$80
    .db $40,$40,$C0,$C0


.atan_table128
    .db $00,$00,$01,$01,$01,$02,$02,$02,$03,$03,$03,$03,$04,$04,$04,$05
    .db $05,$05,$06,$06,$06,$07,$07,$07,$08,$08,$08,$08,$09,$09,$09,$0A
    .db $0A,$0A,$0B,$0B,$0B,$0B,$0C,$0C,$0C,$0D,$0D,$0D,$0D,$0E,$0E,$0E
    .db $0F,$0F,$0F,$0F,$10,$10,$10,$11,$11,$11,$11,$12,$12,$12,$12,$13
    .db $13,$13,$13,$14,$14,$14,$14,$15,$15,$15,$15,$16,$16,$16,$16,$17
    .db $17,$17,$17,$17,$18,$18,$18,$18,$19,$19,$19,$19,$19,$1A,$1A,$1A
    .db $1A,$1A,$1B,$1B,$1B,$1B,$1B,$1C,$1C,$1C,$1C,$1C,$1D,$1D,$1D,$1D
    .db $1D,$1D,$1E,$1E,$1E,$1E,$1E,$1F,$1F,$1F,$1F,$1F,$1F,$20,$20,$20