(define (sscan-init)
	(define df (open-output-file "data"))
	(power-up-sonar)
	(set-sonar-interval! 9)	; about 1 second per scan
	df
)

(define (sscan-final df)
	(close-output-port df)
	(power-down-sonar)
)

(define (sscan-one spot df)
	(define pf (string-append spot " "))
	(scans pf df '(0 6 11 17))
	(scans pf df '(0 6 11 17))
	(scans pf df '(0 6 11 17))
	(scans pf df '(0 6 11 17))
	(display #\newline df)
)

(define (go-and-scan df arg)
	(let ((absc (car arg)) (gcmd (cadr arg)))
		(goto gcmd)
;		(display "goto " df)
;		(display absc df)
;		(display #\newline df)
		(sscan-one absc df)
	)
)

(define (goto arg)
	(rotto (car arg))
	(translate 'by (cadr arg))
)

(define (sscan-plan)
	(let ((df (sscan-init)))
		(map
			(lambda (arg) (go-and-scan df arg))
			'(
				; ( rot trans )
;				((   0    0) ( 0     0))
;				((1000    0) ( 0  1000))
;				((1000 1000) (90  1000))
;				((   0 1000) ( 0 -1000))
				((   0    0) ( 0     0))
				((1000    0) ( 0  1000))
				((2000    0) ( 0  1000))
				((3000    0) ( 0  1000))
				((4000    0) ( 0  1000))
			)
		)
		(sscan-final df)
	)
)

(define (scans pf df positions)
	(if (eq? positions '())
		#f
		(let ()
			(display pf df)
			(display (rotto (car positions)) df)
			(display " " df)
			(display (saround 0) df)
			(display #\newline df)
			(sleep 1000)
			(scans pf df (cdr positions))
		)
	)
)

(define (saround n)
	(if (>= n 16)
		'()
		(cons (read-sonar n) (saround (+ n 1)))
	)
)

(define (rotto angle)
	(let ()
		(define amt (- angle (rotate-where)))
		(rotate 'by amt)
		(rotate-where)
	)
)