//Next Reads the next frame in a XTCObj that has been initialized for read
//With initread. If keep is true, returns a pointer to matrix.DenseMatrix
//With the coordinates read, otherwiser, it discards the coordinates and
//returns nil.
func (X *XTCObj) Next(output *v3.Matrix) error {
	if !X.Readable() {
		return Error{TrajUnIni, X.filename, []string{"Next"}, true}
	}
	cnatoms := C.int(X.natoms)
	worked := C.get_coords(X.fp, &X.cCoords[0], cnatoms)
	if worked == 11 {
		X.readable = false
		return newlastFrameError(X.filename, "Next") //This is not really an error and should be catched in the calling function
	}
	if worked != 0 {
		X.readable = false
		return Error{ReadError, X.filename, []string{"Next"}, true}
	}
	if output != nil { //col the frame
		r, c := output.Dims()
		if r < (X.natoms) {
			panic("Buffer v3.Matrix too small to hold trajectory frame")
		}
		for j := 0; j < r; j++ {
			for k := 0; k < c; k++ {
				l := k + (3 * j)
				output.Set(j, k, (10 * float64(X.cCoords[l]))) //nm to Angstroms
			}
		}
		return nil
	}
	return nil //Just drop the frame
}

//BestPlane returns a row vector that is normal to the plane that best contains the molecule
//if passed a nil Masser, it will simply set all masses to 1.
func BestPlane(coords *v3.Matrix, mol Masser) (*v3.Matrix, error) {
	var err error
	var Mmass []float64
	cr, _ := coords.Dims()
	if mol != nil {
		Mmass, err = mol.Masses()
		if err != nil {
			return nil, errDecorate(err, "BestPlane")
		}
		if len(Mmass) != cr {
			return nil, CError{fmt.Sprintf("Inconsistent coordinates(%d)/atoms(%d)", len(Mmass), cr), []string{"BestPlane"}}
		}
	}
	moment, err := MomentTensor(coords, Mmass)
	if err != nil {
		return nil, errDecorate(err, "BestPlane")
	}
	evecs, _, err := v3.EigenWrap(moment, appzero)
	if err != nil {
		return nil, errDecorate(err, "BestPlane")
	}
	normal, err := BestPlaneP(evecs)
	if err != nil {
		return nil, errDecorate(err, "BestPlane")
	}
	//MomentTensor(, mass)
	return normal, err
}

//MomentTensor returns the moment tensor for a matrix A of coordinates and a column
//vector mass with the respective massess.
func MomentTensor(A *v3.Matrix, massslice []float64) (*v3.Matrix, error) {
	ar, ac := A.Dims()
	var err error
	var mass *mat64.Dense
	if massslice == nil {
		mass = gnOnes(ar, 1)
	} else {
		mass = mat64.NewDense(ar, 1, massslice)
		//		if err != nil {
		//			return nil, err
		//		}
	}
	center, _, err := MassCenter(A, v3.Dense2Matrix(gnCopy(A)), mass)
	if err != nil {
		return nil, errDecorate(err, "MomentTensor")
	}
	sqrmass := gnZeros(ar, 1)
	//	sqrmass.Pow(mass,0.5)
	pow(mass, sqrmass, 0.5) //the result is stored in sqrmass
	//	fmt.Println(center,sqrmass) ////////////////////////
	center.ScaleByCol(center, sqrmass)
	//	fmt.Println(center,"scaled center")
	centerT := gnZeros(ac, ar)
	centerT.Copy(center.T())
	moment := gnMul(centerT, center)
	return v3.Dense2Matrix(moment), nil
}

//MassCenter centers in in the center of mass of oref. Mass must be
//A column vector. Returns the centered matrix and the displacement matrix.
func MassCenter(in, oref *v3.Matrix, mass *mat64.Dense) (*v3.Matrix, *v3.Matrix, error) {
	or, _ := oref.Dims()
	ir, _ := in.Dims()
	if mass == nil { //just obtain the geometric center
		tmp := ones(or)
		mass = mat64.NewDense(or, 1, tmp) //gnOnes(or, 1)
	}
	ref := v3.Zeros(or)
	ref.Copy(oref)
	gnOnesvector := gnOnes(1, or)
	f := func() { ref.ScaleByCol(ref, mass) }
	if err := gnMaybe(gnPanicker(f)); err != nil {
		return nil, nil, CError{err.Error(), []string{"v3.Matrix.ScaleByCol", "MassCenter"}}
	}
	ref2 := v3.Zeros(1)
	g := func() { ref2.Mul(gnOnesvector, ref) }
	if err := gnMaybe(gnPanicker(g)); err != nil {
		return nil, nil, CError{err.Error(), []string{"v3.gOnesVector", "MassCenter"}}
	}
	ref2.Scale(1.0/mass.Sum(), ref2)
	returned := v3.Zeros(ir)
	returned.Copy(in)
	returned.SubVec(returned, ref2)
	/*	for i := 0; i < ir; i++ {
			if err := returned.GetRowVector(i).Subtract(ref2); err != nil {
				return nil, nil, err
			}
		}
	*/
	return returned, ref2, nil
}

//PDBStringWrite writes a string in PDB format for a given reference, coordinate set and bfactor set, which must match each other
//returns the written string and error or nil.
func PDBStringWrite(coords *v3.Matrix, mol Atomer, bfact []float64) (string, error) {
	if bfact == nil {
		bfact = make([]float64, mol.Len())
	}
	cr, _ := coords.Dims()
	br := len(bfact)
	if cr != mol.Len() || cr != br {
		return "", CError{"Ref and Coords and/or Bfactors dont have the same number of atoms", []string{"PDBStringWrite"}}
	}
	chainprev := mol.Atom(0).Chain //this is to know when the chain changes.
	var outline string
	var outstring string
	var err error
	for i := 0; i < mol.Len(); i++ {
		//	r,c:=coords.Dims()
		//	fmt.Println("IIIIIIIIIIIi", i,coords,r,c, "lllllll")
		writecoord := coords.VecView(i)
		outline, chainprev, err = writePDBLine(mol.Atom(i), writecoord, bfact[i], chainprev)
		if err != nil {
			return "", errDecorate(err, "PDBStringWrite "+fmt.Sprintf("Could not print PDB line: %d", i))
		}
		outstring = strings.Join([]string{outstring, outline}, "")
	}
	outstring = strings.Join([]string{outstring, "END\n"}, "")
	return outstring, nil
}

func pdbWrite(out io.Writer, coords *v3.Matrix, mol Atomer, bfact []float64) error {
	if bfact == nil {
		bfact = make([]float64, mol.Len())
	}
	cr, _ := coords.Dims()
	br := len(bfact)
	if cr != mol.Len() || cr != br {
		return CError{"Ref and Coords and/or Bfactors dont have the same number of atoms", []string{"pdbWrite"}}
	}
	chainprev := mol.Atom(0).Chain //this is to know when the chain changes.
	var outline string
	var err error
	iowriteError := func(err error) error {
		return CError{"Failed to write in io.Writer" + err.Error(), []string{"io.Write.Write", "pdbWrite"}}
	}
	for i := 0; i < mol.Len(); i++ {
		//	r,c:=coords.Dims()
		//	fmt.Println("IIIIIIIIIIIi", i,coords,r,c, "lllllll")
		writecoord := coords.VecView(i)
		outline, chainprev, err = writePDBLine(mol.Atom(i), writecoord, bfact[i], chainprev)
		if err != nil {
			return errDecorate(err, "pdbWrite "+fmt.Sprintf("Could not print PDB line: %d", i))
		}
		_, err := out.Write([]byte(outline))
		if err != nil {
			return iowriteError(err)
		}
	}
	_, err = out.Write([]byte("END")) //no newline, this is in case the write is part of a PDB and one needs to write "ENDMODEL".
	if err != nil {
		return iowriteError(err)
	}
	return nil
}

//ScaleBond takes a C-H bond and moves the H (in place) so the distance between them is the one given (bond).
//CAUTION: I have only tested it for the case where the original distance>bond, although I expect it to also work in the other case.
func ScaleBond(C, H *v3.Matrix, bond float64) {
	Odist := v3.Zeros(1)
	Odist.Sub(H, C)
	distance := Odist.Norm(0)
	Odist.Scale((distance-bond)/distance, Odist)
	H.Sub(H, Odist)
}

//Projection returns the projection of test in ref.
func Projection(test, ref *v3.Matrix) *v3.Matrix {
	rr, _ := ref.Dims()
	Uref := v3.Zeros(rr)
	Uref.Unit(ref)
	scalar := test.Dot(Uref) //math.Abs(la)*math.Cos(angle)
	Uref.Scale(scalar, Uref)
	return Uref
}

//BestPlaneP takes sorted evecs, according to the eval,s and returns a row vector that is normal to the
//Plane that best contains the molecule. Notice that the function can't possibly check
//that the vectors are sorted. The P at the end of the name is for Performance. If
//That is not an issue it is safer to use the BestPlane function that wraps this one.
func BestPlaneP(evecs *v3.Matrix) (*v3.Matrix, error) {
	evr, evc := evecs.Dims()
	if evr != 3 || evc != 3 {
		return evecs, CError{"goChem: Eigenvectors matrix must be 3x3", []string{"BestPlaneP"}} //maybe this should be a panic
	}
	v1 := evecs.VecView(2)
	v2 := evecs.VecView(1)
	normal := v3.Zeros(1)
	normal.Cross(v1, v2)
	return normal, nil
}

func EncodeCoords(coords *v3.Matrix, enc *json.Encoder) *Error {
	c := new(Coords)
	t := make([]float64, 3, 3)
	for i := 0; i < coords.NVecs(); i++ {
		c.Coords = coords.Row(t, i)
		if err := enc.Encode(c); err != nil {
			return NewError("postprocess", "chemjson.EncodeCoords", err)
		}
	}
	return nil
}

//SelCone, Given a set of cartesian points in sellist, obtains a vector "plane" normal to the best plane passing through the points.
//It selects atoms from the set A that are inside a cone in the direction of "plane" that starts from the geometric center of the cartesian points,
//and has an angle of angle (radians), up to a distance distance. The cone is approximated by a set of radius-increasing cilinders with height thickness.
//If one starts from one given point, 2 cgnOnes, one in each direction, are possible. If whatcone is 0, both cgnOnes are considered.
//if whatcone<0, only the cone opposite to the plane vector direction. If whatcone>0, only the cone in the plane vector direction.
//the 'initial' argument  allows the construction of a truncate cone with a radius of initial.
func SelCone(B, selection *v3.Matrix, angle, distance, thickness, initial float64, whatcone int) []int {
	A := v3.Zeros(B.NVecs())
	A.Copy(B) //We will be altering the input so its better to work with a copy.
	ar, _ := A.Dims()
	selected := make([]int, 0, 3)
	neverselected := make([]int, 0, 30000)     //waters that are too far to ever be selected
	nevercutoff := distance / math.Cos(angle)  //cutoff to be added to neverselected
	A, _, err := MassCenter(A, selection, nil) //Centrate A in the geometric center of the selection, Its easier for the following calculations
	if err != nil {
		panic(PanicMsg(err.Error()))
	}
	selection, _, _ = MassCenter(selection, selection, nil) //Centrate the selection as well
	plane, err := BestPlane(selection, nil)                 //I have NO idea which direction will this vector point. We might need its negative.
	if err != nil {
		panic(PanicMsg(err.Error()))
	}
	for i := thickness / 2; i <= distance; i += thickness {
		maxdist := math.Tan(angle)*i + initial //this should give me the radius of the cone at this point
		for j := 0; j < ar; j++ {
			if isInInt(selected, j) || isInInt(neverselected, j) { //we dont scan things that we have already selected, or are too far
				continue
			}
			atom := A.VecView(j)
			proj := Projection(atom, plane)
			norm := proj.Norm(0)
			//Now at what side of the plane is the atom?
			angle := Angle(atom, plane)
			if whatcone > 0 {
				if angle > math.Pi/2 {
					continue
				}
			} else if whatcone < 0 {
				if angle < math.Pi/2 {
					continue
				}
			}
			if norm > i+(thickness/2.0) || norm < (i-thickness/2.0) {
				continue
			}
			proj.Sub(proj, atom)
			projnorm := proj.Norm(0)
			if projnorm <= maxdist {
				selected = append(selected, j)
			}
			if projnorm >= nevercutoff {
				neverselected = append(neverselected, j)
			}
		}
	}
	return selected
}

//XYZStringWrite writes the mol Ref and the Coord coordinates in an XYZ-formatted string.
func XYZStringWrite(Coords *v3.Matrix, mol Atomer) (string, error) {
	var out string
	if mol.Len() != Coords.NVecs() {
		return "", CError{"Ref and Coords dont have the same number of atoms", []string{"XYZStringWrite"}}
	}
	c := make([]float64, 3, 3)
	out = fmt.Sprintf("%-4d\n\n", mol.Len())
	//towrite := Coords.Arrays() //An array of array with the data in the matrix
	for i := 0; i < mol.Len(); i++ {
		//c := towrite[i] //coordinates for the corresponding atoms
		c = Coords.Row(c, i)
		temp := fmt.Sprintf("%-2s  %12.6f%12.6f%12.6f \n", mol.Atom(i).Symbol, c[0], c[1], c[2])
		out = strings.Join([]string{out, temp}, "")
	}
	return out, nil
}

//ScaleBonds scales all bonds between atoms in the same residue with names n1, n2 to a final lenght finallengt, by moving the atoms n2.
//the operation is executed in place.
func ScaleBonds(coords *v3.Matrix, mol Atomer, n1, n2 string, finallenght float64) {
	for i := 0; i < mol.Len(); i++ {
		c1 := mol.Atom(i)
		if c1.Name != n1 {
			continue
		}
		for j := 0; j < mol.Len(); j++ {
			c2 := mol.Atom(j)
			if c1.MolID == c2.MolID && c1.Name == n1 && c2.Name == n2 {
				A := coords.VecView(i)
				B := coords.VecView(j)
				ScaleBond(A, B, finallenght)
			}
		}
	}
}

//CenterOfMass returns the center of mass the atoms represented by the coordinates in geometry
//and the masses in mass, and an error. If mass is nil, it calculates the geometric center
func CenterOfMass(geometry *v3.Matrix, mass *mat64.Dense) (*v3.Matrix, error) {
	if geometry == nil {
		return nil, CError{"goChem: nil matrix to get the center of mass", []string{"CenterOfMass"}}
	}
	gr, _ := geometry.Dims()
	if mass == nil { //just obtain the geometric center
		tmp := ones(gr)
		mass = mat64.NewDense(gr, 1, tmp) //gnOnes(gr, 1)
	}
	tmp2 := ones(gr)
	gnOnesvector := mat64.NewDense(1, gr, tmp2) //gnOnes(1, gr)

	ref := v3.Zeros(gr)
	ref.ScaleByCol(geometry, mass)
	ref2 := v3.Zeros(1)
	ref2.Mul(gnOnesvector, ref)
	ref2.Scale(1.0/mass.Sum(), ref2)
	return ref2, nil
}

//writePDBLine writes a line in PDB format from the data passed as a parameters. It takes the chain of the previous atom
//and returns the written line, the chain of the just-written atom, and error or nil.
func writePDBLine(atom *Atom, coord *v3.Matrix, bfact float64, chainprev string) (string, string, error) {
	var ter string
	var out string
	if atom.Chain != chainprev {
		ter = fmt.Sprint(out, "TER\n")
		chainprev = atom.Chain
	}
	first := "ATOM"
	if atom.Het {
		first = "HETATM"
	}
	formatstring := "%-6s%5d  %-3s %-4s%1s%4d    %8.3f%8.3f%8.3f%6.2f%6.2f          %2s  \n"
	//4 chars for the atom name are used when hydrogens are included.
	//This has not been tested
	if len(atom.Name) == 4 {
		formatstring = strings.Replace(formatstring, "%-3s ", "%-4s", 1)
	} else if len(atom.Name) > 4 {
		return "", chainprev, CError{"Cant print PDB line", []string{"writePDBLine"}}
	}
	//"%-6s%5d  %-3s %3s %1c%4d    %8.3f%8.3f%8.3f%6.2f%6.2f          %2s  \n"
	out = fmt.Sprintf(formatstring, first, atom.ID, atom.Name, atom.Molname, atom.Chain,
		atom.MolID, coord.At(0, 0), coord.At(0, 1), coord.At(0, 2), atom.Occupancy, bfact, atom.Symbol)
	out = strings.Join([]string{ter, out}, "")
	return out, chainprev, nil
}

//EulerRotateAbout uses Euler angles to rotate the coordinates in coordsorig around by angle
//radians around the axis given by the vector axis. It returns the rotated coordsorig,
//since the original is not affected. It seems more clunky than the RotateAbout, which uses Clifford algebra.
//I leave it for benchmark, mostly, and might remove it later. There is no test for this function!
func EulerRotateAbout(coordsorig, ax1, ax2 *v3.Matrix, angle float64) (*v3.Matrix, error) {
	r, _ := coordsorig.Dims()
	coords := v3.Zeros(r)
	translation := v3.Zeros(ax1.NVecs())
	translation.Copy(ax1)
	axis := v3.Zeros(ax2.NVecs())
	axis.Sub(ax2, ax1) //now it became the rotation axis
	f := func() { coords.SubVec(coordsorig, translation) }
	if err := gnMaybe(gnPanicker(f)); err != nil {
		return nil, CError{err.Error(), []string{"v3.Matrix.Subvec", "EulerRotateAbout"}}

	}
	Zswitch := RotatorToNewZ(axis)
	coords.Mul(coords, Zswitch) //rotated
	Zrot, err := RotatorAroundZ(angle)
	if err != nil {
		return nil, errDecorate(err, "EulerRotateAbout")
	}
	//	Zsr, _ := Zswitch.Dims()
	//	RevZ := v3.Zeros(Zsr)
	RevZ, err := gnInverse(Zswitch)
	if err != nil {
		return nil, errDecorate(err, "EulerRotateAbout")
	}
	coords.Mul(coords, Zrot) //rotated
	coords.Mul(coords, RevZ)
	coords.AddVec(coords, translation)
	return coords, nil
}

//Super determines the best rotation and translations to superimpose the coords in test
//listed in testlst on te atoms of molecule templa, frame frametempla, listed in templalst.
//It applies those rotation and translations to the whole frame frametest of molecule test, in palce.
//testlst and templalst must have the same number of elements. If any of the two slices, or both, are
//nil or have a zero lenght, they will be replaced by a list containing the number of all atoms in the
//corresponding molecule.
func Super(test, templa *v3.Matrix, testlst, templalst []int) (*v3.Matrix, error) {
	//_, testcols := test.Dims()
	//_, templacols := templa.Dims()
	structs := []*v3.Matrix{test, templa}
	lists := [][]int{testlst, templalst}
	//In case one or both lists are nil or have lenght zero.
	for k, v := range lists {
		if v == nil || len(v) == 0 {
			lists[k] = make([]int, structs[k].NVecs(), structs[k].NVecs())
			for k2, _ := range lists[k] {
				lists[k][k2] = k2
			}
		}
	}
	//fmt.Println(lists[0])
	if len(lists[0]) != len(lists[1]) {
		return nil, CError{fmt.Sprintf("Mismatched template and test atom numbers: %d, %d", len(lists[1]), len(lists[0])), []string{"Super"}}
	}
	ctest := v3.Zeros(len(lists[0]))
	ctest.SomeVecs(test, lists[0])
	ctempla := v3.Zeros(len(lists[1]))
	ctempla.SomeVecs(templa, lists[1])
	_, rotation, trans1, trans2, err1 := RotatorTranslatorToSuper(ctest, ctempla)
	if err1 != nil {
		return nil, errDecorate(err1, "Super")
	}
	test.AddVec(test, trans1)
	//	fmt.Println("test1",test, rotation) /////////////77
	test.Mul(test, rotation)
	//	fmt.Println("test2",test) ///////////
	test.AddVec(test, trans2)
	//	fmt.Println("test3",test) ///////
	return test, nil
}

//XYZStringWrite writes the mol Ref and the Coord coordinates in an XYZ-formatted string.
func XYZWrite(out io.Writer, Coords *v3.Matrix, mol Atomer) error {
	iowriterError := func(err error) error {
		return CError{"Failed to write in io.Writer" + err.Error(), []string{"io.Writer.Write", "XYZWrite"}}
	}
	if mol.Len() != Coords.NVecs() {
		return CError{"Ref and Coords dont have the same number of atoms", []string{"XYZWrite"}}
	}
	c := make([]float64, 3, 3)
	_, err := out.Write([]byte(fmt.Sprintf("%-4d\n\n", mol.Len())))
	if err != nil {
		return iowriterError(err)
	}
	//towrite := Coords.Arrays() //An array of array with the data in the matrix
	for i := 0; i < mol.Len(); i++ {
		//c := towrite[i] //coordinates for the corresponding atoms
		c = Coords.Row(c, i)
		temp := fmt.Sprintf("%-2s  %12.6f%12.6f%12.6f \n", mol.Atom(i).Symbol, c[0], c[1], c[2])
		_, err := out.Write([]byte(temp))
		if err != nil {
			return iowriterError(err)
		}
	}
	return nil
}

//AntiProjection returns a vector in the direction of ref with the magnitude of
//a vector A would have if |test| was the magnitude of its projection
//in the direction of test.
func AntiProjection(test, ref *v3.Matrix) *v3.Matrix {
	rr, _ := ref.Dims()
	testnorm := test.Norm(0)
	Uref := v3.Zeros(rr)
	Uref.Unit(ref)
	scalar := test.Dot(Uref)
	scalar = (testnorm * testnorm) / scalar
	Uref.Scale(scalar, Uref)
	return Uref
}

//Angle takes 2 vectors and calculate the angle in radians between them
//It does not check for correctness or return errors!
func Angle(v1, v2 *v3.Matrix) float64 {
	normproduct := v1.Norm(0) * v2.Norm(0)
	dotprod := v1.Dot(v2)
	argument := dotprod / normproduct
	//Take care of floating point math errors
	if math.Abs(argument-1) <= appzero {
		argument = 1
	} else if math.Abs(argument+1) <= appzero {
		argument = -1
	}
	//fmt.Println(dotprod/normproduct,argument) //dotprod/normproduct, dotprod, normproduct,v1.TwoNorm(),v2.TwoNorm())
	angle := math.Acos(argument)
	if math.Abs(angle) <= appzero {
		return 0.00
	}
	return angle
}