; virualtim

(debug-enable 'debug)
(debug-enable 'backtrace)
(read-enable 'positions)

(define (make-arm-seg attachto axis)
    (push)
    (scale (vector 0.4 0.4 2))
    (let ((bone (build-cube)))
    (pop)
    
    (grab bone)
    (let ((jointpos (vtransform (vector 0 0 -0.5) (get-transform))))
    (ungrab)
    (active-box bone)

    (let ((joint (build-hingejoint attachto bone jointpos axis)))
   
    (joint-param joint "LoStop" -2) 
    (joint-param joint "HiStop" 2)     
    (joint-param joint "FMax" 100) 
    (joint-param joint "FudgeFactor" 0)

    (list bone joint)))))

(define (arm-seg-get-bone arm-seg)
    (list-ref arm-seg 0))

(define (arm-seg-get-joint arm-seg)
    (list-ref arm-seg 1))

(define (make-arm)

    (push)
    (translate (vector 0 1.5 0))
    (scale (vector 1 3 2))
    (let ((torso (build-cube)))
    (pop)
    
    (active-box torso)
    (build-fixedjoint torso)

    (translate (vector 0 2.6 2.2))
    (let ((upper-arm (make-arm-seg torso (vector 0 1 0))))

    (translate (vector 0 0 2.2))
    (let ((lower-arm (make-arm-seg (arm-seg-get-bone upper-arm) (vector 1 0 0))))

    (translate (vector 0 0 1.4))
    (scale (vector 1 1 0.3))
    (let ((hand (make-arm-seg (arm-seg-get-bone lower-arm) (vector 1 0 0))))

    (list upper-arm lower-arm hand))))))


(osc-source "9999")

(define (arm-update arm)
    (let ((shoulder (arm-seg-get-joint (list-ref arm 0)))
          (elbow (arm-seg-get-joint (list-ref arm 1)))
          (wrist (arm-seg-get-joint (list-ref arm 2))))

    (define (drain-upper)
        (cond
            ((osc-msg "/bodydata/1/upper")
                (display (osc 0))(newline)
                (joint-angle shoulder 1 (* (osc 0) 2))
                (drain-upper))))

    (define (drain-lower)
        (cond
            ((osc-msg "/bodydata/1/lower")
                (display (osc 0))(newline)
   
                (joint-angle elbow 1 (* (osc 0) 2))
                (drain-lower))))

;    (joint-angle shoulder 1 1)

     
    (drain-upper)
    (drain-lower)))
    


(gravity (vector 0 -0.01 0))
(show-axis 1)
(clear)
(collisions 1)

(ground-plane (vector 0 1 0) 0)
(push)
(scale (vector 10 10 10))
(rotate (vector 90 0 0))
(build-plane)
(pop)

(define arm (make-arm))

(define (update)
    (arm-update arm))

(every-frame (update))