# @file sincos_64.s
# @author Seth Hillbrand
# @date August 1st, 2010
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

# @details
# This code calculates the Sine and Cosine of a given angle using a Taylor expansion of
# a 13th degree polynomial.  However, it does not calculate the Remez error as it goes,
# so the errors do accumulate to a maximum of 7e-13.  On the other hand, this is fast.

###########################################################################################
# The function prototype is:
#		void fastsincos_64 (uint32_t m_offset, uint32_t m_anglenum, uint32_t m_harmonicnum,
#							double *m_angle_vec, double *m_sincos)
#
# m_sincos can be defined as:
#	struct {
#		double d_cos;
#		double d_sin;
#	} sincos

			.align		16,0x90
			.globl		fastsincos_64
fastsincos_64:
# Offset (uint32_t): %edi
# Number of angles (uint32_t): %esi
# Number of harmonics (uint32_t): %edx
# Pointer to vector of inputs (double *): %rcx
# Pointer to vector of output (double *): %r8
			movslq		%edi, %r9
			movl		%edx, %eax
			imul		%edi, %eax
			cmp			%esi, %edi
			jae			..return

			# Load up our data into the registers for ready access
			# Since we are 64-bit only, we burn through almost all 16 :)
			movapd		dbl_cos_const(%rip), %xmm9
			movapd		16+dbl_cos_const(%rip), %xmm8
			movapd		32+dbl_cos_const(%rip), %xmm7
			movapd		48+dbl_cos_const(%rip), %xmm6
			movapd		64+dbl_cos_const(%rip), %xmm4
			movapd		80+dbl_cos_const(%rip), %xmm5
			movapd		96+dbl_cos_const(%rip), %xmm3
			movapd		inv_two_pi(%rip), %xmm2
			movapd		two_pi(%rip), %xmm1
			movapd		two_over_pi(%rip), %xmm0
			xorps		%xmm11, %xmm11
			#store the previous function data (After loading to align the call)
			movq		%r15, -32(%rsp)
			movq		%rbp, -40(%rsp)
..start_angle:                       
			movddup		(%rcx,%r9,8), %xmm10	# load the next angle from the array
			xor			%r11d, %r11d			# zero out r11
			test		%edx, %edx				# check for positive angle.  I mostly only deal with positive
			jbe			..make_positive			#   so this check is placed at the end of the function.
			mulpd		%xmm2, %xmm10			# angle *= (1/2pi)
			#hsubpd		%xmm10, %xmm10
			lea			(%rax,%rax), %r10d		# The next three instructions increment the frame pointer
			movslq		%r10d, %r10				#  	so that the next instruction loop doesn't need to calculate
..start_harmonic:								#  	anything to load the angle.
			lea			2(%r10), %rbp
			inc			%r11d					# Increment the harmonic number
			cvtsi2sd	%r11d, %xmm12
			movddup		%xmm12, %xmm12
			mulpd		%xmm10, %xmm12			# fn = angle * (1/2pi) * (1 + harmonic)
			roundsd		$1, %xmm12, %xmm13		# rounded = floor(fn)
			subsd		%xmm13, %xmm12			# remainder = fn - rounded
			mulpd		%xmm1, %xmm12			# constrained_angle = 2*pi*remainder
			hsubpd		%xmm12, %xmm12
			comisd		%xmm12, %xmm11			# If the constrained_angle is less than 0,
			jbe			..start_calc			#	increment by 2*pi and try again
			xorps		%xmm13, %xmm13
..increment_harmonic:                  			# 	otherwise, begin the calculation
			movsd		%xmm1, %xmm14
			subsd		%xmm13, %xmm14
			movsd		%xmm14, %xmm15			# The 2pi increment here implements the Kahan Summation Algorithm
			addsd		%xmm12, %xmm15
			movsd		%xmm15, %xmm13
			subsd		%xmm12, %xmm13
			subsd		%xmm14, %xmm13
			movsd		%xmm15, %xmm12
			comisd		%xmm12, %xmm11
			ja			..increment_harmonic
..start_calc:
			prefetchnta	angle_mask(%rip)
			movddup		%xmm12, %xmm13
			mulpd		%xmm0, %xmm13
			movddup		%xmm12, %xmm15
			cvttsd2si	%xmm13, %r15d				# Pick out the quadrant for our masking/shifting ops
			movslq		%r15d, %r15
			shl			$4, %r15
			xorps		angle_mask(%r15), %xmm15
			cmp			%edx, %r11d
			addpd		angle_shift(%r15), %xmm15
			mulpd		%xmm15, %xmm15				# Calculate the polynomial
			movaps		%xmm9, %xmm14
			mulpd		%xmm15, %xmm14
			addpd		%xmm8, %xmm14
			mulpd		%xmm15, %xmm14
			addpd		%xmm7, %xmm14
			mulpd		%xmm15, %xmm14
			addpd		%xmm6, %xmm14
			mulpd		%xmm15, %xmm14
			addpd		%xmm4, %xmm14
			mulpd		%xmm15, %xmm14
			addpd		%xmm5, %xmm14
			mulpd		%xmm14, %xmm15
			addpd		%xmm3, %xmm15
			xorps		quad_mask(%r15), %xmm15		# Apply the appropriate sign to the answers based on quadrant
			movaps		%xmm15, -24(%rsp)
			mov			-24(%rsp), %r15
			mov			%r15, (%r8,%r10,8)
			mov			-16(%rsp), %r15
			mov			%r15, 8(%r8,%r10,8)
			mov			%rbp, %r10
			jb			..start_harmonic
			nop			0x0(%rax,%rax,1)			# Pad 5 bytes in instruction stream to align the end
			add			%edx, %eax
			incq		%r9
			inc			%edi
			cmp			%esi, %edi
			jb			..start_angle
..exit_func:
			movq		-32(%rsp), %r15
			movq		-40(%rsp), %rbp
..return:
			ret
..make_positive:
			addl		%edx, %eax
			incq		%r9
			incl		%edi
			cmp			%esi, %edi
			jb			..start_angle
			jmp			..exit_func

			.align		16,0x90
			.type		fastsincos_64 STT_FUNC
			.size		fastsincos_64,.-fastsincos_64

			.data
			.section 	sincosdata, "a", @progbits
			.align		16
angle_mask:
			.double		0.0e+00, -0.0e+00
			.double		-0.0e+00, 0.0e+00
			.double		0.0e+00, -0.0e+00
			.double		-0.0e+00, 0.0e+00
			.type		angle_mask STT_OBJECT
			.size		angle_mask,64
			.align		16
angle_shift:
			.double		0.0e+00, 1.570796326794897e+00						#0, pi/2
			.double		3.141592653589793e+00, -1.570796326794897e+00		#pi, -pi/2
			.double		-3.141592653589793e+00, 4.712388980384690e+00		#-pi, 3pi/2
			.double		6.283185307179586e+00,-4.712388980384690e+00		#2pi, -3pi/2
			.type		angle_shift STT_OBJECT
			.size		angle_shift,64
			.align		16
quad_mask:
			.double		0.0e+00, 0.0e+00
			.double		-0.0e+00, 0.0e+00
			.double		-0.0e+00, -0.0e+00
			.double		0.0e+00, -0.0e+00
			.type		quad_mask STT_OBJECT
			.size		quad_mask,64
			.align		16
dbl_cos_const:
			.double		1.99078568541e-09, 1.99078568541e-09
			.double		-2.752469638432e-07, -2.752469638432e-07
			.double		2.48010406484558e-05, 2.48010406484558e-05
			.double		-1.388888418000422e-03, -1.388888418000422e-03
			.double		4.1666666473384543e-02, 4.1666666473384543e-02
			.double		-4.999999999702403e-01, -4.999999999702403e-01
			.double		9.99999999999252e-01, 9.99999999999252e-01
			.type		dbl_cos_const STT_OBJECT
			.size		dbl_cos_const,112
			.align		16
inv_two_pi:
			.double		1.59154943091895e-01, 3.33066907387547e-16
			.type		inv_two_pi STT_OBJECT
			.size		inv_two_pi,16
			.align		16
two_over_pi:
			.double		6.366197723675814e-01, 3.33066907387547e-16
			.type		two_over_pi STT_OBJECT
			.size		two_over_pi,16
			.align		16
two_pi:
			.double		6.28318530717959e+00, -3.5527136788005e-15
			.type		two_pi STT_OBJECT
			.size		two_pi,16
			.align		16
