## same directory.
##
## This program is based on a C program by the same name from Dexter Industries.
## See (https://github.com/DexterInd/DI_LEGO_NXT/blob/master/dIMU/RobotC/Gryo_Example.c)
##

package require piio

## same directory.
##
## This program is based on a C program by the same name from Dexter Industries.
## See (https://github.com/DexterInd/DI_LEGO_NXT/blob/master/dIMU/RobotC/Gryo_Example.c)
##

package require piio

##
## These bytes set the full scale range of the gyroscope.
## it is important to define full_scale_range.  Values are:
##      0x00 - 250 dps.  Full scale range.
##      0x10 - 500 dps.  Full scale range.
##      0x30 - 2000 dps.  Full scale range.
##
array set GyroValues {
    250dps  0x00
    500dps  0x10
    2000dps 0x30
    250dps,divisor 128.0
    500dps,divisor 64.0
    2000dps,divisor 16.0
    divisor 1.0
    full_scale_range 0x30
    gryo,offset 0xD2
    gryo,x,axis 0x28
    gryo,y,axis 0x2a
    gryo,z,axis 0x2c
    rate 50.0
    freq 10.0
}

set i2cbus 1
set dIMUaddress {}
set range 2000dps

proc StartGyro {handle range} {
    global GyroValues

    set base $GyroValues(gryo,offset)

    ##
    ## Write CTRL_REG1 - Enable all axes. Disable power down.
    ##
    twowire writeregbyte $handle [expr {$base + 0x20}] 0x0F
    piio usleep 10
    set status [twowire readbyte $handle]

    ##
    ## Write CTRL_REG2 - No High Pass Filter.
    ##
    twowire writeregbyte $handle [expr {$base + 0x21}] 0x00
    piio usleep 10
    set status [twowire readbyte $handle]

    ##
    ## Write CTRL_REG3 - No interrupts.  Date ready.
    ##
    twowire writeregbyte $handle [expr {$base + 0x22}] 0x08
    piio usleep 10
    set status [twowire readbyte $handle]

    ##
    ## Write CTRL_REG4 - Specified scale range.
    ##
    twowire writeregbyte $handle [expr {$base + 0x23}] $GyroValues($range)
    piio usleep 10
    set status [twowire readbyte $handle]

    ##
    ## Write CTRL_REG5 - Enable all axes. Disable power down.
    ##
    twowire writeregbyte $handle [expr {$base + 0x24}] 0x00
    piio usleep 10
    set status [twowire readbyte $handle]

    ##
    ## Set divisor so that the output of our gyro axis readings can be turned
    ## into scaled values.
    ##
    set GyroValues(divisor) GyroValues($range,divisor)

    return;
}

##
## Gyro: gets a full axis reading, scaled to the full scale reading.  Returns
## in degrees per second.
##
proc GryoAxisGeading {axis} {
    global GyroValues

    set intValue [twowire readregword $GyroValues(handle) [expr {$base + $GyroValues(gryo,$axis,axis)}]]
    return [format {%8.3f} [expr {$intValue / $GyroValues(divisor)}]]
}

proc ClearHistory {} {
    global CurrentValue

    set CurrentValue(history) {}
}

proc ReadGyro {} {
    global GyroValues CurrentValue

    if {[info exists GyroValues(afterId)]} {
        after cancel $GyroValues(afterId)
    }
    set GyroValues(afterId) [after [expr {$GyroValues(rate) * 10}] ReadGyro]

    ##
    ## Read the GYROSCOPE
    ##
    set CurrentValue(x) [GryoAxisGeading x]
    set CurrentValue(y) [GryoAxisGeading y]
    set CurrentValue(z) [GryoAxisGeading z]

    ##
    ## See if we need to log it to the listbox widget
    ##
    if {[incr GyroValues(count)] >= $CurrentValue(freq)} {
        set GyroValues(count)
        set timeStamp [clock format [clock seconds]]
        .main.history.data insert {} end -values [list $timeStamp $CurrentValue(x) $CurrentValue(y) $CurrentValue(z)]
    }
}

proc StartReading {} {
    global i2cbus dIMUaddress range GyroValues

    set GyroValues(handle) [twowire twowire $i2cbus $dIMUaddress]
    StartGyro $GyroValues(handle) $range
    set GyroValues(count) 0
    ReadGyro

}

proc StopReading {} {
    global GyroValues

    if {[info exists GyroValues(afterId)]} {
        after cancel $GyroValues(afterId)
    }
}

proc CreateGui {} {
    wm title . "dIMU Gyroscope Example"

    ttk::frame .main
    grid configure .main -sticky nsew
    grid columnconfigure . .main -weight 1
    grid rowconfigure . .main -weight 1
    
    ttk::labelframe .main.controls -text {Controls}
    ttk::labelframe .main.current -text {Current Readings}
    ttk::labelframe .main.history -text {History}
    grid configure .main.controls -sticky nsew
    grid configure .main.current -sticky ew
    grid configure .main.history -sticky nsew
    grid rowconfigure .main .main.history -weight 1
    grid columnconfigure .main .main.history -weight 1

    CreateControlFrame .main.controls
    CreateCurrentReadingsFrame .main.current
    CreateHistoryFrame .main.history
}

proc CreateControlFrame {w} {
    ttk::label $w.busLbl \
        -text {I2C Bus:}
    ttk::combobox $w.busCMB \
        -textvariable ::i2cbus \
        -values [list 1] \
        -state readonly
    ttk::label $w.addrLbl \
        -text {iDMU Address:}
    ttk::entry $w.addrEnt \
        -width 5 \
        -textvariable ::dIMUaddress
    ttk::label $w.rangeLbl \
        -text {Range:}
    ttk::combobox $w.rangeCMB \
        -textvariable ::range \
        -values [list 250dps 500dps 2000dps] \
        -state readonly
    ttk::label $w.rateLbl1 \
        -text {Refresh Rate:}
    ttk::spinbox $w.rateEnt \
        -from 50.0 \
        -to 500.0 \
        -increment 10.0 \
        -format {%3.0f} \
        -wrap no \
        -command [format {set ::GyroValues(rate) [expr {entier([%1$s get])}]}  $w.rateEnt]
    $w.rateEnt set 50.0
    ttk::label $w.rateLbl2 \
        -text { in microseconds}
    ttk::label $w.frequencyLbl1 \
        -text {Logging Rate:}
    ttk::spinbox $w.frequencyEnt \
        -from 1.0 \
        -to 1000.0 \
        -increment 10.0 \
        -format {%4.0f} \
        -wrap no \
        -command [format {set ::GyroValues(freq) [expr {entier([%1$s get])}]}  $w.frequencyEnt]
    $w.frequencyEnt set 1.0
    ttk::label $w.frequencyLbl2 \
        -text { in seconds}
    ttk::frame $w.buttons
    ttk::button $w.buttons.start \
        -text {Start} \
        -width 6 \
        -command StartReading
    ttk::button $w.buttons.stop \
        -text {Stop} \
        -width 6 \
        -command StopReading
    ttk::button $w.buttons.reset \
        -text {Clear Log} \
        -width 10 \
        -command {.main.history.data delete [.main.history.data children {}]}
        
    grid configure $w.busLbl $w.busCMB $w.addrLbl $w.addrEnt $w.rangeLbl $w.rangeCMB x -padx 2 -sticky ew
    grid configure $w.rateLbl1 $w.rateEnt $w.rateLbl2 x x x x -padx 2 -sticky ew
    grid configure $w.frequencyLbl1 $w.frequencyEnt $w.frequencyLbl2 x x x -padx 2 -sticky ew
    grid configure $w.buttons -columnspan 7 -sticky ew -pady 3
    grid configure x $w.buttons.start x  $w.buttons.stop x  $w.buttons.reset x -sticky ew
    grid columnconfigure $w.buttons {0 2 4 6} -weight 1
    grid columnconfigure $w 6 -weight 1
        
}

proc CreateCurrentReadingsFrame {w} {
    ttk::label $w.xLabel \
        -text {X:}
    ttk::entry $w.xValue \
        -textvariable ::CurrentValue(x) \
        -state readonly
    ttk::label $w.yLabel \
        -text {Y:}
    ttk::entry $w.yValue \
        -textvariable ::CurrentValue(y) \
        -state readonly
    ttk::label $w.zLabel \
        -text {Z:}
    ttk::entry $w.zValue \
        -textvariable ::CurrentValue(z) \
        -state readonly
    grid configure $w.xLabel $w.xValue $w.yLabel $w.yValue $w.zLabel $w.zValue x -sticky ew -padx 2 -pady 1
    grid columnconfigure $w [list $w.xLabel $w.yLabel $w.zLabel] -uniform labels -weight 0
    grid columnconfigure $w [list $w.xValue $w.yValue $w.zValue] -uniform values -weight 0
    grid columnconfigure $w 6 -weight 1
}

proc CreateHistoryFrame {w} {
    ttk::treeview $w.data \
        -xscrollcommand [list $w.hsb set] \
        -yscrollcommand [list $w.vsb set] \
        -columns {timestamp xaxis yaxis zaxis} \
        -displaycolumns  {timestamp xaxis yaxis zaxis} \
        -show headings \
        -selectmode none
    ttk::scrollbar $w.vsb \
        -orient vertical \
        -command [list $w.data yview]
    ttk::scrollbar $w.hsb \
        -orient horizontal \
        -command [list $w.data xview]
    
    foreach column {timestamp xaxis yaxis zaxis} heading {"Time Stamp" "X Axis" "Y Axis" "Z Axis"} {
        $w.data column $column \
            -anchor w \
            -stretch yes
        $w.data heading $column \
            -anchor center \
            -text $heading
    }
    $w.data column timestamp \
            -anchor e

    grid configure $w.data $w.vsb -sticky nsew
    grid configure $w.hsb -sticky ew
    grid columnconfigure $w $w.data -weight 1
    grid rowconfigure $w $w.data -weight 1
}

CreateGui