;;; Quintic Pythagorean Hodograph Curve Demo
;;; by Peter Salvi, 2011.10.24.

#lang racket

;;; Libraries
(require racket/gui)

;;; Parameters
(define point-radius 4)
(define line-width 0)
(define resolution 100)

;;; Default placements
(define p0 '(100 200))
(define p1 '(400 200))
(define d0 '(100 100))
(define d1 '(100 -100))

;;; Options
(define control? #f)
(define bezier? #f)
(define hermite? #f)
(define offset? #f)
(define offset-distance 30)

;;; Variables
(define dragged #f)
(define arc-length #f)

;;; Basic Maths
(define (binomial n k)
  (if (= k 0)
      1
      (* (/ n k) (binomial (- n 1) (- k 1)))))
(define (v+ . args) (apply map + args))
(define (v- . args) (apply map - args))
(define (v* u . args) (map (lambda (x) (apply * x args)) u))
(define (vlength u) (sqrt (apply + (map (lambda (x) (* x x)) u))))
(define (vnormalize u) (v* u (/ (vlength u))))
(define (point-distance p q) (vlength (v- q p)))
(define (scalar-product u v) (apply + (map * u v)))
(define (to-system u d)
  (let ([l (vlength u)]
        [u (vnormalize u)]
        [v (vnormalize (list (- (second u)) (first u)))])
    (v* (list (scalar-product d u) (scalar-product d v)) (/ l))))
(define (from-system u d)
  (let ([v (list (- (second u)) (first u))])
    (v+ (v* u (first d)) (v* v (second d)))))
(define (to-complex p) (+ (first p) (* (second p) 0+1i)))
(define (from-complex p) (list (real-part p) (imag-part p)))

;;; Curves
;;; (there is a lot of redundancy here, some computations should be cached)

(define (bezier-evaluate points)
  (let ([n (- (length points) 1)])
    (for/list ([i (in-range resolution)])
      (let* ([u (/ i (- resolution 1))]
             [v (- 1 u)]
             [p '(0 0)])
        (for ([k (in-range (+ n 1))]
              [q points])
          (set! p (v+ p (v* q (binomial n k) (expt u (- n k)) (expt v k)))))
        (make-object point% (first p) (second p))))))

(define (rational-bezier-evaluate points)
  (bezier-evaluate (map (lambda (p)
                          (list (/ (second p) (first p))
                                (/ (third p) (first p))))
                        points)))

(define (bezier-points)
  (let ([p0+d0 (v+ p0 (v* d0 1/3))]
        [p1-d1 (v- p1 (v* d1 1/3))])
    (bezier-evaluate (list p0 p0+d0 p1-d1 p1))))

(define (hermite-points)
  (for/list ([i (in-range resolution)])
    (let* ([u (/ i (- resolution 1))]
           [p (v+ (v* p0 (+ (* 2 u u u) (* -3 u u) 1))
                  (v* d0 (+ (* u u u) (* -2 u u) u))
                  (v* d1 (- (* u u u) (* u u)))
                  (v* p1 (+ (* -2 u u u) (* 3 u u))))])
      (make-object point% (first p) (second p)))))

(define (ph-parameters)
  (let* ([d0 (to-complex (to-system (v- p1 p0) d0))]
         [d1 (to-complex (to-system (v- p1 p0) d1))]
         [w0 (sqrt d0)]
         [w2 (sqrt d1)]
         [w1 (/ (+ (* -3 (+ w0 w2))
                   (sqrt (+ 120 (* -15 w0 w0) (* -15 w2 w2) (* 10 w0 w2))))
                4)])
    (map from-complex (list w0 w1 w2))))

(define (ph-control-points params)
  (match-let ([(list (list u0 v0) (list u1 v1) (list u2 v2)) params])
    (let* ([c0 '(0 0)]
           [c1 (v+ c0 (v* (list (- (* u0 u0) (* v0 v0)) (* 2 u0 v0)) 1/5))]
           [c2 (v+ c1 (v* (list (- (* u0 u1) (* v0 v1)) 
                                (+ (* u0 v1) (* u1 v0)))
                          1/5))]
           [c3 (v+ c2 (v* (list (- (* u1 u1) (* v1 v1)) (* 2 u1 v1)) 2/15)
                   (v* (list (- (* u0 u2) (* v0 v2)) (+ (* u0 v2) (* u2 v0)))
                       1/15))]
           [c4 (v+ c3 (v* (list (- (* u1 u2) (* v1 v2))
                                (+ (* u1 v2) (* u2 v1)))
                          1/5))]
           [c5 (v+ c4 (v* (list (- (* u2 u2) (* v2 v2)) (* 2 u2 v2)) 1/5))])
      (map (lambda (p) (v+ p0 (from-system (v- p1 p0) p)))
           (list c0 c1 c2 c3 c4 c5)))))

(define (control-points)
  (map (lambda (p) (make-object point% (first p) (second p)))
       (ph-control-points (ph-parameters))))

(define (ph-points)
  (bezier-evaluate (ph-control-points (ph-parameters))))

(define (ph-speed params)
  (match-let ([(list (list u0 v0) (list u1 v1) (list u2 v2)) params])
    (list (+ (* u0 u0) (* v0 v0))
          (+ (* u0 u1) (* v0 v1))
          (+ (* 2/3 (+ (* u1 u1) (* v1 v1)))
             (* 1/3 (+ (* u0 u2) (* v0 v2))))
          (+ (* u1 u2) (* v1 v2))
          (+ (* u2 u2) (* v2 v2)))))

(define (ph-arc-length)
  (* (/ (foldl + 0 (ph-speed (ph-parameters))) 5)
     (point-distance p0 p1)))

(define (ph-offset-control-points distance)
  (let* ([parameters (ph-parameters)]
         [points (map (lambda (p) (cons 1 p))
                      (ph-control-points parameters))]
         [normals (map (lambda (i)
                         (match-let ([(list 0 x y)
                                      (v- (list-ref points (+ i 1))
                                          (list-ref points i))])
                           (list 0 y (- x))))
                       '(0 1 2 3 4))]
         [speed (ph-speed parameters)])
    (for/list ([k (in-range 10)])
      (let ([p '(0 0 0)])
        (for ([j (in-range (max 0 (- k 5)) (+ (min 4 k) 1))])
          (set! p (v+ p (v* (v+ (v* (list-ref points (- k j))
                                    (list-ref speed j))
                                (v* (list-ref normals j) 5 distance))
                            (/ (* (binomial 4 j) (binomial 5 (- k j)))
                               (binomial 9 k))))))
        p))))

(define (offset-control-points distance)
  (map (lambda (p)
         (make-object point% (/ (second p) (first p)) (/ (third p) (first p))))
       (ph-offset-control-points distance)))

(define (offset-points distance)
  (rational-bezier-evaluate
   (ph-offset-control-points distance)))

;;; Graphics

(define (draw-point dc p)
  (send dc draw-ellipse
        (- (first p) point-radius) (- (second p) point-radius)
        (* point-radius 2) (* point-radius 2)))

(define (draw canvas dc)
  (unless dragged
    (when bezier?
      (send dc set-pen "GREEN" line-width 'solid)
      (send dc draw-lines (bezier-points)))
    (when hermite?
      (send dc set-pen "GREEN" line-width 'solid)
      (send dc draw-lines (hermite-points)))
    (when control?
      (send dc set-pen "RED" line-width 'solid)
      (send dc draw-lines (control-points))
      (when offset?
        (send dc draw-lines
              (offset-control-points (/ offset-distance 100)))
        (send dc draw-lines
              (offset-control-points (/ (- offset-distance) 100))))))
  (send dc set-pen "BLACK" line-width 'solid)
  (when (and (not dragged) offset?)
    (send dc draw-lines (offset-points (/ offset-distance 100)))
    (send dc draw-lines (offset-points (/ (- offset-distance) 100))))
  (send dc draw-lines (ph-points))
  (let ([p0+d0 (v+ p0 (v* d0 1/3))]
        [p1+d1 (v+ p1 (v* d1 1/3))])
    (send dc set-brush "BLACK" 'solid)
    (send dc set-pen "BLACK" line-width 'solid)
    (for-each (lambda (p) (draw-point dc p)) (list p0 p1))
    (send dc set-brush "BLUE" 'solid)
    (send dc set-pen "BLUE" line-width 'solid)
    (for-each (lambda (p) (draw-point dc p)) (list p0+d0 p1+d1))
    (send dc draw-line (first p0) (second p0) (first p0+d0) (second p0+d0))
    (send dc draw-line (first p1) (second p1) (first p1+d1) (second p1+d1)))
  (send arc-length set-label (format "Arc length: ~a pixels" (ph-arc-length))))

;;; GUI

(define (handle-mouse-movement event)
  (if dragged
      (let ([p (list (send event get-x) (send event get-y))])
        (case dragged
          [(0) (set! p0 p)]
          [(1) (set! d0 (v* (v- p p0) 3))]
          [(2) (set! p1 p)]
          [(3) (set! d1 (v* (v- p p1) 3))])
        #t)
    #f))

(define (handle-mouse-down event)
  (if dragged
      (handle-mouse-up event)
      (let* ([p (list (send event get-x) (send event get-y))]
             [p0+d0 (v+ p0 (v* d0 1/3))]
             [p1+d1 (v+ p1 (v* d1 1/3))]
             [points (list p0 p0+d0 p1 p1+d1)])
        (for ([i (in-range 4)]
              [pi points])
          (when (< (point-distance p pi) point-radius)
            (set! dragged i))))))

(define (handle-mouse-up event)
  (set! dragged #f)
  #t)

(define ph-canvas%
  (class canvas%
    (inherit refresh)
    (define/override (on-event event)
      (when (case (send event get-event-type)
              [(motion) (handle-mouse-movement event)]
              [(left-down) (handle-mouse-down event)]
              [(left-up) (handle-mouse-up event)])
        (refresh)))
    (super-new)))

(let* ([frame (new frame% [label "Pythagorean Hodograph Curves"])]
       [vbox (new vertical-pane% [parent frame])]
       [canvas (new ph-canvas% [parent vbox]
                    [min-width 640]
                    [min-height 480]
                    [paint-callback draw])]
       [hbox1 (new horizontal-pane% [parent vbox])]
       [hbox2 (new horizontal-pane% [parent vbox])]
       [hbox3 (new horizontal-pane% [parent vbox])])
  (let-syntax ([add-option (syntax-rules ()
                             [(_ var a-label box)
                              (new check-box% [parent box]
                                   [label a-label] [value var]
                                   [callback (lambda (c e)
                                               (set! var (not var))
                                               (send canvas refresh))])])])
    (add-option control? "Show control points" hbox1)
    (add-option bezier? "Show cubic curve (using Bézier)" hbox1)
    (add-option hermite? "Show cubic curve (using Hermite)" hbox1)
    (add-option offset? "Show offset curves" hbox2)) 
  (new slider% [label "Offset distance:"] [parent hbox2]
       [style '(horizontal plain)] [init-value offset-distance]
       [min-value 5] [max-value 60]
       [callback (lambda (b e)
                   (set! offset-distance (send b get-value))
                   (send canvas refresh))])
  (set! arc-length (new message% [label "Arc length: N/A"] [parent hbox3]
                        [stretchable-width #t]))
  (send frame show #t))