#!/bin/sh  
# \
exec oagtclsh "$0" "$@"

if ![info exists env(OAG_TOP_DIR)] { 
    set env(OAG_TOP_DIR) /usr/local/
}

set auto_path [linsert $auto_path 0  $env(OAG_TOP_DIR)/oag/apps/lib/$env(HOST_ARCH)]
set auto_path [linsert $auto_path 0 $env(OAG_TOP_DIR)/oag/lib_patch/$env(HOST_ARCH)]

APSStandardSetup
package require sdds

set usage {usage: generateBalancedFillOrder -output <filename> [-playback <filename] -bunches <number> [-verbose {0|1}(0)}
set bunches 48
set playback ""
set output ""
set verbose 0
set args $argv
if {[APSStrictParseArguments {output playback bunches verbose}] || ![string length $output] || [expr $bunches<=0] || \
   [expr $bunches>1296]} {
    return -code error "$usage"
}
if [file exists $output] {
    return -code error "in use: $output"
}

set vx 0.0
set vy 0.0
set qTotal 0.0
set pi [expr 4*atan(1)]
set qBunch [expr 200*3.68/$bunches]

for {set b 0} {$b<$bunches} {incr b} {
    set qStored($b) 0.0
    set bucket($b) [expr $b*1296/$bunches]
}

set vx 0.0
set vy 0.0
proc addVector {q slot} {
    global vx vy pi
    set k [expr 2*$pi/1296]
    set vx [expr $vx+$q*cos($k*$slot)]
    set vy [expr $vy+$q*sin($k*$slot)]
    #puts stderr "V = ($vx, $vy)"
}

proc findClosestEmptyBucket {angle} {
    global pi qStored bunches bucket
    set fIdeal [expr $angle/(2*$pi)*1296]
    set bestDistance 1e10
    set fBest 0
    for {set b 0} {$b<$bunches} {incr b} {
	set f [expr $bucket($b)]
	if [expr $qStored($b)==0] {
	    set distance [expr abs($f-$fIdeal)]
	    if [expr $distance<$bestDistance] {
		set bestDistance $distance
		set fBest $f
	    }
	}
    }
    return $fBest
}

# Compute the distance (in buckets) between two bunches and a third (trial) location
proc computeBunchDistance {b1 b2} {
    global qStored bunches bucket
    set s1 $bucket($b1)
    set s2 $bucket($b2)
    return [computeBucketDistance $s1 $s2]
}

# Compute the distance (in buckets) between two bunches
proc computeBucketDistance {s1 s2} {
    set distance1 [expr ($s1-$s2)%1296]
    set distance2 [expr ($s2-$s1)%1296]
    return [expr min($distance1,$distance2)]
}

proc findDistantEmptyBucket {sLast1 sLast2} {
    global pi qStored bunches bucket
    set bestDistance -1e10
    set bBest -1
    # Find an empty slot with a maximally large distance from any filled bucket
    for {set b1 0} {$b1<[expr $bunches]} {incr b1} {
	if [expr $qStored($b1)==0] {
            # At this point we have a loop over empty buckets
            set minDistance 1296
            for {set b2 0} {$b2<$bunches} {incr b2} {
                if [expr $qStored($b2)!=0] {
                    # This is effectively a loop over filled buckets
                    set minDistance [expr min([computeBunchDistance $b1 $b2],$minDistance)]
		}
	    }
            if $minDistance>$bestDistance {
                set bestDistance $minDistance 
                set bBest $b1
                set sBest $bucket($b1)
                set bestList $b1
            } elseif $minDistance==$bestDistance {
                lappend bestList $b1
            }
        }
    }

    if [llength $bestList]>1 {
	# Second stage: try to distance ourselves from the last two filled bunches
	set bestDistance -1e10
	set sBest -1
	foreach bTrial $bestList {
	    set distance1 [computeBucketDistance $bucket($bTrial) $sLast1]
	    set distance2 [computeBucketDistance $bucket($bTrial) $sLast2]
	    set distance [expr min($distance1,$distance2)]
	    if $distance>$bestDistance {
		set sBest $bucket($bTrial)
		set bestDistance $distance
	    }
	}
    }
    if $sBest==-1 {
	for {set b 0} {$b<$bunches} {incr b} {
	    if [expr $qStored($b)==0] {
		set sBest $bucket($b)
		break
	    }
	}
    }
    #puts stderr "findDistantEmptyBucket: $sBest"
    return $sBest
}

proc saveStep {step} {
    global bunches qStored bucket bucketList chargeList 
    set chargeList($step) [list ]
    set bucketList($step) [list ]
    for {set b 0} {$b<$bunches} {incr b} {
	lappend chargeList($step) [format %.3f $qStored($b)]
	lappend bucketList($step) [expr $bucket($b)]
    }
    #puts stderr "bucketList($step): $bucketList($step)"
    #puts stderr "chargeList: $chargeList($step)"
}

set sLast1 0
set sLast2 0
for {set b 0} {$b<$bunches} {incr b} {
    if $verbose>0 {
        puts stderr "Working on $b of $bunches"
    }
    if $b==0 {
	# Fill bucket 0 first
	set slot 0
        if $verbose>1 {
            puts stderr "Filling bucket $slot"
        }
        lappend reasonList "Empty"
    } else {
	if [expr abs($vx)<1e-6 && abs($vy)<1e-6] {
	    # Find a bucket distant from any filled bucket
	    set slot [findDistantEmptyBucket $sLast1 $sLast2]
            if $verbose>1 {
                puts stderr "Filling distant bucket $slot"
            }
            lappend reasonList "Distance"
	} else {
	    set thetaOpt [expr atan2(-$vy, -$vx)]
	    if [expr $thetaOpt<0] {
		set thetaOpt [expr $thetaOpt+2*$pi]
	    }
	    set slot [findClosestEmptyBucket $thetaOpt]
            if $verbose>1 {
                puts stderr "Filling best bucket $slot"
            }
            lappend reasonList "Vector"
	}
    }
    addVector $qBunch $slot
    lappend orderedSlotList $slot
    lappend orderedList $slot
    set b1 [expr $bunches*$slot/1296]
    set qStored($b1) $qBunch
    set sLast2 $sLast1
    set sLast1 $slot
    lappend pvList S-INJ:BucketFillQueue[format %03d $b]C.VAL
    # Add a charge of 1 for each filled bucket, can be scaled in IOC 
    lappend pvList S-INJ:ChargeFillQueue[format %03d $b]C.VAL
    lappend orderedList 1
    saveStep $b
}

if [string length $playback] {
    # Write output
    set data(ColumnNames) [list Bucket Charge]
    set data(ParameterNames) [list Reason]
    set data(ColumnInfo.Bucket) "type SDDS_SHORT"
    set data(ColumnInfo.Charge) "type SDDS_DOUBLE units nC"
    set data(ParameterInfo.Reason) "type SDDS_STRING"
    set data(Column.Bucket) [list ]
    set data(Column.Charge) [list ]
    set data(Parameter.Reason) $reasonList
    for {set step 0} {$step<$bunches} {incr step} {
        lappend data(Column.Bucket) $bucketList($step)
        lappend data(Column.Charge) $chargeList($step)
    }
    if [catch {sdds save $playback data} result] {
        puts stderr "$result"
    }
}

set lfbPattern [format 1:%d:1296 [expr 1296/$bunches]]

                   
lappend pvList S-INJ:FillQueueNumberOfBunchesC
lappend orderedList $bunches
lappend pvList S-LFB:Z:FB:PATTERN S-TFB:X:FB:PATTERN S-TFB:Y:FB:PATTERN
lappend orderedList $lfbPattern $lfbPattern $lfbPattern

exec sddsmakedataset $output -column=Bucket,type=short -data=[join $orderedSlotList ,]

exec sddsmakedataset $output.putMe  \
  -column=ControlName,type=string -data=[join $pvList ,] \
  -column=ValueString,type=string -data=[join $orderedList ,] 
