#pragma rtGlobals=1		// Use modern global access method.
#pragma version = 1.1
#pragma ModuleName= bentSi


Menu "Macros"
	"Pick angle axis",bentSi#PickAngles(Rodriques,$"")
	"X rotation between cursors",/Q,yAngleBetweenCursors()
End



Function GetStrains(fileRoot,i0,i1)
	String fileRoot									// e.g. "ws7_1_"
	Variable i0,i1
//	Variable i, i0=142, i1=188
//	Variable i, i0=151, i1=188
	if (!strlen(fileRoot))
		DoAlert 0, "you must give the file root"
		return 1
	endif
	String str = StrVarOrDefault("strainPath","")
	if (strlen(str)<1)
		Abort "no path string found"
	endif
	printf "using path '%s'\r",str
	Variable N = i1-i0+1
//	Make/N=(N)/O ezz=NaN,eyy=NaN,exx=NaN
	Make/N=(N)/O exx=NaN, exy=NaN, exz=NaN
	Make/N=(N)/O eyx=NaN, eyy=NaN, eyz=NaN
	Make/N=(N)/O ezx=NaN, ezy=NaN, ezz=NaN
	Make/N=(N,2)/O int=NaN						// will be redimensioned later
//	SetScale/P x i0,1,"µm", exx,eyy,ezz,int
	SetScale/P x i0,1,"µm", exx,exy,exz,eyx,eyy,eyz,ezx,ezy,ezz,int
	Make/N=(3,3)/O/D rot							// a single rotation matrix
	Make/N=(N,3)/O/D Rodriques
	Rodriques = NaN

	Variable i,j,nn, angle
	String hklList,hkl
	String fName,origFile="", noteStr="", wName
	wName = UniqueName("vec3",1,0)
	Make/N=3/O/D $wName
	Wave vec3 = $wName
	wName = UniqueName("symOp",1,0)
	Make/N=(3,3)/D/O $wName
	Wave symOp=$wName
	Variable refMatNum = NumVarOrDefault(":refMatNum",(i0+i1)/2+1)
	sprintf fName, "%s%d.IND",fileRoot,abs(refMatNum)	// use refrence matrix from the center of the scan
	if (refMatNum>=0)								// a valid refMatNum
		Make/N=(3,3)/O/D refMat
		printf "reading refMat using '%s'\r",fName
		ReadOrientationMat(fName,refMat)			// get the refrence matrix 'refMat'
		SquareUpMatrix(refMat)
		Note/K refMat, "fName="+fName
	else
		printf "using the existing refMat\r"
	endif
	printWave(refMat)

	hklList = StringByKey("hklList",readIntensities(fName),"=")
	noteStr = ReplaceStringByKey("hklList",noteStr,hklList,"=")
	nn = ItemsInList(hklList,",")
	Redimension/N=(N,nn) int
	int = NaN
	for (j=0;j<nn;j+=1)
		hkl= ReplaceString("-", StringFromList(j,hklList,","), "_")	// need to change from '(-1-20)'  to  'x_1_20'
		hkl[0,0] = "x"
		hkl= ReplaceString(")", hkl, "")
		SetDimLabel 1,j,$hkl,int
	endfor

	for (i=i0;i<=i1;i+=1)
		sprintf fName, "%s%d.STR",fileRoot,i
		Wave mat = $readStrainMat(fName)
		if (WaveExists(mat))
			if (i==abs(refMatNum))
				origFile = StringByKey("origFile",note(mat),"=")
			endif
			mat *= 1e-3
//			exx[i-i0] = mat[0][0]
//			eyy[i-i0] = mat[1][1]
//			ezz[i-i0] = mat[2][2]
			exx[i-i0]=mat[0][0]  ;  exy[i-i0]=mat[0][1]  ;  exz[i-i0]=mat[0][2]
			eyx[i-i0]=mat[1][0]  ;  eyy[i-i0]=mat[1][1]  ;  eyz[i-i0]=mat[1][2]
			ezx[i-i0]=mat[2][0]  ;  ezy[i-i0]=mat[2][1]  ;  ezz[i-i0]=mat[2][2]
			str = note(mat)
			for (j=0;j<nn;j+=1)
				int[i-i0][j] = NumberByKey(StringFromList(j,hklList,","),str,"=")
			endfor
			KillWaves/Z mat

			fName = ReplaceString(".STR",fName,".IND")
			ReadOrientationMat(fName,rot)			// get the refrence matrix 'refMat'
			SquareUpMatrix(rot)
			BestSymOp(rot,refMat,symOp)
			MatrixOp/O mat = Inv(symOp) x rot		// the new rot, symmetry reduced
			rot = mat
			KillWaves/Z mat

			MatrixOp/O rho__ = (rot^t) x refMat	// rho = gm^-1 x g0
			angle = axisOfMatrix(rho__,vec3)		// returned angle (degrees)
			vec3 *= tan(angle*PI/180/2)			// this is now the Rodriques vectoraxis with length of degrees
			Rodriques[i-i0][0] = vec3[0]
			Rodriques[i-i0][1] = -vec3[1]			// Wenge reversed definition of F and H for the matricies
			Rodriques[i-i0][2] = -vec3[2]
		endif
	endfor
	noteStr = ReplaceStringByKey("origFile",noteStr,origFile,"=")
	Duplicate/O exx, zzs,yys,xxs					// subtract the exx component from the others
	xxs = exx[p] - exx[p]
	yys = eyy[p] - exx[p]
	zzs = ezz[p] - exx[p]
//	Note/K exx,noteStr
//	Note/K eyy,noteStr
//	Note/K ezz,noteStr
	Note/K exx,noteStr  ;  Note/K exy,noteStr  ;  Note/K exz,noteStr
	Note/K eyx,noteStr  ;  Note/K eyy,noteStr  ;  Note/K eyz,noteStr
	Note/K ezx,noteStr  ;  Note/K ezy,noteStr  ;  Note/K ezz,noteStr
//	Note/K xxs,noteStr
//	Note/K yys,noteStr
//	Note/K zzs,noteStr
	Note/K xxs,noteStr  ;  Note/K yys,noteStr  ;  Note/K zzs,noteStr
	Note/K int,noteStr

	vec3 = {0,0,0}
	PickAngles(Rodriques,vec3)
	Duplicate/O angles anglesTotal
	vec3 = {1,0,0}
	PickAngles(Rodriques,vec3)
	Duplicate/O angles anglesX
	vec3 = {0,1,0}
	PickAngles(Rodriques,vec3)
	Duplicate/O angles anglesY
	vec3 = {0,0,1}
	PickAngles(Rodriques,vec3)
	Duplicate/O angles anglesZ
	FindLevel/Q/P anglesX, 0						// find where anglesX changes sign
	if (V_flag)
		anglesTotal = abs(anglesTotal[p]) * sign(anglesX[p])	// change sign of total angles to match X angles
	else
		anglesTotal *= p<V_LevelX ? 1 : -1			// make total angle negative after xangle crosses zero
	endif
	CopyScales/I exx, anglesTotal,anglesX,anglesY,anglesZ
	Note/K anglesTotal,noteStr
	Note/K anglesX,noteStr
	Note/K anglesY,noteStr
	Note/K anglesZ,noteStr
	KillWaves/Z rot, rho__, vec3, symOp, angles
	print origFile
End
//
Static Function BestSymOp(A,B,Si)					// return best cubic symmetry op for going from A to B
	Wave A,B										// two orientation matricies
	Wave Si											// best cubic symmetry op (returned)
	String wName
	wName = UniqueName("SymMatrix",1,0)	;  Make/N=(3,3)/D/O $wName  ;  Wave symOp = $wName
	wName = UniqueName("matrix1D",1,0)		;  Make/N=(1)/D/O $wName	;  Wave trace11 = $wName

	Wave symOps=$StrVarOrDefault("root:Packages:Grains:SymmetryOpsPath","")	// wave with symmetry ops
	Variable N = NumberByKey("Nproper",note(symOps),"=")	// number of symmetyry operations to check
	N = numtype(N) ? DimSize(symOps,0) : N
	Variable i, traceMax = -4
	for (i=0;i<N;i+=1)
		symOp = symOps[i][p][q]					// symmetry operation
		MatrixOp/O trace11 = Trace(B x Inv(A) x symOp)	// Tr[ B(A^-1)(S^i) ]
		if (trace11[0] > traceMax)					// save value for optimum rotation matrix
			traceMax = trace11[0]
			Si = symOp
		endif
	endfor
	KillWaves/Z symOp, trace11
End


Static Function/S readStrainMat(fName)
	String fName

	SVAR pathStr=strainPath
	NewPath/O/Q/Z strain, pathStr
	if (V_flag)
		Abort "could not set path"
	endif
	Variable refNum

	String matName=UniqueName("strainMat",1,0)
	Make/N=(3,3)/O/D $matName=NaN
	Wave mat=$matName
	String noteStr=""

	Open/M="the .str file"/P=strain/R/T=".STR"/Z=1 refNum as fName
	if (!V_flag)
		//	String line=" ", find="deviatoric strain in crystal reference u1 u2 u3", origFile
		String line=" ", find="deviatoric strain in lab reference X Y Z", origFile
		FReadLine refNum, origFile
		origFile = ReplaceString("\r", origFile,"")		// remove trailing carriage return
		noteStr="origFile="+origFile
		for (;strsearch(line,find,0)<0 && strlen(line)>0;)
			FReadLine refNum, line
		endfor
		Variable a,b,c
		FReadLine refNum, line
		sscanf line, "%g %g %g",a,b,c
		mat[0][0] = a	;	mat[0][1] = b	;	mat[0][2] = c
		FReadLine refNum, line
		sscanf line, "%g %g %g",a,b,c
		mat[1][0] = a	;	mat[1][1] = b	;	mat[1][2] = c
		FReadLine refNum, line
		sscanf line, "%g %g %g",a,b,c
		mat[2][0] = a	;	mat[2][1] = b	;	mat[2][2] = c
		Close refNum
	endif

	fName = ReplaceString(".STR",fName,".IND")
	noteStr = readIntensities(fName)+noteStr			// returns list of intensities for a predefined list of hkl's
	Note/K mat
	Note mat, noteStr
	return GetWavesDataFolder(mat,2)
End


Static Function ReadOrientationMat(fName,mat)	// read one orientation mat from an '.IND' file
	String fName
	Wave mat									// the orientation mat to read

	Variable refNum
	Open/M="the .IND file"/P=strain/R/T=".IND"/Z=1 refNum as fName
	if (V_flag)
		mat = NaN								// default to bad data
		return 1								// could not open the file
	endif

	Variable i
	String line
	do
		FReadLine/N=200 refNum, line
		i = strsearch(line,"matrix X  Y  Z -> h  k  l",0)
	while(i<0 && strlen(line))
	if (!strlen(line))							// came to EOF before finding matrix
		Close refNum
		return 1
	endif

	Variable a,b,c
	FReadLine/N=200 refNum, line
	sscanf line, "%g %g %g",a,b,c
	mat[0][0] = a  ;  mat[0][1] = b  ;  mat[0][2] = c
	FReadLine/N=200 refNum, line
	sscanf line, "%g %g %g",a,b,c
	mat[1][0] = a  ;  mat[1][1] = b  ;  mat[1][2] = c
	FReadLine/N=200 refNum, line
	sscanf line, "%g %g %g",a,b,c
	mat[2][0] = a  ;  mat[2][1] = b  ;  mat[2][2] = c
	Close refNum

	return 0
End


Static Function/S readIntensities(fName)	// returns list of intensities for a predefined list of hkl's
	String fName

//	String list = "0,0,4;2,2,8;-2,0,6;0,2,6"
//	String list = "0,0,4;2,2,8;-1,1,7;1,3,7"
//	String list = "0,0,4;2,2,8;-1,1,7;-1,-1,9"	
//	String list = "0,0,4;2,2,8;1,1,7;-1,-1,9"			// list of peaks for intensity
	String list = "0,0,4;2,2,8;0,-2,10;-1,-1,9"		// list of peaks for intensity

	Variable refNum
	String noteStr=""
	Open/M="the .ind file"/P=strain/R/T=".IND"/Z=1 refNum as fName
	if (V_flag)
		return ""
	endif

	Close refNum
	LoadWave/G/M/N=data/O/P=strain/Q S_fileName
	String listOfWaves = S_waveNames
	Wave data=data0

	String str,hkl,item=" ",hklList=""
	Variable j, h,k,l
	for (j=0, item=StringFromList(j,list); strlen(item);j+=1, item=StringFromList(j,list))
		sscanf item, "%d,%d,%d",h,k,l
		sprintf hkl, "(%d%d%d)",h,k,l
		hklList += SelectString(j,"",",")+hkl
	endfor
	noteStr = ReplaceStringByKey("hklList",noteStr,hklList,"=")

	Make/N=3/O/D hkl0,hkl1
	Variable i,int, N=DimSize(data,0)
	for (j=0, item=StringFromList(j,list); strlen(item);j+=1, item=StringFromList(j,list))
		sscanf item, "%d,%d,%d",h,k,l
		sprintf hkl, "(%d%d%d)",h,k,l
		hkl0 = {h,k,l}
		int = NaN
		for (i=0;i<N;i+=1)
			hkl1 = data[i][2+p]
			if (abs(cosineAB(hkl0,hkl1))>(1-6e-10))	// 1-cos(0.002¡) = 6e-10
				int = data[i][11]
				break
			endif
		endfor
		noteStr = ReplaceNumberByKey(hkl,noteStr,roundSignificant(int,6),"=")
	endfor
	for (i=0, item=StringFromList(i,listOfWaves); strlen(item);i+=1, item=StringFromList(i,listOfWaves))
		KillWaves/Z $item
	endfor
	KillWaves/Z hkl0,hkl1
	return noteStr
End


Static Function PickAngles(R,axis)
	Wave R					// list of Rodriques vectors
	Wave axis
	Variable N = DimSize(R,0)
	if (N<1 || DimSize(R,1)!=3 || WaveDims(R)!=2)
		DoAlert 0, "'"+NameOfWave(R)+" has illegal dimensions"
		return 1
	endif
	Variable needPrompt=!WaveExists(axis)
	if (!needPrompt)
		needPrompt += (WaveDims(axis)==3)
		needPrompt += (DimSize(axis,9)==3)
		needPrompt += numtype(sum(axis))
	endif

	String wName = UniqueName("vec3",1,0)
	Make/N=3/O/D $wName
	Wave axisv = $wName
	wName = UniqueName("vec3",1,0)
	Make/N=3/O/D $wName
	Wave vec3 = $wName

	Variable total = 0
	if (needPrompt)
		String axisName
		Prompt axisName,"axis direction",popup,"total angle;X;Y;Z"
		DoPrompt "Axis Direction",axisName
		if (V_flag)
			KillWaves/Z axisv,vec3
			return 1
		endif
		if (stringMatch(axisName,"X"))
			axisv = {1,0,0}
		elseif (stringMatch(axisName,"Y"))
			axisv = {0,1,0}
		elseif (stringMatch(axisName,"Z"))
			axisv = {0,0,1}
		elseif (stringMatch(axisName,"total angle"))
			total = 1
		else
			Abort "impossible in PickAngles()"
		endif
	else
		axisv = axis
		total = (norm(axis)==0)
	endif
	normalize(axisv)
	Make/N=(N)/O/D angles
	Variable i,angle, dot
	for (i=0;i<N;i+=1)
		vec3 = R[i][p]
		angle = 2*atan(normalize(vec3))*180/PI
		if (numtype(vec3[0])==2)
			angle = NaN
		endif
		dot = total ? 1 : MatrixDot(axisv,vec3)
		angles[i] = angle * dot
	endfor
	KillWaves/Z axisv,vec3
	return 0
End


// compute angle and axis of a rotation matrix
Static Function axisOfMatrix(rot,axis)
	// returns total rotation angle, and sets axis to the axis of the total rotation
	Wave rot									// rotation matrix
	Wave axis									// axis of the rotation (angle is returned)

DoAlert 0,"check definition of routine 'rotationMatAboutAxis()'"
	Variable sumd = notRotationMatrix(rot)		// accept positive sumd that are less than 1e-4
	if (sumd<0 || sumd>1e-4)
		DoAlert 0, "'"+NameOfWave(rot)+"' is not a rotation matrix in axisOfMatrix()"
		axis = NaN
		return NaN
	elseif (0<sumd)
		Make/N=(3,3)/O/D axisMat__
		axisMat__ = rot
		if (SquareUpMatrix(axisMat__))
			DoAlert 0, "cannot square up '"+NameOfWave(rot)+"' in axisOfMatrix()"
			axis = NaN
			return NaN
		endif
		Wave rr = axisMat__
	else
		Wave rr = rot
	endif

	Variable cosine = (MatrixTrace(rr)-1)/2	// trace = 1 + 2*cos(theta)
	cosine = min(cosine,1)
	if (cosine<= -1)							// special for 180¡ rotation,
		axis[0] = sqrt((rr[0][0]+1)/2)
		axis[1] = sqrt((rr[1][1]+1)/2)
		axis[2] = sqrt((rr[2][2]+1)/2)		// always assume z positive
		axis[0] = (rr[0][2]+rr[2][0])<0 ? -axis[0] : axis[0]
		axis[1] = (rr[1][2]+rr[2][1])<0 ? -axis[1] : axis[1]
	else											// rotaion < 180¡, usual formula works
		axis[0] = rr[1][2] - rr[2][1]
		axis[1] = rr[2][0] - rr[0][2]
		axis[2] = rr[0][1] - rr[1][0]
	endif
	normalize(axis)
	KillWaves /Z axisMat__
	return acos(cosine)*180/PI				// rotation angle in degrees
End


// fix up a matrix so that it is exactly a rotation matrix, not just close to one
Static Function SquareUpMatrix(mat)
	Wave mat

	Wave vec0=$MakeUnique3Vector($"")
	Wave vec1=$MakeUnique3Vector($"")
	Wave vec2=$MakeUnique3Vector($"")
	Variable err
	vec0 = mat[p][0]
	vec1 = mat[p][1]
	vec2 = mat[p][2]
	Variable norm0=norm(vec0), norm1=norm(vec1), norm2=norm(vec2)

	// Start with the longest vector, and assume it is correct
	if (norm0>=norm1 && norm0>=norm2)	// X is longest
		Cross vec0,vec1						// Z = X x Y
		Wave W_Cross=W_Cross
		vec2 = W_Cross
		Cross vec2,vec0						// Y = Z x X
		vec1 = W_Cross
	elseif (norm1>=norm0 && norm1>=norm2)// Y is longest
		Cross vec1,vec2						// X = Y x Z
		Wave W_Cross=W_Cross
		vec0 = W_Cross
		Cross vec0,vec1						// Z = X x Y
		vec2 = W_Cross
	else											// Z is longest
		Cross vec2,vec0						// Y = Z x X
		Wave W_Cross=W_Cross
		vec1 = W_Cross
		Cross vec1,vec2						// X = Y x Z
		vec0 = W_Cross
	endif

	err = normalize(vec0)
	err += normalize(vec1)
	err += normalize(vec2)
	mat[][0] = vec0[p]
	mat[][1] = vec1[p]
	mat[][2] = vec2[p]

	err = ( notRotationMatrix(mat) != 0 )		// 0 is a rotation matrix
	KillWaves/Z vec0,vec1,vec2,W_Cross
	return numtype(err)
End
//
Static Function notRotationMatrix(mat)			// false if mat is a rotation matrix, i.e. mat^T = mat^-1
	Wave mat
	String wName = UniqueName("rot",1,0)
	MatrixOp/O $wName = Abs(( mat x mat^t ) - Identity(3))
	Wave diff = $wName
	Variable sumd = sum(diff)
	// printWave(diff)
	// print "sum(diff) = ",sumd
	KillWaves/Z $wName

	Variable returnVal, thresh=2e-10
	if (sumd>thresh)
		returnVal = sumd					// not a rotation matrix
	elseif (MatrixDet(mat)<0)
		returnVal = -1						// an improper rotation matrix
	else
		returnVal = 0						// yes it is a rotation matrix
	endif
	if (ItemsInList(GetRTStackInfo(0))<2)
		printf "'%s'  is %s matrix\r",NameOfWave(mat),SelectString(returnVal ,"an improper rotation","a rotation","NOT a rotation")
		if (sumd>=thresh && sumd<1e-5)
			printf "	but, it is almost a rotation matrix, sum(|diff|) = %g\r",sumd
		endif
	endif
	return returnVal
End
//
// Used to make temporary internal waves, a typical use would be:    Wave xxx=$MakeUnique3Vector($"")
// You will probably want to KillWaves/Z xxx, at the end of the routine.
Static Function/S MakeUnique3Vector(preset)		// make a new 3-vector, and optionally set it to preset[]
	Wave preset							// optional wave, if none call with $""
	String wName = UniqueName("vec3_",1,0)
	Make/N=3/D $wName
	Wave vec=$wName
	if (WaveExists(preset))
		vec = preset
	endif
	return GetWavesDataFolder(vec,2)		// return full path for assingment to a wave
End
//
// Used to make temporary internal waves, a typical use would be:    Wave xxx=$MakeUnique3Vector($"")
// You will probably want to KillWaves/Z xxx, at the end of the routine.
Static Function/S MakeUnique3x3Mat(preset)		// make a new 3x3 matrix, and optionally set it to preset[]
	Wave preset							// optional wave, if none call with $""
	String wName = UniqueName("mat3x3_",1,0)
	Make/N=(3,3)/D $wName
	Wave mat=$wName
	if (WaveExists(preset))
		mat = preset
	endif
	return GetWavesDataFolder(mat,2)		// return full path for assingment to a wave
End


Static Function parallel(a,b)					// true if a & b are parallel or anti-parallel
	Wave a,b
	Variable cosine = MatrixDot(a,b)/norm(a)/norm(b)
	return (abs(cosine)>(1-6e-10))	// 1-cos(0.002¡) = 6e-10
End
//
Static Function cosineAB(a,b)					// cosine of angle between a and b
	Wave a,b
	Variable cosine = MatrixDot(a,b)/norm(a)/norm(b)
	return cosine
End