#============================================================================
# rigid body EXP editing (to move into readexp.tcl)
#============================================================================

# AddRigidBody: add a new rigid body definition into the .EXP file
# arguments are:
#   multlist: defines a list of multipliers for each set of coordinates. In the 
#             simplest case this will be {1}
#   coordlist: a nested list of coordinates such as { { {0 0 0} {.1 .1 .1} {.2 .2 .2} } }
# note that when the length of multlist > 1 then coordlist must have the same length. 
# for input where 
#     multlist = {s1 s2} and 
#     coordlist = { { {0 0 0} {1 1 0} {.0 .0 .0} ...} 
#                     {0 0 0} {1 1 0} {2 1 2} ...} 
#                 }   
# the cartesian coordinates are defined from the input as 
#    atom 1 = s1 * (0,0,0) + s2*(0,0,0) [ = (0,0,0)]
#    atom 2 = s1 * (1,1,0) + s2*(1,1,0) [ = (s1+s2) * (1,1,0)]
#    atom 3 = s1 * (0,0,0) + s2*(2,1,2) [ = s2 * (2,1,2)]
#    ...
# Returns the number of the rigid body that has been created
proc AddRigidBody {multlist coordlist} {
    # 
    set n [string trim [readexp "RGBD  NRBDS"]]
    if {$n == ""} {
	makeexprec "RGBD  NRBDS"
	set n 0
    }
    incr n
    validint n 5
    setexp "RGBD  NRBDS" $n 1 5
    SetRigidBody $n $multlist $coordlist
    return $n
}

# ReplaceRigidBody: replace all the information for rigid body #rbnum
# Works the sames as AddRigidBody (see above) except that the rigid body is replaced rather 
# than added.
# Note that count of the # of times the body is used is preserved
proc ReplaceRigidBody {rbnum multlist coordlist} {
    set value $rbnum
    validint value 2
    set key "RGBD${value}"
    set line [readexp "$key NBDS"]
    foreach key [array names ::exparray "${key}*"] {
	#puts $key
	delexp $key
    }
    SetRigidBody $rbnum $multlist $coordlist
    setexp "$key NBDS" $line 1 68
}

# Edit the parameters for rigid body #rbnum 
# (normally called from ReplaceRigidBody or AddRigidBody)
proc SetRigidBody {rbnum multlist coordlist} {
    set value $rbnum
    validint value 2
    set key "RGBD${value}"
    # number of atoms
    set value [llength [lindex $coordlist 0]]
    validint value 5
    makeexprec "$key NATR"
    setexp "$key NATR" $value 1 5
    # number of times used
    set value 0
    validint value 5
    makeexprec "$key NBDS"
    setexp "$key NBDS" $value 1 5
    # number of coordinate matrices
    set value [llength multlist]
    validint value 5
    makeexprec "$key NSMP"
    setexp "$key NSMP" $value 1 5
    set i 0
    foreach mult $multlist coords $coordlist {
	incr i
	makeexprec "${key}${i}PARM"
	setexp "${key}${i}PARM" [format "%10.5f%5d%5d" $mult 0 0] 1 20
	set j 0
	foreach item $coords {
	    #puts $item
	    incr j
	    set value $j
	    validint value 3
	    makeexprec "${key}${i}SC$value"
	    setexp "${key}${i}SC$value" [eval format "%10.6f%10.6f%10.6f%10s" $item] 1 40
	}
    }
}

# convert a decimal to the GSAS hex encoding with a field $digits long.
proc ToHex {num digits} {
    return [string toupper [format "%${digits}x" $num]]
}

# convert a GSAS hex encoding to a decimal integer
proc FromHex {hex} {
    return [scan $hex "%x"]
}

# ApplyRigidBody: define an "instance" of a rigid body: meaning that the coordinates 
# (and optionally U values) for a set of atoms will be generated from the rigid body
# arguments:
#   phase: phase number (1-9)
#   bodytyp: number of rigid body (1-15) as returned from AddRigidBody
#   firstatom: sequence number of the first atom in phase (note that atoms may 
#              not be numbered sequentially)
#   position: list of three fractional coordinates for the origin of the rigid body coordinates
#   angles: list of 3 angles to rotate the rigid body coordinates around x, y, z of the 
#           cartesian system before the body is translated to position. 
# returns the instance # (number of times body $bodytyp has been used in phase $phase)
proc ApplyRigidBody {phase bodytyp firstatom position angles} {
    # number of bodies of this type in this phase
    set key "RGBD[ToHex $phase 1][ToHex $bodytyp 1]"
    set n [string trim [readexp "$key  NBDS"]]
    if {$n == ""} {
	makeexprec "$key  NBDS"
	set n 0
    }
    incr n
    set value $n
    validint value 5
    setexp "$key  NBDS" $value 1 5
    set key "RGBD[ToHex $phase 1][ToHex $bodytyp 1][ToHex $n 1]"
    set value $firstatom
    validint value 5
    makeexprec "$key  NDA"
    setexp "$key  NDA" $value 1 5
    set l1 {}
    set l2 {}
    for {set i 0} {$i < 9} {incr i} {
	append l1 [format %5d 0]
	append l2 [format %1d 0]
    }
    makeexprec "$key BDFL"
    setexp "$key BDFL" $l1$l2 1 54
    makeexprec "${key} BDLC"
    setexp "${key} BDLC" [eval format "%10.6f%10.6f%10.6f" $position] 1 30
    makeexprec "${key} BDOR"
    set l1 {}
    foreach val "$angles 0 0 0" dir "1 2 3 1 1 1" {
	append l1 [format "%8.2f%2d" $val $dir]
    }
    setexp "${key} BDOR" $l1 1 60
    makeexprec "${key} LSTF"
    setexp "${key} LSTF" [format "%5d" 0] 1 5
    return $n
}

# EditRigidBody: edit parameters that define an "instance" of a rigid body (see ApplyRigidBody)
# arguments:
#   phase: phase number (1-9)
#   bodytyp: number of rigid body (1-15) as returned from AddRigidBody
#   bodynum: instance number, as returned by ApplyRigidBody
#   position: list of three fractional coordinates for the origin of the rigid body coordinates
#   angles: list of 3 angles to rotate the rigid body coordinates around x, y, z of the 
#           cartesian system before the body is translated to position. 
# 
proc EditRigidBody {phase bodytyp bodynum position angles} {
    # number of bodies of this type in this phase
    set key "RGBD[ToHex $phase 1][ToHex $bodytyp 1][ToHex $bodynum 1]"
    setexp "${key} BDLC" [eval format "%10.6f%10.6f%10.6f" $position] 1 30
    set l1 {}
    foreach val "$angles 0 0 0" dir "1 2 3 1 1 1" {
	append l1 [format "%8.2f%2d" $val $dir]
    }
    setexp "${key} BDOR" $l1 1 60
}
#============================================================================
#============================================================================

package require La

# Use the GSAS geometry program to compute a set of cartesian coordinates for a 
# set of atoms in a .EXP file and provide the origin shift and Euler angles needed to 
# place the cartesian system into the crystal coordinates. Used for setting up a rigid body.
# Returns a nested list of lists:
#   element 0: a list of the origin location {x y z} in fraction coordinates
#   element 1: a list of three rotation angles in form {a1 a2 a3}
#              where a1, a2 and a3 are rotations around the cartesian x, y and z axes
#   element 2: a list of $natom cartesian coordinate triples {{x1 y1 z1} {x2 y2 z2}...}
# arguments:
    # phase: phase #
    # natom: number of atoms in group
    # firstatom: sequence # in phase (may be > than number of the atom)
    # originlist: atoms to define origin (where 1 is first atom in group; <= natom)
    # vector1: list of 3 values with X, Y or Z, atom #a and #b (number as in origin)
    # vector2: list of 3 values with X, Y or Z, atom #a and #b (number as in origin)
    # note that vector2 must define a different axis than vector1 
    # also and vector1 and vector2 cannot use the same atom pair
proc ExtractRigidBody {phase natom firstatom originlist vector1 vector2} {
    global expgui
    set fp [open "geom.inp" "w"]
    puts $fp "N"
    puts $fp "N"
    puts $fp $phase
    puts $fp "N"

    puts $fp "R"
    puts $fp "$natom"
    puts $fp "$firstatom"
    puts $fp [llength $originlist]
    foreach i $originlist {
	puts $fp $i
    }
    foreach i [concat $vector1 $vector2] {
	puts $fp $i
    }
    puts $fp "0"
    puts $fp "X"
    close $fp
    catch {
	exec [file join $expgui(gsasexe) geometry] $expgui(expfile) < geom.inp > geom.out
    }
    file delete geom.inp
    set fp [open geom.out r]
    while {[gets $fp line] >= 0} {
	if {[string first "Cell coordinates of origin" $line] != -1} {
	    set origin [lrange [string range $line [string first "are" $line] end] 1 3]
	}
	if {[string first "Rotation angles" $line] != -1} {
	    set Euler {}
	    foreach i [lrange [split $line "="] 1 3] {
		lappend Euler [lindex $i 0]
	    }
	    #puts $line
	    #puts $Euler
	}
	if {[string first "Atom   Orthon" $line] != -1} {
	    set coordlist {}
	    for {set i 1} {$i <= $natom} {incr i} {
		gets $fp line
		set coord {}
		lappend coord [string trim [string range $line 9 15]]
		lappend coord [string trim [string range $line 16 22]]
		lappend coord [string trim [string range $line 23 29]]
		lappend coord [string trim [string range $line 0 8]]
		#puts $line
		#puts $coord
		lappend coordlist $coord
	    }
	    #puts $coordlist
	}
    }
    #file delete geom.out
    return [list $origin $Euler $coordlist]
}

# updates the coordinates in a .EXP file after a rigid body instance has been added/changed
proc RunRecalcRBCoords { } {
    global expgui tcl_platform
    set input [open resetmult.inp w]
    puts $input "Y"
    puts $input "l b"
    puts $input "n"
    puts $input "x x x"
    puts $input "x"
    close $input
    # Save the current exp file
    savearchiveexp
    # disable the file changed monitor
    set expgui(expModifiedLast) 0
    set expnam [file root [file tail $expgui(expfile)]]
    set err [catch {
	if {$tcl_platform(platform) == "windows"} {
	    exec [file join $expgui(gsasexe) expedt.exe] $expnam < resetmult.inp >& resetmult.out
	} else {
	    exec [file join $expgui(gsasexe) expedt] $expnam < resetmult.inp >& resetmult.out
	}
    } errmsg]
    loadexp $expgui(expfile)
    set fp [open resetmult.out r]
    set out [read $fp]
    close $fp
    set expgui(exptoolout) $out
    catch {file delete resetmult.inp resetmult.out}
    if {$err} {
	return $errmsg
    } else {
	return ""
    }
}


# compute a rotation matrix for orthogonal coordinates (based on MAKMATD in GSAS)
# rotate angle degrees around axis (1, 2 or 3) for (x, y, or z)
# returns a list that can be used as a matrix in the La package
proc RotMatrix {axis angle} {
    set ang [expr {$angle * acos(0) / 90.}]
    set mat "1 0 0 0 1 0 0 0 1"
    if {$axis == 1}  {
	set i1 1
	set i2 2
    } elseif {$axis == 2}  {
	set i1 2
	set i2 0
    } else {
	set i1 0
	set i2 1
    }
    proc imat {i1 i2} {return [expr {(3*$i2) + $i1}]}
    foreach item {
	{$i1 $i1 [expr {cos($ang)}]}
	{$i2 $i2 [expr {cos($ang)}]}
	{$i1 $i2 [expr {-sin($ang)}]}
	{$i2 $i1 [expr {sin($ang)}]}
    } {
	foreach {c r val} [subst $item] {}
	set mat [lreplace $mat [imat $c $r] [imat $c $r] $val]	
    }
    return "2 3 3 $mat"
}

# compute the derivative of the rotation matrix with respect to the angle, see RotMatrix
# (based on MAKMATD in GSAS)
# returns a list that can be used as a matrix in the La package
proc DerivRotMatrix {axis angle} {
    set ang [expr {$angle * acos(0) / 90.}]
    set mat "0 0 0 0 0 0 0 0 0"
    if {$axis == 1}  {
	set i1 1
	set i2 2
    } elseif {$axis == 2}  {
	set i1 2
	set i2 0
    } else {
	set i1 0
	set i2 1
    }
    proc imat {i1 i2} {return [expr {(3*$i2) + $i1}]}
    foreach item {
	{$i1 $i1 [expr {-sin($ang) * acos(0) / 90.}]}
	{$i2 $i2 [expr {-sin($ang) * acos(0) / 90.}]}
	{$i1 $i2 [expr {-cos($ang) * acos(0) / 90.}]}
	{$i2 $i1 [expr {cos($ang) * acos(0) / 90.}]}
    } {
	foreach {c r val} [subst $item] {}
	set mat [lreplace $mat [imat $c $r] [imat $c $r] $val]	
    }
    return "2 3 3 $mat"
}

# compute an orthogonalization matrix from cell parameters (based on AMATRX in GSAS)
# returns a list that can be used as a matrix in the La package
proc OrthoMatrix {a b c alpha beta gamma} {
    set CA [expr {cos($alpha * acos(0) / 90.)}]
    set CB [expr {cos($beta * acos(0) / 90.)}]
    set CG [expr {cos($gamma * acos(0) / 90.)}]
    set SA [expr {sin($alpha * acos(0) / 90.)}]
    set SB [expr {sin($beta * acos(0) / 90.)}]
    set SC [expr {sin($gamma * acos(0) / 90.)}]
    set CASTAR [expr { ($CB*$CG-$CA)/($SB*$SC) }]    ;#! cos(Alpha*)
    set CBSTAR [expr { ($CA*$CG-$CB)/($SA*$SC) }]    ;#! cos(Beta*)
    set CCSTAR [expr { ($CA*$CB-$CG)/($SA*$SB) }]    ;#! cos(Gamma*)
    set SASTAR [expr { sqrt(max(0.0,1.0-$CASTAR**2)) }]    ;#! sin(Alpha*)
    set SBSTAR [expr { sqrt(max(0.0,1.0-$CBSTAR**2)) }]    ;#! sin(Beta*)
    set SCSTAR [expr { sqrt(max(0.0,1.0-$CCSTAR**2)) }]    ;#! sin(Gamma*)

    set A  "2 3 3      $a 0 0    0 $b 0    0 0 $c"
    set A1 "2 3 3     1.0 0 0    $CG [expr {$SASTAR*$SC}] [expr {-$CASTAR*$SC}]    $CB 0.0 $SB"
    #!This matrix is
    #!   (1.0      0.0            0.0             )
    #!   (cos(G)  sin(A*)*sin(G) -cos(A*)*sin(G)  )
    #!   (cos(B)   0.0            sin(B)          )
    return [La::transpose [La::mmult $A $A1]]
}

# compute the transformation matrix that converts a rigid body coordinates into fractional 
# coordinates
# arguments: 
#   rotations: a list of axes and angles to rotate: { {axis1 angle1} {axis2 angle2} ...} 
#              where axis1,... can be 1, 2 or 3 corresponding to the cartesian X, Y or Z axes
#   cellprms: a list with "a b c alpha beta gamma" in Angstroms and degrees
# returns a list that can be used as a matrix in the La package
proc CalcXformMatrix {rotations cellprms} {
    set prod {}
    foreach item $rotations {
	#puts $item
	set mat [eval RotMatrix $item]
	if {$prod == ""} {
	    set prod $mat
	} else {
	    set prod [La::mmult $prod $mat]
	}
    }
    #puts "--- rotation product ---"
    #puts [La::show $prod]

    set ortho [eval OrthoMatrix $cellprms]
    #puts "--- ortho ---"
    #puts [La::show $ortho]
    set deortho [La::msolve $ortho [La::mident 3] ]
    #puts "--- deortho ---"
    #puts [La::show $deortho]
    #puts "--- xform ---"
    set xform [La::mmult $deortho $prod]
    return $xform
}

# transforms a triplet of orthogonal coordinates into fractional ones using 
# arguments: 
#    xform: a transformation matrix from CalcXformMatrix
#    origin: a list of fraction coordinates {x y z} describing the location of the 
#            origin of the orthogonal coordinates in the crystal system
#    ortho: a triplet of othogonal coordinates
# returns a triplet of fractional coordinates
proc Ortho2Xtal {xform origin ortho} {
    set ocv "2 3 0 $ortho"
    set frac [La::mmult $xform $ocv]
    #puts [La::show [La::transpose $frac]]
    #puts $frac
    set frac [La::madd $frac "[lrange $frac 0 2] $origin"]
    #puts [La::show [La::transpose $frac]]
    return $frac
}

# compute the derivative of the transformation matrix (see CalcXformMatrix) 
# with respect to every rotation in the $rotations list
# arguments: 
#   rotations: a list of axes and angles to rotate: { {axis1 angle1} {axis2 angle2} ...} 
#              where axis1,... can be 1, 2 or 3 corresponding to the cartesian X, Y or Z axes
#   cellprms: a list with "a b c alpha beta gamma" in Angstroms and degrees
# returns a list of matrices where each matrix is a list that can be used as a 
# matrix in the La package
proc CalcDerivMatrix {rotations cellprms} {
    set ortho [eval OrthoMatrix $cellprms]
    set deortho [La::msolve $ortho [La::mident 3] ]
    set derivlist {}

    foreach item $rotations {
	#puts $item
	set mat [eval DerivRotMatrix $item]
	#puts $item
	#puts [La::show $mat]
	set xform [La::mmult $deortho $mat]
	lappend derivlist $xform
    }
    return $derivlist
}

# fit a rigid body's origin 
# arguments: 
#  Euler: a list of axes and angles to rotate: { {axis1 angle1} {axis2 angle2} ...} 
#              where axis1,... can be 1, 2 or 3 corresponding to the cartesian X, Y or Z axes
#  cell: a list with "a b c alpha beta gamma" in Angstroms and degrees
#  ortholist: list containing triplets with orthogonal coordinates 
#  useflag: list of flags to indicate if an atom should be used (1) or ignored (0)
#  fraclist: list containing triplets with fractional coordinates  
#  origin: a list of fraction coordinates {x y z} describing the location of the 
#            origin of the orthogonal coordinates in the crystal system
#     note that the length of ortholist, useflag and fraclist should be the same
# Returns a list with the following elements
#   0: a list with three new origin values
#   1: the root-mean square difference between the fraclist coordinates and those computed 
#      using the input values for those atoms where $use is one (in Angstroms)
#   2: the computed fractional coordinates for all atoms
#   3: individual rms values for all atoms (in Angstroms)
# note that items 1-3 are computed with the imput origin, not the revised one
proc FitBodyOrigin {Euler cell ortholist useflag fraclist origin} {
    set xform [CalcXformMatrix $Euler $cell]
    foreach var {x y z} {set sum($var) 0.0}

    set i 0
    set sumdvs 0
    set fracout {}
    set rmsout {}
    foreach oc $ortholist use $useflag coord $fraclist {
	#puts "ortho: $oc"
	set frac [lrange [Ortho2Xtal $xform $origin $oc] 3 end]
	lappend fracout $frac
	if {$use} {incr i}
	set dvs 0
	foreach var {x y z} v1 $frac v2 $coord abc [lrange $cell 0 2] {
	    set dv [expr {($v2 - $v1)}]
	    set dvs [expr {$dvs + $dv**2}]
	    set sumdvs [expr {$sumdvs + $dv**2}]
	    if {$use} {set sum($var) [expr {$sum($var) + $dv}]}
	}
	lappend rmsout [expr {sqrt($dvs)}]
    }
    set rms 0
    if {$i > 1} {set rms [expr {sqrt($sumdvs)/$i}]}
    set neworig {}
    foreach var {x y z} o $origin {
	lappend neworig [expr {$o + ($sum($var)/$i)}]
    }
    return [list $neworig $rms $fracout $rmsout]
}

# fit a rigid body's Euler angles using least-squares
# arguments: 
#  Euler: a list of axes and angles to rotate: { {axis1 angle1} {axis2 angle2} ...} 
#              where axis1,... can be 1, 2 or 3 corresponding to the cartesian X, Y or Z axes
#  cell: a list with "a b c alpha beta gamma" in Angstroms and degrees
#  ortholist: list containing triplets with orthogonal coordinates 
#  useflag: list of flags to indicate if an atom should be used (1) or ignored (0)
#  fraclist: list containing triplets with fractional coordinates  
#  origin: a list of fraction coordinates {x y z} describing the location of the 
#            origin of the orthogonal coordinates in the crystal system
#     note that the length of ortholist, useflag and fraclist should be the same
# Returns a list of new Euler angles 
proc FitBodyRot {Euler cell ortholist useflag fraclist origin} {
    set xform [CalcXformMatrix $Euler  $cell]
    set derivlist [CalcDerivMatrix $Euler  $cell]
    set A "2 [expr 3*[llength $ortholist]] 3"
    foreach oc $ortholist use $useflag coord $fraclist {
	if {! $use} continue
	foreach deriv $derivlist {
	    foreach xyz [lrange [Ortho2Xtal $deriv "0 0 0" $oc] 3 end] {
		lappend A $xyz
	    }
	}
    }
    #puts "A: [La::show $A]"
    set y "2 [expr 3*[llength $ortholist]] 1"
    foreach oc $ortholist use $useflag coord $fraclist {
	if {! $use} continue
	set frac [lrange [Ortho2Xtal $xform $origin $oc] 3 end]
	foreach xyz $coord XYZ $frac {
	    lappend y [expr {$XYZ - $xyz}]
	}
    }

    set AtA [La::mmult [La::transpose $A] $A]
    set Aty [La::mmult [La::transpose $A] $y]
    
    set l {}
    #set shifts {}
    foreach delta [lrange [La::msolve $AtA $Aty] 3 end] item $Euler {
	#lappend shifts $delta
	lappend l "[lindex $item 0] [expr {$delta + [lindex $item 1]}]"
    }
    #puts "shifts = $shifts"
    return $l
}

# fit a rigid body's Origin and Euler angles
# arguments: 
#  Euler: a list of axes and angles to rotate: { {axis1 angle1} {axis2 angle2} ...} 
#              where axis1,... can be 1, 2 or 3 corresponding to the cartesian X, Y or Z axes
#  cell: a list with "a b c alpha beta gamma" in Angstroms and degrees
#  ortholist: list containing triplets with orthogonal coordinates 
#  useflag: list of flags to indicate if an atom should be used (1) or ignored (0)
#  fraclist: list containing triplets with fractional coordinates  
#  origin: a list of fraction coordinates {x y z} describing the location of the 
#            origin of the orthogonal coordinates in the crystal system
#     note that the length of ortholist, useflag and fraclist should be the same
# Returns a list containing 
#   new origin
#   new Euler angles
#   total rms 
#   fractional coordinates
#   rms deviation in fractional coordinates of new Euler angles 
proc FitBody {Euler cell ortholist useflag fraclist origin "ncycle 5"} {
    puts "start origin = $origin"
    foreach {
	origin 
	startrms 
	fracout 
	rmsout } [FitBodyOrigin $Euler $cell $ortholist $useflag $fraclist $origin] {}
    puts "start rms = $startrms"
    set rmsprev $startrms
    puts "new origin = $origin"
    for {set i 0} {$i < $ncycle} {incr i} {
	set Eulerprev $Euler
	set Euler [FitBodyRot $Euler $cell $ortholist $useflag $fraclist $origin]
	puts "New Euler $Euler"
	puts "after fit" 
	foreach {
	    origin 
	    rms 
	    fracout 
	    rmsout } [FitBodyOrigin $Euler $cell $ortholist $useflag $fraclist $origin] {}
	if {$rms > (1.1 * $rmsprev) + 0.01} {
	    puts "rms = $rms, new origin = $origin"
	    set rmsprev $rms
	}
    }	
    #proc FitBodyOrigin {Euler cell ortholist useflag fraclist origin} 
    #return "$neworig $rms $fracout $rmsout"
    set fmt  {"%8.5f %8.5f %8.5f     %8.5f %8.5f %8.5f   %6.3f"}
    foreach fracin $fraclist fraccalc $fracout rmsi $rmsout {
	puts "[eval format $fmt $fracin $fraccalc $rmsi]"
    }
    return [list $origin $Euler $rms $fracout $rmsout]
}

# zmat2coord converts a z-matrix to a set of cartesian coordinates
#   a z-matrix is also known as "internal coordinates" or "torsion space"
#   (see Journal of Computational Chemistry, Vol 26, #10, p. 1063–1068, 2005 or
#    http://www.cmbi.ru.nl/molden/zmat/zmat.html)
# INPUT:
#   atmlist is a list of ascii lines where each line contains
#     lbl c1 distance c2 angle c3 torsion
#   where each atom is computed from the previous where the new atom is: 
#     distance $distance from atom $c1 (angstrom)
#     angle $angle from $c1--$c2 (degrees)
#     torsion $torsion from $c1--$c2--$c3 (degrees)
# OUTPUT:
#  zmat2coord returns a list of atom labels and cartesian coordinates, 
#  with 4 items in each element (label, x, y, z)
# this routine was tested against results from Babel via the web interface at 
# http://www.shodor.org/chemviz/zmatrices/babel.html and sample input at 
# http://iopenshell.usc.edu/howto/zmatrix/
proc zmat2coord {atmlist} { 
    set torad [expr {acos(0)/90.}]
    set i 0
    foreach line $atmlist {
	incr i
	foreach {lbl c1 dist c2 angle c3 torsion} $line {} 
	if {$i == 1} {
	    set atm(1) [list $lbl 0 0 0] ; # 1st atom is at origin
	} elseif {$i == 2} {
	    set dist1 $dist
	    set atm(2) [list $lbl $dist1 0 0] ; # 2nd atom is along x-axis
	} elseif {$i == 3} {
	    # 3rd atom can be bonded to the 1st or 2nd
	    if {$c1 == 1} {
		set atm(3) [list $lbl \
				[expr {$dist * cos($torad * $angle)}] \
				[expr {$dist * sin($torad * $angle)}] \
				0]
	    } else {
		set atm(3) [list $lbl \
				[expr {$dist1 - $dist * cos($torad * $angle)}] \
				[expr {$dist * sin($torad * $angle)}] \
				0]
	    }
	} else {
	    set atm($i) [concat $lbl \
			     [ahcat "atm" $c1 $dist $c2 $angle $c3 $torsion]]
	}
    }
    set coordlist {}
    foreach key [lsort -integer [array names atm]] {
	lappend coordlist $atm($key)
    }
    return $coordlist
}
# Compute the length of a vector
proc vlen {a} {
    set sum 0.0
    foreach ai $a {
	set sum [expr {$sum + $ai*$ai}]
    }
    return [expr sqrt($sum)]
}
# compute vector (a + z * b) and optionally normalize to length d
proc vadd {a b d z} {
    set c {}
    foreach ai $a bi $b {
        lappend c [expr {$bi + $z * $ai}]
    }
    set v [vlen $c]
    if {$d != 0} {
	set r {}
	foreach ci $c {
	    lappend r [expr {$d * $ci / $v}]
	}
	return [list $v $r]
    }
    return [list $v $c]
}
# normalize a vector
proc vnrm {x} {
    set v [vlen $x]
    if {abs($v) < 1e-8} {return [list 0 0 0]}
    set y {}
    foreach xi $x {
	lappend y [expr {$xi / $v}]
    }
    return $y
}
# compute the coordinates for an atom that is bonded:
#   distance $dist from atom $nc
#   angle $bondang from $nc--$nb
#   torsion $torsang from $nc--$nb--$na
#   coordinates are found in array $atmarr in the calling routine
# based on a Fortran routine provided by Peter Zavalij (Thanks Peter!)
proc ahcat {atmarr nc dist nb bondang na torsang} {
    upvar 1 $atmarr atm
    set xa [lrange $atm($na) 1 3]
    set xb [lrange $atm($nb) 1 3]
    set xc [lrange $atm($nc) 1 3]
    set torad [expr {acos(0)/90.}]
    # x = unit Vector A-B
    foreach {x1 x2 x3} [lindex [vadd $xb $xa 1. -1.] 1] {}
    # y = unit Vector C-B
    set y [lindex [vadd $xb $xc 1. -1.] 1]
    foreach {y1 y2 y3} $y {}
    set z1 [expr {$x2*$y3 - $x3*$y2}]
    set z2 [expr {$x3*$y1 - $x1*$y3}]
    set z3 [expr {$x1*$y2 - $x2*$y1}]
    set z [vnrm [list $z1 $z2 $z3]]
    set q1 [expr {$y2*$z3 - $y3*$z2}]
    set q2 [expr {$y3*$z1 - $y1*$z3}]
    set q3 [expr {$y1*$z2 - $y2*$z1}]
    set q [vnrm [list $q1 $q2 $q3]]
    set th [expr {$bondang * $torad}]
    set ph [expr {-1. * $torsang * $torad}]
    set cth [expr {cos($th)}]
    set sth [expr {sin($th)}]
    set cph [expr {cos($ph)}]
    set sph [expr {sin($ph)}]
    set xh {}
    foreach xci $xc xi $q zi $z yi $y {
	lappend xh [expr {
			  $xci +
			  $dist*($sth*($cph*$xi + $sph*$zi)-$cth*$yi)
		      }]
    }
    return $xh
}
 


#AddRigidBody {1} { {{0 0 0 xe} {1 1 1 o} {2 2 2 si+4}} }
#puts [GetRB 1 6 8 "1 2" "X 1 2" "Y 1 3"]
#puts [GetRB 1 4 8 "1" "X 1 2" "Z 3 4"]
#ApplyRigidBody 1 1 7 ".11 .22 .33" "11 12 13"


#AddRigidBody {1} { {
#    {1 1 1 o} {-1 1 1 o} {1 -1 1 o} {-1 -1 1 o} 
#    {1 1 -1 o} {-1 1 -1 o} {1 -1 -1 o} {-1 -1 -1 o} 
#} }
#set n [ApplyRigidBody 1 1 1 ".2 .3 .4" "13 17 19"]
#puts "body $n created"
#incr expgui(changed)
#RunRecalcRBCoords
#puts "press Enter to continue"
#gets stdin line
#ApplyRigidBody 1 1 $n ".5 .5 .5" "0 0 0"
#incr expgui(changed)
#RunRecalcRBCoords

puts "Test FitBody"
set fraclist {
    { 0.5483305238484277 0.4887545024531055 0.6167996784631056 }
    { 0.1036801409356145 0.5954016321779562 0.5129448102437683 }
    { 0.26404665760133855 0.09455414439078394 0.612655365147539 }
    { -0.18060372531147473 0.20120127411563465 0.5088004969282018 }
    { 0.5806037253114747 0.3987987258843653 0.2911995030717982 }
    { 0.13595334239866147 0.5054458556092161 0.18734463485246095 }
    { 0.2963198590643855 0.004598367822043814 0.2870551897562318 }
    { -0.1483305238484277 0.1112454975468945 0.1832003215368945 }
}
set ortholist {
    {1 1 1} 
    {-1 1 1}
    {      1.000000   -1.000000    1.000000}
    {     -1.000000   -1.000000    1.000000}
    {      1.000000    1.000000   -1.000000}
    {     -1.000000    1.000000   -1.000000}
    {      1.000000   -1.000000   -1.000000}
    {     -1.000000   -1.000000   -1.000000} 
}

set useflag {1 1 1 1 1 1 1 1}
set cell {4.  5.  6.  95.  100.  105.}
#set origin ".20 .30 .40"
set origin ".0 .0 .0"
#set Euler  {{1 13} {2 17} {3 19}}
#set Euler  {{1 0} {2 180} {3 0}}
set Euler  {{1 0} {2 0} {3 0}}

#puts [La::show $xform]
puts "out: [FitBody $Euler $cell $ortholist $useflag $fraclist $origin 30]"


# test zmat2coord
set atmlist {
    {C1 0 0.0 0 0.0 0 0.0}
    {O2 1 1.20 0 0.0 0 0.0}
    {H3 1 1.10 2 120.0 0 0.0}
    {C4 1 1.50 2 120.0 3 180.0}
    {H5 4 1.10 1 110.0 2 0.00}
    {H6 4 1.10 1 110.0 2 120.0}
    {H7 4 1.10 1 110.0 2 -120.0}
}
#  C        0.00000        0.00000        0.00000
#  O        1.20000        0.00000        0.00000
#  H       -0.55000        0.95263        0.00000
#  C       -0.75000       -1.29904       -0.00000
#  H       -0.04293       -2.14169       -0.00000
#  H       -1.38570       -1.36644        0.89518
#  H       -1.38570       -1.36644       -0.89518
set coordlist [zmat2coord $atmlist]
set i 0
puts "\nZmatrix in"
foreach line $atmlist {
    incr i
    puts "$i) $line"
}
puts "Cartesian out"
foreach line $coordlist {
    puts [eval format "%-4s%10.5f%10.5f%10.5f" $line]
}