(def can-timeout 500)
(def last-msg-time (systime))
(def adc-enabled nil)
(def last-speed-erpm 0)
(def last-mode-byte 0)
(def cruise-active nil)

(defun check-timeout ()
    (if (> (- (systime) last-msg-time) can-timeout) {
        (app-adc-override 0 0.0)
        (app-adc-override 1 0.0)
    })
)

(defun proc-sid (id data)
    (cond
        ((= id 0x690) {
            (var control-byte (bufget-u8 data 0))
            (if (= control-byte 0x20)
                (def adc-enabled t)
                (def adc-enabled nil)
            )
            (var target-erpm (+ (* (bufget-u8 data 2) 256) (bufget-u8 data 3)))
            (if (and (> target-erpm 0) (!= target-erpm last-speed-erpm)) {
                (def last-speed-erpm target-erpm)
                (conf-set 'l-max-erpm (to-float target-erpm))
            })
        })
        
        ((= id 0x100) {
            (if adc-enabled {
                (def last-msg-time (systime))

                (var gas-raw (bufget-u8 data 0))
                (var brake-raw (bufget-u8 data 1))
                (var mode-byte (bufget-u8 data 2))

                (var gas-percent (/ gas-raw 200.0))
                (var brake-percent (/ brake-raw 200.0))

                (if (and (= last-mode-byte 0x04) (= mode-byte 0x06)) {
                    (def cruise-active t)
                    (app-adc-override 3 1.0)
                })
                (if (and (= last-mode-byte 0x06) (= mode-byte 0x04)) {
                    (def cruise-active nil)
                    (app-adc-override 3 0.0)
                })
                (def last-mode-byte mode-byte)

                (var gas-voltage (* gas-percent 3.3))
                (var brake-voltage (* brake-percent 3.3))

                (if (= mode-byte 0x06) {
                    (if (> brake-raw 0) {
                        (if cruise-active {
                            (def cruise-active nil)
                            (app-adc-override 3 0.0)
                        })
                        (app-adc-override 0 0.0)
                        (app-adc-override 1 brake-voltage)
                    } {
                        (app-adc-override 1 0.0)
                    })
                } {
                    (if (> brake-raw 0) {
                        (app-adc-override 0 0.0)
                        (app-adc-override 1 brake-voltage)
                    } {
                        (app-adc-override 0 gas-voltage)
                        (app-adc-override 1 0.0)
                    })
                })
            }
            {
                (app-adc-override 0 0.0)
                (app-adc-override 1 0.0)
            })
        })
    )
)

(defun event-handler ()
    (loopwhile t
        (recv
            ((event-can-sid . ((? id) . (? data))) (proc-sid id data))
            (_ nil)
        )
    )
)

(spawn 150 (fn () 
    (loopwhile t {
        (check-timeout)
        (sleep 0.1)
    })
))

(spawn 256 (fn ()
    (loopwhile t {
        (var batt-val (to-i (* (get-batt) 1000.0)))
        (var poles (to-i (conf-get 'si-motor-poles)))
        (var diam-mm (to-i (* (conf-get 'si-wheel-diameter) 1000.0)))
        (var my-id (to-i (conf-get 'controller-id)))
        (var tx-buf (bufcreate 8))
        (bufset-u8 tx-buf 0 (shr batt-val 8))
        (bufset-u8 tx-buf 1 (bitwise-and batt-val 255))
        (bufset-u8 tx-buf 2 my-id)
        (bufset-u8 tx-buf 3 0)
        (bufset-u8 tx-buf 4 0)
        (bufset-u8 tx-buf 5 poles)
        (bufset-u8 tx-buf 6 (shr diam-mm 8))
        (bufset-u8 tx-buf 7 (bitwise-and diam-mm 255))
        (can-send-eid 0x692 tx-buf)
        (sleep 1.0)
    })
))

(app-adc-detach 3 1)

(def event-handler-thread (spawn event-handler))

(event-register-handler event-handler-thread)

(event-enable 'event-can-sid)