#written by Charlie for Weta Comp dept
#export a Shake Tracker node containing all the points in the selected pointgroup, for the selected sequence

set path [get3DEInstallPath]
source $path/user_data/tcl_archive/veclib.tcl


post3DEInfoRequester "Export 2D tracking curves or 3D backprojected curves of the currently selected points as a Shake Tracker Node.\n\nWritten by Charlie Tait for Weta Comp dept.\n\n(Version 5.1, updated by rs)"

open3DEConsole

proc getBackProjectedPoint2D { pg point fobj frame }\
	{
	set pg_type [getPGroupType $pg]
	set cam [getFobjCamera $fobj]

	# Geomery of the Camera
	set focal [getCameraFocalLength $cam]
	set width [getCameraFBackWidth $cam]
	set height [getCameraFBackHeight $cam]
	# Lens center offset
	set lcx [getCameraLensCenterX $cam]
	set lcy [getCameraLensCenterY $cam]

	# Field of View size in pixel
	set fov_width [getFobjImageWidthFOV $fobj]
	set fov_height [getFobjImageHeightFOV $fobj]

	set x [getPointCalcPosition3D $pg $point]
	
	set pos [getPGroupPosition3D $pg $fobj $frame]
	set rot [getPGroupRotation3D $pg $fobj $frame]
	
	if {$pg_type == "CAMERA"}\
		{
		# Difference vector from the camera position to the point in 3d
		set d [vec3-vec3 $x $pos]
		# Calculate the inverse of the camera matrix
		set rotinv [mat3trans $rot]
		# Reformulate the Difference vector in the coordiante frame of the camera
		set dnew [mat3*vec3 $rotinv $d]
		# Project this into the z=-1-Plane, that is: x' = x / -z, y' = y' -z (remember: the camera looks into -z direction)
		set d2 [vec2/s [list [vget $dnew 0] [vget $dnew 1]] [expr -[vget $dnew 2]]]
		# Now we want to express this in pixel respective to the lower left corner of the field of view (dotted line)
		# We scale it to metric units, remove the lens center offset, scale it down to dimensionless coordinates,
		# so that (0,0) is the lower left corner of the field of view, and (1,1) is the lower upper corner.
		set dfovx [expr ([vget $d2 0] * $focal - $lcx) / ($width) + .5]
		set dfovy [expr ([vget $d2 1] * $focal - $lcy) / ($height) + .5]
		}

	# This doesn't work yet:
	if {$pg_type == "OBJECT"}\
		{
		set rotinv [mat3invert $rot]
		set d [mat3*vec3 $rotinv $x]
		set dnew [vec3+vec3 $d $pos]
		set d2 [vec2/s [list [vget $dnew 0] [vget $dnew 1]] [expr -[vget $dnew 2]]]
		set dfovx [expr ([vget $d2 0] * $focal - $lcx) / ($width) + .5]
		set dfovy [expr ([vget $d2 1] * $focal - $lcy) / ($height) + .5]
		}
			
	set pos2d [list $dfovx $dfovy]
	
	return $pos2d
	}
		
	

set fobj [getSelectedFobj]
if {$fobj==0}\
        {
                bell
                post3DEInfoRequester "Error, there is no sequence selected!"
                return
        }
set name [getFobjName $fobj]
print3DEConsole "sequence = $fobj ($name)\n"

set pg [getSelectedPGroup]
if {$pg==0}\
        {
                bell
                post3DEInfoRequester "Error, there is no point group selected!"
                return
        }
set name [getPGroupName $pg]
print3DEConsole "group = $pg ($name)\n"

set count 0;
for {set point [getFirstPoint $pg]} {$point!=0} {set point [getNextPoint $pg $point]}\
	{
	set selected [getPointSelectionFlag $pg $point]
	if {$selected==1} { set count [expr $count+1] }
	}
if {$count==0}\
	{
	bell
	post3DEInfoRequester "\n      Error: There are no points selected!      \n"
	return
	}
set np $count
print3DEConsole "number of points = $np\n"

set width [getFobjImageWidth $fobj]
print3DEConsole "tracked width = $width\n"
set height [getFobjImageHeight $fobj]
print3DEConsole "tracked height = $height\n"

flush3DEConsole

set filename [post3DEFileRequester "Save Shake Tracker Node..." "*"]
if {$filename==""} { return }

# nothing entered...
set basewidth $width
set baseheight $height
set timeshift 0 

set string [post3DEPromptRequester "Please enter: <Base Width=$width> <Base Height=$height> <TimeShift=$timeshift>"]
if {$string!=""}\
        {
                # parse user input...
                scan $string "%ld %ld %ld" basewidth baseheight timeshift
        }


set file [open $filename w+]
set no_frames [getFobjNoFrames $fobj]
#show intended image size...values in shake Tracker node are not normailsed.
print3DEConsole "basewidth = $basewidth\n"
print3DEConsole "baseheight = $baseheight\n"

#they entered nothing in timeshift field
if {$timeshift==""}\
        {
        set timeshift 0
        }

print3DEConsole "timeshift = $timeshift\n"


set export_tracking_curves [post3DEQuestionRequester "\n      Export original 2D tracking curves or 3D backprojected curves?      \n" "2D Tracking Curves" "Backprojected Curves"]

print3DEConsole "Writing $np points in pointgroup $pg to $filename\n\n"
flush3DEConsole

#start writing shake script.  This is now non-version specific.  It'll still work in later versions.....

puts -nonewline $file "// Input nodes\n"
puts -nonewline $file "\n"

#simply a base size black frame as an input to the tracker node.
puts -nonewline $file "Color1 = Color($basewidth, $baseheight, 1, 0, red, red, 1, 0);\n"
puts -nonewline $file "\n"
puts -nonewline $file "// Processing nodes\n"
puts -nonewline $file "\n"

#write the tracker node in its ugly format
puts -nonewline $file "Tracker1 = Tracker(Color1, \"1-$no_frames\", \"1/16\", \"luminance\", 0.75, \n"
puts -nonewline $file "    \"use start frame\", 0.5, \"stop\", 1, 1, "

#open point loop

set count 1

for {set point [getFirstPoint $pg]} {$point!=0} {set point [getNextPoint $pg $point]}\
		{
		#begin this point's X perameters
		
		set selected [getPointSelectionFlag $pg $point]
		if {$selected==0} continue;

		puts -nonewline $file "\n"

		puts -nonewline $file "\"track$count\", " 
		#puts -nonewline $file "\n\n// Track$point  X curve\n"
		puts -nonewline $file "Linear(0" 
		#start frame loop
		for {set frame 1} {$frame<=$no_frames} {set frame [expr $frame+1]}\
			{
			if {$export_tracking_curves} \
				{
				# original tracking...
				set srcxy [getPointPosition2D $pg $point $fobj $frame]
				#un-normalize values, based on final image size in shake
				set srcx [expr [lindex $srcxy 0]*$basewidth]
				set sframe [expr $frame+$timeshift]
				#writes X Value at Frame
				if {[isPointPos2DValid $pg $point $fobj $frame]==1}\
					{
					puts -nonewline $file ",$srcx@$sframe"
					}
				}\
			else\
				{
				# backprojected values...
				set srcxy [getBackProjectedPoint2D $pg $point $fobj $frame]
				#un-normalize values, based on final image size in shake
				set srcx [expr [lindex $srcxy 0]*$basewidth]
				set sframe [expr $frame+$timeshift]
				#writes X Value at Frame
				puts -nonewline $file ",$srcx@$sframe"
				}
			}
		puts -nonewline $file "),"
		
		#begin this point's Y curve		
		#puts -nonewline $file "\n\n// Track$count  Y curve"

		puts -nonewline $file "\n"

		puts -nonewline $file "    Linear(0"
		#start frame loop
		for {set frame 1} {$frame<=$no_frames} {set frame [expr $frame+1]}\
			{
			if {$export_tracking_curves} \
				{
				set srcxy [getPointPosition2D $pg $point $fobj $frame]
				#un-normalize values, based on final image size in shake
				set srcy [expr [lindex $srcxy 1]*$baseheight]
				set sframe [expr $frame+$timeshift]
				#writes Y Value at Frame
				if {[isPointPos2DValid $pg $point $fobj $frame]==1}\
					{
					puts -nonewline $file ",$srcy@$sframe"
					}
				}\
			else\
				{
				set srcxy [getBackProjectedPoint2D $pg $point $fobj $frame]
				#un-normalize values, based on final image size in shake
				set srcy [expr [lindex $srcxy 1]*$baseheight]
				set sframe [expr $frame+$timeshift]
				#writes Y Value at Frame
				puts -nonewline $file ",$srcy@$sframe"
				}
			}
		puts -nonewline $file "),"
			
		puts -nonewline $file "\n"

			
		puts -nonewline $file "    Linear(0,1@1),"
		set frame 1
		#this defines the (now unimportant) feature and search sizes for the points...again....horribly foratted in shake
		#puts -nonewline $file "\n\n// Track$point  Feature and search sizes\n"
		if {$export_tracking_curves} \
			{
			set srcxy [getPointPosition2D $pg $point $fobj $frame]
			}\
		else\
			{
			set srcxy [getBackProjectedPoint2D $pg $point $fobj $frame]
			}
		set srcx [expr [lindex $srcxy 0]*$basewidth]
		set srcy [expr [lindex $srcxy 1]*$baseheight]
		set t1l [expr $srcx-25]
		set t1r [expr $srcx+25]
		set t1b [expr $srcy-25]
		set t1t [expr $srcy+25]
		set t1ls [expr $srcx-50]
		set t1rs [expr $srcx+50]
		set t1bs [expr $srcy-50]
		set t1ts [expr $srcy+50]
		puts -nonewline $file "$t1l, $t1r, $t1b, $t1t, $t1ls, $t1rs, $t1bs, $t1ts,"


		#assign point position to center of tracker window!  Yup...Shake is crazy.
		#puts -nonewline $file "\n\n// Track$point  Center of track window\n"
		puts -nonewline $file " track$count\X, track$count\Y, "


		#create validity curve (visibility)
		#puts -nonewline $file "\n\n// Match Visibility to Enable\n"
		puts -nonewline $file "  track$count\Enabled,"
		#create validity curve (visibility)
		#puts -nonewline $file "    Linear(0,"
		#for {set frame 1} {$frame<=$no_frames} {set frame [expr $frame+1]}\
		#	{
		#	set vis [isPointPos2DValid $pg $point $fobj $frame]
		#	set sframe [expr $frame+$timeshift]
		#	puts -nonewline $file "$vis@$sframe"
		#		#if its the last frame, close current curve
		#		if {$frame==$no_frames}\
		#			{
		#			puts -nonewline $file "),"
		#			}\
		#		else\
		#			{
		#			puts -nonewline $file ","
		#			}
		#	}
		

		#create validity curve (enable)

		puts -nonewline $file "\n"
		#puts -nonewline $file "\n\n//track$point enable curve"
		puts -nonewline $file "    Linear(0,"
		for {set frame 1} {$frame<=$no_frames} {set frame [expr $frame+1]}\
			{
			if {$export_tracking_curves} \
				{
				set en [isPointPos2DValid $pg $point $fobj $frame]
				}\
			else\
				{
				set en 1
				}
			set sframe [expr $frame+$timeshift]
			puts -nonewline $file "$en@$sframe"
			
			#if its the last frame, close current curve
			if {$frame==$no_frames}\
				{
				if {$count==$np}\
					{
					puts -nonewline $file ")); "
					}\
				else\
					{
					puts -nonewline $file "),"
					}\
				}\
			else\
				{
				puts -nonewline $file ","
				}
			}

	set count [expr $count+1]
}


                
post3DEInfoRequester "Data sucessfully written in file \"$filename.\""