# @brief Parse motor attribute file and make list of motor attributes and controllers
# @param fname Name of two column csv file of motor attribute names and values
# @param fhr Report log file-handle.
# @param fhe Error log file-handle.
# Requires The global ::ENC_ATTLIST
# Creates the following globals,
# ::mn(matt) is an array of motor attributes where mn = motor name and matt = the attribute name
# ::mn_encatts(matt) is an array of encoder attributes where mn = motor name and matt = an attributed from ENC_ATTLIST
# ::controllers(cn) Counts number of axes used on each controller, cn = controller name.
# ::motor_attcnt(mn) The keys of this array provide a list of motor names and each value is a count of attributes found for each motor.
proc parse_file {fname fhr fhe} {
  puts $fhr "\nPARSE '$fname'"
  set lcount 0
  set fh [open $fname "r"]
  while {[gets $fh line] >= 0} {
    incr lcount
    foreach {name val} [split $line {,}] {}
    if [regexp {(_?[a-zA-Z][a-zA-Z0-9]*[0-9]*)_([a-zA-Z0-9_]+)} $name rem mn matt] {
      puts $fhr "Processing '$line'"
      if {[lsearch $::ENC_ATTLIST $matt] != -1} {
        puts $fhr "Add ::${mn}_encatts($matt) = $val"
        set ::${mn}_encatts($matt) $val
      } else {
        puts $fhr "Add ::${mn}_attarr($matt) = $val"
        set ::${mn}_attarr($matt) $val
      }
      incr ::motor_attcnt($mn)
      if { $matt == "mc" } {
        incr ::controllers($val)
      }
    } else {
      incr ::ERRCNT
      puts $fhe "Failed to match '$line'"
    }
  }
  close $fh
  puts -nonewline $fhr "CONTROLLER USAGE COUNT: "
  foreach {cont} [lsort -dictionary [array names ::controllers]] {
    set count $::controllers($cont)
    puts -nonewline $fhr "$cont count=$count; "
  }
  puts $fhr ""
  puts $fhr "PARSED $lcount lines in '$fname'; failed $::ERRCNT lines"
  puts $fhe "$::ERRCNT ERRORS IN PARSE STAGE '$fname'. PROCESSED $lcount lines"
}

# @brief Generate motor configuration file header and make asyncqueues
# @param fhmc Motor configuration file handle.
# Requires the following globals,
# ::argv0, ::argv, ::file_list, and ::controllers
proc mk_cfg_header {fhmc} {
  puts $fhmc "#### SICS motor driver configuration ####"
  puts $fhmc "# Generated by: $::argv0 $::argv"
# puts $fhmc "# Date: [clock format [clock seconds] -format %Y-%m-%dT%H:%M:%S]"
  puts $fhmc "# Generated from the following files,"
  foreach f $::file_list {
    incr fcntr
    puts $fhmc "# file$fcntr: $f"
  }
  puts $fhmc ""
  puts $fhmc "# Load motor driver configuration parameters"
  puts $fhmc "set flist \[list\\"
  foreach f $::file_list {
    puts $fhmc "              \{$f\}\\"
  }
  puts $fhmc "          \]"
puts $fhmc {
foreach fattfile $flist {
  if [catch {
    set fattpath config/motors/$fattfile
    set fh [open $fattpath RDONLY]
    while {[gets $fh line] >= 0} {
      eval "set [split $line {,}]"
    }
    close $fh
  } msg] {
    clientput ERROR: $msg
  }
}

}
puts $fhmc {set sim_mode [SplitReply [motor_simulation]]}

# Setup addresses of Galil DMC2280 controllers.
puts $fhmc {
if {$sim_mode == true} {
  set motor_driver_type asim
} else {
  set motor_driver_type DMC2280
}
}
puts $fhmc "if {\$sim_mode == false} \{"
foreach cont [lsort -dictionary [array names ::controllers]] {
  set index [string toupper $cont]
  puts $fhmc "  [subst -nocommands {MakeAsyncQueue $cont DMC2280 [dict get \$::MOTOR_HOSTPORT $index HOST] [dict get \$::MOTOR_HOSTPORT $index PORT]}]"
}
puts $fhmc \}

}

# @brief Generate motor driver configuration
# @param mot Motor name
# @param fh Motor configuration file handle
# @param absEnc boolean, generate absolute encoder configuration if true.
# @param posnum Number of discrete positions
# @param posit_list Name of global list of name value pairs, "posit_n pos". Can be empty.
proc mk_motconf {mot fh absEnc posnum posit_list} {
  set mc [set ::${mot}_attarr(mc)]
  set axis [set ::${mot}_attarr(axis)]
  set units ${mot}_units
  set hardlowerlim ${mot}_rev_lim
  set hardupperlim ${mot}_fwd_lim
  set softlowerlim ${mot}_rev_lim
  set softupperlim ${mot}_fwd_lim
  set maxSpeed ${mot}_maxspeed
  set maxAccel ${mot}_maxaccel
  set maxDecel ${mot}_maxdecel
  set stepsPerX ${mot}_steps_per_x
  set cntsPerX ${mot}_cnts_per_x
  set absEncHome ${mot}_absenchome
  set home ${mot}_home
  set part ${mot}_part
  set long_name $mot

  puts $fh "# $mot configuration"
  if [info exists ::${mot}_attarr(description)] {
    puts $fh "# [string trim [set ::${mot}_attarr(description)]]"
  }
  if [info exists ::${mot}_attarr(axis_number)] {
    puts $fh "# Axis number [set ::${mot}_attarr(axis_number)]"
  }
  puts $fh "Motor $mot \$motor_driver_type \[params\\"
  puts $fh "  asyncqueue $mc\\"
  puts $fh "  axis $axis\\"
  puts $fh "  units \$$units\\"
  puts $fh "  hardlowerlim \$$hardlowerlim\\"
  puts $fh "  hardupperlim \$$hardupperlim\\"
  puts $fh "  maxSpeed \$$maxSpeed\\"
  puts $fh "  maxAccel \$$maxAccel\\"
  puts $fh "  maxDecel \$$maxDecel\\"
  puts $fh "  stepsPerX \$$stepsPerX\\"
  puts $fh "  posit_count $posnum\\"
  foreach positline $::posit_list {
    puts $fh "  $positline\\"
  }
  if {$absEnc} {
    puts $fh "  absEnc 1\\"
    puts $fh "  absEncHome \$$absEncHome\\"
    puts $fh "  cntsPerX \$$cntsPerX\]"
  } else {
    puts $fh "  absEnc 0\]"
  }
  puts $fh "$mot softlowerlim \$$softlowerlim"
  puts $fh "$mot softupperlim \$$softupperlim"
  puts $fh "$mot home \$$home"
  puts $fh "$mot part \$$part"
  puts $fh "$mot long_name $long_name"
  if [info exists ::${mot}_attarr(dflt_speed_steps)] {
    puts $fh "$mot speed \$${mot}_speed"
  }
  if [info exists ::${mot}_attarr(dflt_accel_steps)] {
    puts $fh "$mot accel \$${mot}_accel"
  }
  if [info exists ::${mot}_attarr(dflt_decel_steps)] {
    puts $fh "$mot decel \$${mot}_decel"
  }
  puts $fh ""
}



################################################################################
# UTILITY FUNCTIONS
################################################################################

# @brief Difference between sets A and B, ie A\B
# Invalid sets are allowed. Doesn't check for repetition.
# @return list of elements in A but not in B
proc setdiff {A B} {
  set diff {}
  foreach e $A {
    if { [lsearch $B $e] == -1 } {
      lappend diff $e
    }
  }
  return $diff
}

# @brief Tests if A is subset of B
# The sets must be sorted
# Doesn't check if sets are valid, eg repetition is allowed.
proc subset {A B} {
  set Alen [llength $A]
  set Blen [llength $B]
  if { $Alen > $Blen } {
    return false
  }
  set Aend [expr {$Alen - 1}]
  set Bend [expr {$Blen - 1}]
  set asi 0
  set aei 0
  set bsi 0
  set bei 0
  while { $asi <= [expr {$Aend - $aei}] } {
#   puts "A = [lrange $A $asi end-$aei]"
    set firstmatch false
    set lastmatch false
    while { $bsi <= [expr {$Bend - $bei}] } {
#     puts "    B = [lrange $B $bsi end-$bei]"
      if { $firstmatch == false } {
        set aval [lindex $A $asi]
        set bval [lindex $B $bsi]
        if { $aval == $bval} {
          set firstmatch true
#         puts "     firstmatch = $firstmatch"
        } elseif {$aval < $bval} {
          return false
        }
        incr bsi
      }
      if { $lastmatch == false } {
        set aval [lindex $A end-$aei]
        set bval [lindex $B end-$bei]
        if { $aval == $bval } {
          set lastmatch true
#         puts "     lastmatch = $lastmatch"
        } elseif {$aval > $bval} {
          return false
        }
        incr bei
      }
      if { ($firstmatch == true) && ($lastmatch == true) } {
        break
      }
    }
    if { ($firstmatch == false) && ($lastmatch == false) } {
      return false
    }
    incr asi
    incr aei
  }

  return true
}