proc deg2rad { angle_degrees } {expr {($angle_degrees)*0.0174532925199}}
proc rad2deg { angle_radians } {expr {($angle_radians)*57.2957795131}}

proc math_sin_cos { angle nickname } {
    uplevel [list set cos_$nickname [expr {cos(($angle))}]]
    uplevel [list set sin_$nickname [expr {sin(($angle))}]]
}

proc matrix_rotation_z { angle } {
    math_sin_cos $angle t
    list $cos_t $sin_t 0 0  [expr -$sin_t] $cos_t 0 0 0 0 1 0  0 0 0 1
}

proc matrix_rotation_x { angle } {
    math_sin_cos $angle t
    list 1 0 0 0  0 $cos_t $sin_t 0  0 [expr -$sin_t] $cos_t 0 0 0 0 1
}

proc matrix_rotation_y { angle } {
    math_sin_cos $angle t
    list $cos_t 0 $sin_t 0  0 1 0 0  [expr -$sin_t] 0 $cos_t 0 0 0 0 1
}

proc matrix_translation {x y z} {
    #list 1 0 0 $x 0 1 0 $y 0 0 1 $z 0 0 0 1
    list 1 0 0 0 0 1 0 0 0 0 1 0  $x $y $z 1
}

proc matrix_identity {} {
    list 1 0 0 0 0 1 0 0 0 0 1 0  0 0 0 1
}

proc matrix_multiply { matA matB } {
    # use tcl trick to assign these variables
    foreach {b11 b12 b13 b14  b21 b22 b23 b24  b31 b32 b33 b34  b41 b42 b43 b44} $matB {}

    set result {}
    foreach {x y z w} $matA {
        lappend result \
            [expr {$x*$b11+$y*$b21+$z*$b31+$w*$b41}] \
            [expr {$x*$b12+$y*$b22+$z*$b32+$w*$b42}] \
            [expr {$x*$b13+$y*$b23+$z*$b33+$w*$b43}] \
            [expr {$x*$b14+$y*$b24+$z*$b34+$w*$b44}] 
    }
    return $result
}

proc apply_current_matrix { coords } {
    set ::currentmatrix [matrix_multiply $::worldmatrix $::viewmatrix]

    # use tcl trick to assign these variables
    foreach {a11 a12 a13 a14  a21 a22 a23 a24  a31 a32 a33 a34  a41 a42 a43 a44} $::currentmatrix {}

    set result {}
    foreach {x y z ONE} $coords {
        lappend result \
            [expr {$x*$a11+$y*$a21+$z*$a31+ $a41}] \
            [expr {$x*$a12+$y*$a22+$z*$a32+ $a42}] \
            [expr {$x*$a13+$y*$a23+$z*$a33+ $a43}] \
            1
    }
    set coords $result

    set result {}
    foreach { x y z o } $coords {
        if { $::perspective == 0 } {
            lappend result [expr $x + $::screencenter_x] \
                                         [expr $::screencenter_y - $y]
        } else {
            set z [expr $z + 1500]
            if { $z == 0 } { set z 1 }
            lappend result [expr $x/$z*1200 + $::screencenter_x] \
                                         [expr $::screencenter_y - $y/$z*1200]
        }
        
    }
    return $result
}

## ------------------------------------------------------------
## Basic graphical elements.
## ------------------------------------------------------------

proc draw_3D_line { xyzcoords {obj_tag {}} {tags {}} } {
    set coords [apply_current_matrix $xyzcoords]

    if { ![info exists ::id($obj_tag)] } {
        set ::id($obj_tag) [$::maincanvas create line $coords -tags \
                                                            [lappend tags $obj_tag] ]
    } else {
        $::maincanvas coords $::id($obj_tag) $coords
    }
             
}

proc draw_prism { prism_tag xsize ysize zsize \
                                                        xoffset yoffset zoffset {tags Robot} } {
    foreach {face coords} {
        A {-1 -1 -1  1    1 -1 -1  1    1  1 -1  1   -1  1 -1  1   -1 -1 -1  1}
        B {-1 -1  1  1    1 -1  1  1    1  1  1  1   -1  1  1  1   -1 -1  1  1}
        C {-1 -1 -1  1   -1 -1  1  1   -1  1  1  1   -1  1 -1  1   -1 -1 -1  1}
        D { 1 -1 -1  1    1 -1  1  1    1  1  1  1    1  1 -1  1    1 -1 -1  1}
        } {
            set results {}
            foreach {x y z one} $coords {
                lappend results \
                    [expr $x * $xsize + $xoffset] \
                    [expr $y * $ysize + $yoffset] \
                    [expr $z * $zsize + $zoffset] \
                    1
            }
            set coords $results
            draw_3D_line $coords ${prism_tag}_$face $tags
        }
}

proc draw_plane { obj_tag indices {tags {}} } {
         set delta 30.0
         set num 11
         set max [expr {$delta*($num-1)}]
         set coords { 0 0 0 1  0 0 0 1}
         set counter 0
         foreach { a b c } $indices {
             set vector $coords
             set plane_tag ${obj_tag}_[incr counter]
             for {set x 0.0} {$x <= $max} {set x [expr {$x+$delta}]} {
                 lset vector $a $x
                 lset vector $b $x
                 lset vector $c $max
                 draw_3D_line $vector ${plane_tag}_$x [concat [list $obj_tag] $tags]
             }
         }
}

## ------------------------------------------------------------
## TK options.
## ------------------------------------------------------------

option add *command_buttons.exit.text        "Exit"

## ------------------------------------------------------------
## Widget procedures.
## ------------------------------------------------------------

proc create_scale { linkname master varname {from 0} {to 90} {default 0}} {
    set ::mem($varname.from) $from
    set ::mem($varname.to) $to
    set ::mem($varname.default) $default
    set $varname $default

    set f [labelframe $master.$varname -class [string totitle $varname]]
    set column_index 0

    label [set label_widget $f.lab_$varname] -text $linkname
    scale [set scale_widget $f.$varname] \
        -from $::mem($varname.from) -to $::mem($varname.to)

    $scale_widget set $::mem($varname.default)

    $scale_widget configure -command \
            [list update_parameter_from_scale_widget $varname]

    grid $label_widget -column $column_index -row 0 -sticky news
    grid $scale_widget -column $column_index -row 1 -sticky news
    incr column_index

    return $f
}

proc update_parameter_from_scale_widget { param_name param_value } {
    set $param_name $param_value
    after 0 update_and_draw
}

## ------------------------------------------------------------
## Main procedures.
## ------------------------------------------------------------

proc main_exit {} {exit}

proc main_program {} {

         widget_setup_defaults

         # configure_toplevel
         wm geometry . +20+20
         wm title . "Simple 3D Robot Arm"

         grid columnconfigure . 0 -weight 1
         grid rowconfigure    . 0 -weight 1

         set ::maincanvas [canvas .drawing -width 600 -height 600 -bg #f8f8f8]
         frame .controlsFrame
         grid  $::maincanvas .controlsFrame  -sticky news

         set camerabox [build_camera_controls .controlsFrame]
         set robotbox  [build_robot_controls  .controlsFrame]
         grid  $camerabox -sticky news
         grid  $robotbox  -sticky news

         set ::bp [button .controlsFrame.pers -command toggle_perspective]
         $::bp config -text "Perspective is On"
         grid $::bp
         grid [button .controlsFrame.exit -command main_exit -text "Exit"]

         update_and_draw

         configure_drawing ;# do this once
}

proc toggle_perspective {} {
    if {$::perspective} {
        set ::perspective 0
         $::bp config -text "Perspective is Off"
    } else {
        set ::perspective 1
         $::bp config -text "Perspective is On"
    }
    after 0 update_and_draw
}

proc set_worldmatrix {mat} {
        set ::worldmatrix $mat
}
proc set_viewmatrix {mat} {
        set ::viewmatrix  $mat
}

proc mult_worldmatrix {mat} {
        set ::worldmatrix [matrix_multiply $mat $::worldmatrix]
}
proc mult_viewmatrix {mat} {
        set ::viewmatrix [matrix_multiply $mat $::viewmatrix]
}

proc get_worldmatrix {} {
        return $::worldmatrix
}
proc get_viewmatrix {} {
        return $::viewmatrix
}

#============================================================
proc update_and_draw {} {

     set_viewmatrix  [matrix_rotation_x [deg2rad $::cam_angle1]]
    mult_viewmatrix  [matrix_rotation_y [deg2rad $::cam_angle2]]

     set_worldmatrix [matrix_identity]

    # draw workspace -----------------------------------
    draw_plane WorkSpace_XY  { 0 4 5   1 5 4 } [list WorkSpace]
    draw_plane WorkSpace_YZ  { 1 5 6   2 6 5 } [list WorkSpace]
    draw_plane WorkSpace_ZX  { 2 6 0   4 0 6 } [list WorkSpace]
    draw_3D_line {0 0 0 1   300   0   0 1} XAXIS
    draw_3D_line {0 0 0 1     0 300   0 1} YAXIS
    draw_3D_line {0 0 0 1     0   0 300 1} ZAXIS

    ### move this
    set ::mem(link0) [matrix_rotation_y [deg2rad $::theta_base]]
    set ::mem(link1) [matrix_rotation_x [deg2rad $::anngle1]]
    set ::mem(link2) [matrix_rotation_x [deg2rad $::anngle2]]
    set ::mem(link3) [matrix_rotation_x [deg2rad $::anngle3]]
    set ::mem(link4) [matrix_rotation_z [deg2rad $::theta_hand]]

    set ::mem(left5)  [matrix_translation        $::sllde5  0 0]
    set ::mem(right5) [matrix_translation [expr -$::sllde5] 0 0]

    # draw robot ---------------------------------------
    set_worldmatrix [matrix_translation 200 0 150]
    mult_worldmatrix $::mem(link0)
    # draw base
    draw_prism base 40 50 40  -40 50 0

    mult_worldmatrix [matrix_translation 5 75 0]
    mult_worldmatrix $::mem(link1)
    # draw link 1
    draw_prism part1  5 10 10  0 0 0
    draw_prism part2 10 10 50   15 0 30

    mult_worldmatrix [matrix_translation  0 0 80]
    mult_worldmatrix $::mem(link2)
    # draw link 2
    draw_prism part3  5 10 10  0 0 0
    draw_prism part4 10 10 50   -15 0 30

    mult_worldmatrix [matrix_translation 0 0 80]
    mult_worldmatrix $::mem(link3)
    # draw link 3
    draw_prism part5  5 10 10  0 0 0
    draw_prism part6 10 10 50   15 0 30


    mult_worldmatrix [matrix_translation 15 0 85]
    mult_worldmatrix $::mem(link4)
    draw_prism part7 10 10 5  0 0 0 
    draw_prism part8 60 10 10  0 0 15

    set savemat [get_worldmatrix]  ;# save current matrix

    mult_worldmatrix $::mem(left5)
    draw_prism lefthand  10 10 50   10 0 75

    set_worldmatrix $savemat       ;# restore previous matrix
    mult_worldmatrix $::mem(right5)
    draw_prism righthand 10 10 50  -10 0 75
}

proc build_robot_controls { master } {
    set frame0 [labelframe $master.robot -text "Robot Controls"]

    grid \
        [create_scale base  $frame0 ::theta_base   180 -180  0] \
        [create_scale arm1  $frame0 ::anngle1 180 -180  0] \
        [create_scale arm2  $frame0 ::anngle2 180 -180  0] \
        [create_scale arm3  $frame0 ::anngle3 180 -180  0] \
        -sticky news

    grid \
        [create_scale handspin $frame0 ::theta_hand 90 -90  0] \
        [create_scale grasp    $frame0 ::sllde5 40 0 40] \
        -sticky news

    return $frame0
}

proc build_camera_controls { master } {
    set f [labelframe $master.camera -text "Camera Controls"]
    grid \
        [create_scale psi $f ::cam_angle1  180 -180  -22] \
        [create_scale phi $f ::cam_angle2  180 -180 -150] \
        -sticky news
    return $f
}


proc configure_drawing {} {
    $::maincanvas itemco WorkSpace -fill #00bb00
    $::maincanvas itemco Robot -width 2

    $::maincanvas itemco XAXIS -fill red
    $::maincanvas itemco YAXIS -fill #008800
    $::maincanvas itemco ZAXIS -fill blue
}

proc widget_setup_defaults {} {
    set ::screencenter_x 300
    set ::screencenter_y 400
    set ::perspective 1

    option add *Workspace.text                "Camera"
    option add *Robot.text                "Robot"
}

main_program