$include "gp32regs.inc"
$BASE           10T             ; von jetzt an dezimal

; globale Vereinbarungen
RamStart        equ     $40
RomStart        equ     $8000
Vektor          equ     $ffdc
Stack           equ     $100    ; Startwert des Stack, ab hier abwärts

TRUE            EQU     1
FALSE           EQU     0
DELTA           EQU     $04

;;;;;;;;;;;;;;;;;;; RAM-Initialisierung ;;;;;;;;;;;;;;;;;;;;
        org     RamStart
; Variablen
; periodisch aktualisierte Helligkeitswerte, höherer Wert = dunkler
left       ds      1       ; links
right      ds      1       ; rechts
leftl      ds      1       ; links low     ; temporäre Werte
rightl     ds      1       ; rechts low
modus      ds      1       ; 0 - unbeeinflußte maximale zeitkonstante Ausrichtung
                           ; 1 - Stop der maximal zeitkonstanten Ausrichtung bei gleichen Werten
                           ; 2 - unbeeinflußte schrittweite zeitkonstante Ausrichtung
orient     ds      1       ; aktuelle Ausrichtung

;;;;;;;;;;;;;;;;;;; ROM-Initialisierung ;;;;;;;;;;;;;;;;;;;;
        org     RomStart
;;;;;;;;;;;;;;;;;;;; Konstanten ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;; Programmcode ;;;;;;;;;;;;;;;;;;;;;;;;;;;

$include "PLL.asm"
$include "S_ADC.asm"
$include "S_Servo.asm"
$include "TOOLS.asm"

; hier ist der Einstiegspunkt
MAIN:   ldhx    #Stack
        txs
        clra
        clrx
        clrh
; programmiere PLL (Taktfrequenz wird in Datei "PLL.ASM" gewählt)
;        jsr PLLInit            ; !!! im Simulator deaktivieren

; set CONFIGs appropriately
;        bset 1,CONFIG2         ; OSCSTOPENB, für TB on im Stop Mode
        ;            x         ; STOP-Instruction enabled
        ;             x        ; COPD, Watchdog komplett aus
        lda  #%01110011
        sta CONFIG1
; alles unnötige ausschalten
;        jsr     BRKPOWEROFF
;        jsr     KBPOWEROFF
;        jsr     SCIPOWEROFF
;        jsr     TIMPOWEROFF
; hier weitere Initialisierungen
        jsr     ADCINIT
        jsr     SERVOINIT

        ; Mittelausrichtung
        lda     #0
        sta     modus
        lda     #$7F
        sta     orient
        psha
        jsr     SERVO
        pula

; Main-Loop
HP:
;        sta     COPCTL          ; streichele Watchdog
        ; messen
        jsr ADC
        ; auswerten
        lda     left
        sub     right
        bpl HP01
        ; Differenz in A negativ! -> Kehrwert
        coma
        inca
HP01:
        cmp #DELTA
        ble HP3   ; branch if |left-right|<=DELTA
        ; einer ist kleiner/größer
        lda     left
        cmp     right
        bgt HP2   ; branch if left>right bzw. links dunkler
        ; left<right bzw. links heller
HP1:
        ; 1/20 nach links
        lda     #$02
        sta     modus
        lda     orient
        add     #DELTA
        bcc HP12
        lda     #$FF    ; Überlauf
HP12:        
        sta     orient
        psha
        jsr     SERVO
        pula
        bra     HP3
HP2:
        ; 1/20 nach rechts
        lda     #$02
        sta     modus
        lda     orient
        sub     #DELTA
        bcc HP22
        lda     #$00    ; Unterlauf
HP22:
        sta     orient
        psha
        jsr     SERVO
        pula
        bra     HP3

HP3:
        ; alles ausschalten
        jsr ADCPOWEROFF
        ; Timer starten
        ;      xxx      Time Base Rate Selection: 1Hz
        lda #%00000110
        sta TBCR
        ; schlafen
HP4:
        STOP
        ; wieder aufwachen
;        lda #0
;        sta modus
;        lda #$3F
;        psha
;        jsr SERVO
;        pula
        
        jsr ADCINIT
        bra     HP


;;;;;;;;;;;;;;;;;; Interrupt Service Routinen ;;;;;;;;;;;;;;
TBMISR:
        ; TBM abstellen
        lda #%00000000
        sta TBCR
        rti
; DummyISR, fängt illegale Interrupts ab.
DUMMY:
        rti
;;;;;;;;;;;;;;;;;;;;;; Vectortabelle ;;;;;;;;;;;;;;;;;;;;;;;
        org     Vektor
; Deklariere Vektortabelle
        dw      TBMISR          ; Time Base Interrupt
        dw      DUMMY           ; ADC Conversion Complete
        dw      DUMMY           ; Keyboard Interrupt
        dw      DUMMY           ; SCI Transmitter Int.
        dw      DUMMY           ; SCI Receiver Int.
        dw      DUMMY           ; SCI Status Int.
        dw      DUMMY           ; SPI Transmitter Int.
        dw      DUMMY           ; SPI Receiver Int.
        dw      DUMMY           ; TIM2 Overflow
        dw      DUMMY           ; Timer 2 Channel 1
        dw      DUMMY           ; Timer 2 Channel 0
        dw      DUMMY           ; TIM1 Overflow
        dw      DUMMY           ; Timer 1 Channel 1
        dw      DUMMY           ; Timer 1 Channel 0
        dw      DUMMY           ; Clock Generator Module
        dw      DUMMY           ; IRQ Pin
        dw      DUMMY           ; SWI Befehl
        dw      MAIN            ; Resetvektor
