;*****************************************************************************************
;	Mega 128 Scope Controller Program
;	(c) Martin Cibulski
;****************************************************************************************

;.db "UMi Alp ",0xAF,0x1A,0x87,0x7D
.EQU	ATEST1_AZ_STEPS	= 63917
.EQU	ATEST1_ALT_STEPS	= -899
.EQU	ATEST1_RECT_INT	= 0x1aaf
.EQU	ATEST1_DECL_INT	= 0x7d87
.EQU	ATEST1_TIME	= 8*128

;.db "Leo Alp ",0xF0,0x6A,0xD4,0x10
.EQU	ATEST2_AZ_STEPS	= 5095031
.EQU	ATEST2_ALT_STEPS	= -1455213
.EQU	ATEST2_RECT_INT	= 0x6af0
.EQU	ATEST2_DECL_INT	= 0x10d4
.EQU	ATEST2_TIME	= 186*128

;****************************************************************************************

.EQU	ALG_POS_NULL	= 0
.EQU	ALG_POS_NEW	= 1
.EQU	ALG_POS_VALID	= 2

.DSEG

alg_search_z3_yn:	.byte	1			;Search Z3 (1=Yes)
alg_sz3_errno:	.byte	1			;Search Z3 Error

;first alignment point
alg_rectint1:	.byte	2			;int coordinates from database
alg_declint1:	.byte	2			;
alg_rect1:	.byte	8			;radians
alg_decl1:	.byte	8			;radians
alg_time1:	.byte	4			;time in IRQs (128 per second)
alg_az_steps1:	.byte	4			;steps in az
alg_alt_steps1:	.byte	4			;steps in alt
alg_az_rad1:	.byte	8
alg_alt_rad1:	.byte	8

;second alignment point
alg_rectint2:	.byte	2
alg_declint2:	.byte	2
alg_rect2:	.byte	8
alg_decl2:	.byte	8
alg_time2:	.byte	4
alg_az_steps2:	.byte	4
alg_alt_steps2:	.byte	4
alg_az_rad2:	.byte	8
alg_alt_rad2:	.byte	8

;third point for fine adjustment
alg_rectint3:	.byte	2
alg_declint3:	.byte	2
alg_rect3:	.byte	8
alg_decl3:	.byte	8
alg_time3:	.byte	4
alg_az_steps3:	.byte	4
alg_alt_steps3:	.byte	4
alg_az_rad3:	.byte	8
alg_alt_rad3:	.byte	8

alg_newpos1:	.byte	1
alg_newpos2:	.byte	1
alg_newpos3:	.byte	1
alg_completed:	.byte	1

alg_mat_mnt:
alg_mntvec1x:	.byte	8
alg_mntvec1y:	.byte	8
alg_mntvec1z:	.byte	8
alg_mntvec2x:	.byte	8
alg_mntvec2y:	.byte	8
alg_mntvec2z:	.byte	8
alg_mntvec3x:	.byte	8
alg_mntvec3y:	.byte	8
alg_mntvec3z:	.byte	8

alg_mat_sky:
alg_skyvec1x:	.byte	8
alg_skyvec1y:	.byte	8
alg_skyvec1z:	.byte	8
alg_skyvec2x:	.byte	8
alg_skyvec2y:	.byte	8
alg_skyvec2z:	.byte	8
alg_skyvec3x:	.byte	8
alg_skyvec3y:	.byte	8
alg_skyvec3z:	.byte	8

alg_mat_mnt2sky:	.byte	9*FLT_SIZE
alg_mat_sky2mnt:	.byte	9*FLT_SIZE

alg_minz3:	.byte	8
alg_maxz3:	.byte	8
alg_minsep:	.byte	8
alg_maxsep:	.byte	8
alg_truesep:	.byte	8

;****************************************************************************************
.EQU	ALG_ADJ_MAX	= 100
.EQU	ALG_ADJ_SIZE	= (4 + 4 + 4 + 2 + 2 + 8)

.DSEG
alg_eep_open:	.byte	1
alg_ana_errsum:	.byte	8
alg_eep_counter:	.byte	1

;****************************************************************************************

.CSEG
alg_label_tab:
	.dw	alg_pos1
	.db	L_CMD	,"APOS1      "
	.dw	alg_pos2
	.db	L_CMD	,"APOS2      "
	.dw	alg_pos3
	.db	L_CMD	,"APOS3      "
	.dw	alg_pos
	.db	L_CMD	,"APOS       "
	.dw	alg_calc
	.db	L_XCMD	,"ACALC      "
	.dw	alg_analysis
	.db	L_XCMD	,"AANA       "
	.dw	eep_ana_array
	.db	L_BEEP	,"AEARRAY    "
	.dw	alg_eep_open
	.db	L_BRAM	,"AEOPEN     "
	.dw	alg_eep_counter
	.db	L_BRAM	,"AECOUNTER  "
	.dw	alg_ana_errsum
	.db	L_FRAM	,"AERRSUM    "

	.dw	clock
	.db	L_LRAM	,"CLOCK      "
	.dw	clock_step
	.db	L_BRAM	,"CLOCKSTEP  "

	.dw	alg_rectint1
	.db	L_WRAM	,"ARECTINT1  "
	.dw	alg_declint1
	.db	L_WRAM	,"ADECLINT1  "
	.dw	alg_rect1
	.db	L_FRAM	,"ARECTRAD1  "
	.dw	alg_decl1
	.db	L_FRAM	,"ADECLRAD1  "
	.dw	alg_time1
	.db	L_LRAM	,"ATIME1     "
	.dw	alg_az_steps1
	.db	L_LRAM	,"AAZSTEPS1  "
	.dw	alg_alt_steps1
	.db	L_LRAM	,"AALTSTEPS1 "
	.dw	alg_az_rad1
	.db	L_FRAM	,"AAZRAD1    "
	.dw	alg_alt_rad1
	.db	L_FRAM	,"AALTRAD1   "
	.dw	alg_newpos1
	.db	L_BRAM	,"AFLAGS1    "

	.dw	alg_rectint2
	.db	L_WRAM	,"ARECTINT2  "
	.dw	alg_declint2
	.db	L_WRAM	,"ADECLINT2  "
	.dw	alg_rect2
	.db	L_FRAM	,"ARECTRAD2  "
	.dw	alg_decl2
	.db	L_FRAM	,"ADECLRAD2  "
	.dw	alg_time2
	.db	L_LRAM	,"ATIME2     "
	.dw	alg_az_steps2
	.db	L_LRAM	,"AAZSTEPS2  "
	.dw	alg_alt_steps2
	.db	L_LRAM	,"AALTSTEPS2 "
	.dw	alg_az_rad2
	.db	L_FRAM	,"AAZRAD2    "
	.dw	alg_alt_rad2
	.db	L_FRAM	,"AALTRAD2   "
	.dw	alg_newpos2
	.db	L_BRAM	,"AFLAGS2    "

	.dw	alg_search_z3_yn
	.db	L_BRAM	,"ASEARCHZ3  "
	.dw	alg_sz3_errno
	.db	L_BRAM	,"ASZ3ERRNO  "

	.dw	alg_mntvec1x
	.db	L_FRAM	,"AMNTVEC1   "
	.dw	alg_mntvec2x
	.db	L_FRAM	,"AMNTVEC2   "
	.dw	alg_mntvec3x
	.db	L_FRAM	,"AMNTVEC3   "

	.dw	alg_skyvec1x
	.db	L_FRAM	,"ASKYVEC1   "
	.dw	alg_skyvec2x
	.db	L_FRAM	,"ASKYVEC2   "
	.dw	alg_skyvec3x
	.db	L_FRAM	,"ASKYVEC3   "

	.dw	alg_minz3
	.db	L_FRAM	,"AMINZ3     "
	.dw	alg_maxz3
	.db	L_FRAM	,"AMAXZ3     "
	.dw	alg_minsep
	.db	L_FRAM	,"AMINSEP    "
	.dw	alg_maxsep
	.db	L_FRAM	,"AMAXSEP    "
	.dw	alg_truesep
	.db	L_FRAM	,"ATRUESEP   "

	.dw	0
	.db	L_END	, 0

;****************************************************************************************
;	Monitor Command APOS3
;	Set AzSteps, AltSteps, Clock for Alignment Tests
;
;	APOS <az_steps> <alt_steps> <time>
;****************************************************************************************

alg_pos1:
	putc	'a'
	putc	'z'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_az_steps1
	call	printf_byte_hex
	lds	r16	,alg_az_steps1+1
	call	printf_byte_hex
	lds	r16	,alg_az_steps1+2
	call	printf_byte_hex
	lds	r16	,alg_az_steps1+3
	call	printf_byte_hex
	putc	' '

	putc	'a'
	putc	'l'
	putc	't'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_alt_steps1
	call	printf_byte_hex
	lds	r16	,alg_alt_steps1+1
	call	printf_byte_hex
	lds	r16	,alg_alt_steps1+2
	call	printf_byte_hex
	lds	r16	,alg_alt_steps1+3
	call	printf_byte_hex
	putc	' '

	putc	't'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_time1
	call	printf_byte_hex
	lds	r16	,alg_time1+1
	call	printf_byte_hex
	lds	r16	,alg_time1+2
	call	printf_byte_hex
	lds	r16	,alg_time1+3
	call	printf_byte_hex
	putc	' '

	putc	'r'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_rectint1
	call	printf_byte_hex
	lds	r16	,alg_rectint1+1
	call	printf_byte_hex
	putc	' '

	putc	'd'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_declint1
	call	printf_byte_hex
	lds	r16	,alg_declint1+1
	call	printf_byte_hex

	putc	13
	ret

alg_pos2:
	putc	'a'
	putc	'z'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_az_steps2
	call	printf_byte_hex
	lds	r16	,alg_az_steps2+1
	call	printf_byte_hex
	lds	r16	,alg_az_steps2+2
	call	printf_byte_hex
	lds	r16	,alg_az_steps2+3
	call	printf_byte_hex
	putc	' '

	putc	'a'
	putc	'l'
	putc	't'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_alt_steps2
	call	printf_byte_hex
	lds	r16	,alg_alt_steps2+1
	call	printf_byte_hex
	lds	r16	,alg_alt_steps2+2
	call	printf_byte_hex
	lds	r16	,alg_alt_steps2+3
	call	printf_byte_hex
	putc	' '

	putc	't'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_time2
	call	printf_byte_hex
	lds	r16	,alg_time2+1
	call	printf_byte_hex
	lds	r16	,alg_time2+2
	call	printf_byte_hex
	lds	r16	,alg_time2+3
	call	printf_byte_hex
	putc	' '

	putc	'r'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_rectint2
	call	printf_byte_hex
	lds	r16	,alg_rectint2+1
	call	printf_byte_hex
	putc	' '

	putc	'd'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_declint2
	call	printf_byte_hex
	lds	r16	,alg_declint2+1
	call	printf_byte_hex

	putc	13
	ret

alg_pos3:
	putc	'a'
	putc	'z'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_az_steps3
	call	printf_byte_hex
	lds	r16	,alg_az_steps3+1
	call	printf_byte_hex
	lds	r16	,alg_az_steps3+2
	call	printf_byte_hex
	lds	r16	,alg_az_steps3+3
	call	printf_byte_hex
	putc	' '

	putc	'a'
	putc	'l'
	putc	't'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_alt_steps3
	call	printf_byte_hex
	lds	r16	,alg_alt_steps3+1
	call	printf_byte_hex
	lds	r16	,alg_alt_steps3+2
	call	printf_byte_hex
	lds	r16	,alg_alt_steps3+3
	call	printf_byte_hex
	putc	' '

	putc	't'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_time3
	call	printf_byte_hex
	lds	r16	,alg_time3+1
	call	printf_byte_hex
	lds	r16	,alg_time3+2
	call	printf_byte_hex
	lds	r16	,alg_time3+3
	call	printf_byte_hex
	putc	' '

	putc	'r'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_rectint3
	call	printf_byte_hex
	lds	r16	,alg_rectint3+1
	call	printf_byte_hex
	putc	' '

	putc	'd'
	putc	'='
	putc	'0'
	putc	'x'
	lds	r16	,alg_declint3
	call	printf_byte_hex
	lds	r16	,alg_declint3+1
	call	printf_byte_hex

	putc	13
	ret

;****************************************************************************************
;	Monitor Command APOS
;	Set AzSteps, AltSteps, Clock for Alignment Tests
;
;	APOS <az_steps> <alt_steps> <time>
;****************************************************************************************

alg_pos:
	call	next_flt_value
	brtc	alg_pos_exit

	cli
	sts	mot_a_pos	,AKKU_5
	sts	mot_a_pos+1	,AKKU_6
	sts	mot_a_pos+2	,AKKU_7
	sts	mot_a_pos+3	,AKKU_8
	sei

	call	next_flt_value
	brtc	alg_pos_exit

	cli
	sts	mot_b_pos	,AKKU_5
	sts	mot_b_pos+1	,AKKU_6
	sts	mot_b_pos+2	,AKKU_7
	sts	mot_b_pos+3	,AKKU_8
	sei

	call	next_flt_value
	brtc	alg_pos_exit

	cli
	sts	clock	,AKKU_5
	sts	clock+1	,AKKU_6
	sts	clock+2	,AKKU_7
	sts	clock+3	,AKKU_8
	sei

	set
alg_pos_exit:
	ret


;****************************************************************************************

alg_init:
	f_const	flt__0
	f_store	mco_z3
	f_store	mco_az_offset
	f_store	mco_alt_offset

	ldi	r16	,0
	ldi	r17	,alg_newpos1 - alg_rectint1
	ldi	xl	,low(alg_rectint1)
	ldi	xh	,high(alg_rectint1)
alg_init_loop:
	st	X+	,r16
	dec	r17
	brne	alg_init_loop

	ldi	r16	,ALG_POS_NULL
	sts	alg_newpos1	,r16
	sts	alg_newpos2	,r16
	sts	alg_newpos3	,r16

	ldi	r16	,0
	sts	alg_completed	,r16

	ldi	r16	,0
	sts	alg_eep_counter	,r16

	ret

;****************************************************************************************

alg_set_test_data:
	ldi	r16	,byte4(ATEST1_AZ_STEPS)
	ldi	r17	,byte3(ATEST1_AZ_STEPS)
	ldi	r18	,high(ATEST1_AZ_STEPS)
	ldi	r19	,low(ATEST1_AZ_STEPS)
	sts	alg_az_steps1	,r16
	sts	alg_az_steps1+1	,r17
	sts	alg_az_steps1+2	,r18
	sts	alg_az_steps1+3	,r19
	ldi	r16	,byte4(ATEST1_ALT_STEPS)
	ldi	r17	,byte3(ATEST1_ALT_STEPS)
	ldi	r18	,high(ATEST1_ALT_STEPS)
	ldi	r19	,low(ATEST1_ALT_STEPS)
	sts	alg_alt_steps1	,r16
	sts	alg_alt_steps1+1	,r17
	sts	alg_alt_steps1+2	,r18
	sts	alg_alt_steps1+3	,r19
	ldi	r16	,byte4(ATEST1_TIME)
	ldi	r17	,byte3(ATEST1_TIME)
	ldi	r18	,high(ATEST1_TIME)
	ldi	r19	,low(ATEST1_TIME)
	sts	alg_time1	,r16
	sts	alg_time1+1	,r17
	sts	alg_time1+2	,r18
	sts	alg_time1+3	,r19
	ldi	r16	,high(ATEST1_RECT_INT)
	ldi	r17	,low(ATEST1_RECT_INT)
	sts	alg_rectint1	,r16
	sts	alg_rectint1+1	,r17
	ldi	r16	,high(ATEST1_DECL_INT)
	ldi	r17	,low(ATEST1_DECL_INT)
	sts	alg_declint1	,r16
	sts	alg_declint1+1	,r17

	ldi	r16	,byte4(ATEST2_AZ_STEPS)
	ldi	r17	,byte3(ATEST2_AZ_STEPS)
	ldi	r18	,high(ATEST2_AZ_STEPS)
	ldi	r19	,low(ATEST2_AZ_STEPS)
	sts	alg_az_steps2	,r16
	sts	alg_az_steps2+1	,r17
	sts	alg_az_steps2+2	,r18
	sts	alg_az_steps2+3	,r19
	ldi	r16	,byte4(ATEST2_ALT_STEPS)
	ldi	r17	,byte3(ATEST2_ALT_STEPS)
	ldi	r18	,high(ATEST2_ALT_STEPS)
	ldi	r19	,low(ATEST2_ALT_STEPS)
	sts	alg_alt_steps2	,r16
	sts	alg_alt_steps2+1	,r17
	sts	alg_alt_steps2+2	,r18
	sts	alg_alt_steps2+3	,r19
	ldi	r16	,byte4(ATEST2_TIME)
	ldi	r17	,byte3(ATEST2_TIME)
	ldi	r18	,high(ATEST2_TIME)
	ldi	r19	,low(ATEST2_TIME)
	sts	alg_time2	,r16
	sts	alg_time2+1	,r17
	sts	alg_time2+2	,r18
	sts	alg_time2+3	,r19
	ldi	r16	,high(ATEST2_RECT_INT)
	ldi	r17	,low(ATEST2_RECT_INT)
	sts	alg_rectint2	,r16
	sts	alg_rectint2+1	,r17
	ldi	r16	,high(ATEST2_DECL_INT)
	ldi	r17	,low(ATEST2_DECL_INT)
	sts	alg_declint2	,r16
	sts	alg_declint2+1	,r17

	ldi	r16	,ALG_POS_NEW
	sts	alg_newpos1	,r16
	sts	alg_newpos2	,r16

	ret

;****************************************************************************************

alg__db2rect:	.db	0x3F,0x19,0x6B,0x0F,0x1F,0xBF,0x73,0x84	;2PI/64800
alg__db2decl:	.db	0x3F,0x09,0x6B,0x0F,0x1F,0xBF,0x73,0x84	; PI/64800

alg_calc:
	lds	r16	,alg_newpos1
	cpi	r16	,ALG_POS_NEW
	brne	alg_calc_no_newpos1

	clr	AKKU_2
	clr	AKKU_3
	lds	AKKU_4	,alg_rectint1
	lds	AKKU_5	,alg_rectint1+1
	call	flt_convert_long
	f_push
	f_const	alg__db2rect
	f_mul
	f_store	alg_rect1

	lds	AKKU_2	,alg_declint1
	lds	AKKU_3	,alg_declint1+1
	call	flt_convert_word
	f_push
	f_const	alg__db2decl
	f_mul
	f_store	alg_decl1

	ldi	r16	,high(alg_rect1)
	ldi	r17	,low(alg_rect1)
	ldi	r18	,high(alg_skyvec1x)
	ldi	r19	,low(alg_skyvec1x)
	call	alg_sky_to_vec

	ldi	r16	,ALG_POS_VALID
	sts	alg_newpos1	,r16
	clr	r16
	sts	alg_completed	,r16

alg_calc_no_newpos1:

	lds	r16	,alg_newpos2
	cpi	r16	,ALG_POS_NEW
	breq	alg_calc_01

	rjmp	alg_calc_no_newpos2

alg_calc_01:
	clr	AKKU_2
	clr	AKKU_3
	lds	AKKU_4	,alg_rectint2
	lds	AKKU_5	,alg_rectint2+1
	call	flt_convert_long
	f_push
	f_const	alg__db2rect
	f_mul
	f_store	alg_rect2

	lds	AKKU_2	,alg_declint2
	lds	AKKU_3	,alg_declint2+1
	call	flt_convert_word
	f_push
	f_const	alg__db2decl
	f_mul
	f_store	alg_decl2

	ldi	r16	,high(alg_rect2)
	ldi	r17	,low(alg_rect2)
	ldi	r18	,high(alg_skyvec2x)
	ldi	r19	,low(alg_skyvec2x)
	call	alg_sky_to_vec

	ldi	r16	,high(alg_skyvec1x)
	ldi	r17	,low(alg_skyvec1x)
	call	alg_third_column

	ldi	r16	,ALG_POS_VALID
	sts	alg_newpos2	,r16
	clr	r16
	sts	alg_completed	,r16

alg_calc_no_newpos2:
	lds	r16	,alg_newpos1
	cpi	r16	,ALG_POS_VALID
	brne	alg_calc_end1

	lds	r16	,alg_newpos2
	cpi	r16	,ALG_POS_VALID
	breq	alg_calc_matrix
alg_calc_end1:
	ret

alg_calc_matrix:
	lds	r16	,alg_completed
	tst	r16
	brne	alg_calc_end1

	lds	r16	,alg_search_z3_yn
	tst	r16
	breq	alg_calc_dont_search_z3

	rcall	alg_search_z3
	brts	alg_calc_z3_found

alg_calc_dont_search_z3:
	ldi	r16	,high(alg_az_steps1)
	ldi	r17	,low(alg_az_steps1)
	ldi	r18	,high(alg_az_rad1)
	ldi	r19	,low(alg_az_rad1)
	call	alg_mnt_steps2rad

	ldi	r16	,high(alg_az_steps1)
	ldi	r17	,low(alg_az_steps1)
	ldi	r18	,high(alg_mntvec1x)
	ldi	r19	,low(alg_mntvec1x)
	call	mco_rad2vec

	ldi	r16	,high(alg_az_steps2)
	ldi	r17	,low(alg_az_steps2)
	ldi	r18	,high(alg_az_rad2)
	ldi	r19	,low(alg_az_rad2)
	call	alg_mnt_steps2rad

	ldi	r16	,high(alg_az_steps2)
	ldi	r17	,low(alg_az_steps2)
	ldi	r18	,high(alg_mntvec2x)
	ldi	r19	,low(alg_mntvec2x)
	call	mco_rad2vec

alg_calc_z3_found:
	ldi	r16	,high(alg_mntvec1x)
	ldi	r17	,low(alg_mntvec1x)
	call	alg_third_column

	ldi	r16	,high(alg_mntvec1x)	;Check:
	ldi	r17	,low(alg_mntvec1x)	;distance between mount_vector_1 and 2
	ldi	r18	,high(alg_mntvec2x)	;
	ldi	r19	,low(alg_mntvec2x)	;
	call	flt_scalar_product
	f_push
	ldi	r16	,high(alg_skyvec1x)	;Check:
	ldi	r17	,low(alg_skyvec1x)	;distance between sky_vector_1 and 2
	ldi	r18	,high(alg_skyvec2x)	;
	ldi	r19	,low(alg_skyvec2x)	;
	call	flt_scalar_product
	f_sub

	ldi	r16	,high(alg_mntvec1x)	;Check:
	ldi	r17	,low(alg_mntvec1x)	;mount_vector_1
	ldi	r18	,high(alg_mntvec1x)	;must have length 1.0
	ldi	r19	,low(alg_mntvec1x)	;scalar product = 1.0
	call	flt_scalar_product

	ldi	r16	,high(alg_mntvec2x)	;Check:
	ldi	r17	,low(alg_mntvec2x)	;mount_vector_2
	ldi	r18	,high(alg_mntvec2x)	;must have length 1.0
	ldi	r19	,low(alg_mntvec2x)	;scalar product = 1.0
	call	flt_scalar_product

	ldi	r16	,high(alg_mntvec3x)	;Check:
	ldi	r17	,low(alg_mntvec3x)	;mount_vector_3
	ldi	r18	,high(alg_mntvec3x)	;must have length 1.0
	ldi	r19	,low(alg_mntvec3x)	;scalar product = 1.0
	call	flt_scalar_product

	ldi	r16	,high(alg_mntvec1x)	;Check:
	ldi	r17	,low(alg_mntvec1x)	;mount_vector_3 and mount_vector_1
	ldi	r18	,high(alg_mntvec3x)	;must be perpendicular
	ldi	r19	,low(alg_mntvec3x)	;scalar product = zero !
	call	flt_scalar_product

	ldi	r16	,high(alg_mntvec2x)	;Check:
	ldi	r17	,low(alg_mntvec2x)	;mount_vector_3 and mount_vector_2
	ldi	r18	,high(alg_mntvec3x)	;must be perpendicular
	ldi	r19	,low(alg_mntvec3x)	;scalar product = zero !
	call	flt_scalar_product

	ldi	r16	,high(alg_skyvec1x)	;Check:
	ldi	r17	,low(alg_skyvec1x)	;sky_vector_1
	ldi	r18	,high(alg_skyvec1x)	;must have length 1.0
	ldi	r19	,low(alg_skyvec1x)	;scalar product = 1.0 !
	call	flt_scalar_product

	ldi	r16	,high(alg_skyvec2x)	;Check:
	ldi	r17	,low(alg_skyvec2x)	;sky_vector_2
	ldi	r18	,high(alg_skyvec2x)	;must have length 1.0
	ldi	r19	,low(alg_skyvec2x)	;scalar product = 1.0 !
	call	flt_scalar_product

	ldi	r16	,high(alg_skyvec3x)	;Check:
	ldi	r17	,low(alg_skyvec3x)	;sky_vector_3
	ldi	r18	,high(alg_skyvec3x)	;must have length 1.0
	ldi	r19	,low(alg_skyvec3x)	;scalar product = 1.0 !
	call	flt_scalar_product

	ldi	r16	,high(alg_skyvec1x)	;Check:
	ldi	r17	,low(alg_skyvec1x)	;sky_vector_3 and sky_vector_2
	ldi	r18	,high(alg_skyvec3x)	;must be perpendicular
	ldi	r19	,low(alg_skyvec3x)	;scalar product = zero !
	call	flt_scalar_product

	ldi	r16	,high(alg_skyvec2x)	;Check:
	ldi	r17	,low(alg_skyvec2x)	;sky_vector_3 and sky_vector_2
	ldi	r18	,high(alg_skyvec3x)	;must be perpendicular
	ldi	r19	,low(alg_skyvec3x)	;scalar product = zero !
	call	flt_scalar_product

	ldi	r16	,high(alg_mat_mnt)	;Calc:
	ldi	r17	,low(alg_mat_mnt)		;inverse (mount_matrix)
	ldi	r18	,high(flt_mat_buffer)
	ldi	r19	,low(flt_mat_buffer)
	call	flt_mat_invert

	ldi	r16	,high(alg_mat_mnt)	;Check:
	ldi	r17	,low(alg_mat_mnt)		;mount_matrix * inverse (mount_matrix)
	ldi	r18	,high(flt_mat_buffer)	; = identity_matrix !
	ldi	r19	,low(flt_mat_buffer)
	ldi	r20	,high(alg_mat_mnt2sky)
	ldi	r21	,low(alg_mat_mnt2sky)
	call	flt_mat_mat_mul

	ldi	r16	,high(alg_mat_sky)	;Calc:
	ldi	r17	,low(alg_mat_sky)		;transformation matrix (mount to sky)
	ldi	r18	,high(flt_mat_buffer)	; = sky_matrix / mount_matrix
	ldi	r19	,low(flt_mat_buffer)	; = sky_matrix * inverse (mount_matrix)
	ldi	r20	,high(alg_mat_mnt2sky)
	ldi	r21	,low(alg_mat_mnt2sky)
	call	flt_mat_mat_mul

	ldi	r16	,high(alg_mat_mnt2sky)	;Calc:
	ldi	r17	,low(alg_mat_mnt2sky)	;transformation matrix (sky to mount)
	ldi	r18	,high(alg_mat_sky2mnt)	; = inverse (transformation matrix (mount to sky))
	ldi	r19	,low(alg_mat_sky2mnt)
	call	flt_mat_invert

	ldi	r16	,high(alg_mat_mnt2sky)	;Check:
	ldi	r17	,low(alg_mat_mnt2sky)	;transformation matrix (mount to sky) *
	ldi	r18	,high(alg_mat_sky2mnt)	;transformation matrix (sky to mount)
	ldi	r19	,low(alg_mat_sky2mnt)	; = identity_matrix !
	ldi	r20	,high(flt_mat_buffer)
	ldi	r21	,low(flt_mat_buffer)
	call	flt_mat_mat_mul

	ldi	r16	,high(alg_mat_mnt2sky)	;Check:
	ldi	r17	,low(alg_mat_mnt2sky)	;mount position 1 * transformation matrix (mount to sky)
	ldi	r18	,high(alg_mntvec1x)	; = sky position 1
	ldi	r19	,low(alg_mntvec1x)
	ldi	r20	,high(flt_mat_buffer)
	ldi	r21	,low(flt_mat_buffer)
	call	flt_mat_vec_mul

	ldi	r16	,high(alg_mat_mnt2sky)	;Check:
	ldi	r17	,low(alg_mat_mnt2sky)	;mount position 2 * transformation matrix (mount to sky)
	ldi	r18	,high(alg_mntvec2x)	; = sky position 2
	ldi	r19	,low(alg_mntvec2x)
	ldi	r20	,high(flt_mat_buffer+24)
	ldi	r21	,low(flt_mat_buffer+24)
	call	flt_mat_vec_mul

	ldi	r16	,high(alg_mat_mnt2sky)	;Check:
	ldi	r17	,low(alg_mat_mnt2sky)	;mount position 3 * transformation matrix (mount to sky)
	ldi	r18	,high(alg_mntvec3x)	; = sky position 3
	ldi	r19	,low(alg_mntvec3x)
	ldi	r20	,high(flt_mat_buffer+48)
	ldi	r21	,low(flt_mat_buffer+48)
	call	flt_mat_vec_mul

	ldi	r16	,high(alg_mat_sky2mnt)	;Check:
	ldi	r17	,low(alg_mat_sky2mnt)	;sky position 1 * transformation matrix (sky to mount)
	ldi	r18	,high(alg_skyvec1x)	; = mount position 1
	ldi	r19	,low(alg_skyvec1x)
	ldi	r20	,high(flt_mat_buffer)
	ldi	r21	,low(flt_mat_buffer)
	call	flt_mat_vec_mul

	ldi	r16	,high(alg_mat_sky2mnt)	;Check:
	ldi	r17	,low(alg_mat_sky2mnt)	;sky position 2 * transformation matrix (sky to mount)
	ldi	r18	,high(alg_skyvec2x)	; = mount position 2
	ldi	r19	,low(alg_skyvec2x)
	ldi	r20	,high(flt_mat_buffer+24)
	ldi	r21	,low(flt_mat_buffer+24)
	call	flt_mat_vec_mul

	ldi	r16	,high(alg_mat_sky2mnt)	;Check:
	ldi	r17	,low(alg_mat_sky2mnt)	;sky position 3 * transformation matrix (sky to mount)
	ldi	r18	,high(alg_skyvec3x)	; = mount position 3
	ldi	r19	,low(alg_skyvec3x)
	ldi	r20	,high(flt_mat_buffer+48)
	ldi	r21	,low(flt_mat_buffer+48)
	call	flt_mat_vec_mul

	;clr	r15
	;lds	r16	,alg_ana_counter
	;cpse	r16	,r15
	;call	alg_adj_analysis			

	ldi	r16	,1
	sts	alg_completed	,r16

	ldi	r16	,TGO_ST_NONE
	sts	tgo_status	,r16
	
	ret

;****************************************************************************************
;Find Z3
;
;Z3 is searched by iteration so that the separation of the two alignment stars
;in mount coordinates matches the true separation in sky coordinates
;
;****************************************************************************************

alg_sz3__maxcossep:				;cos (minimum azimut separation)
	.db	0x3F,0xEF,0x83,0x8B,0x8C,0x81,0x1C,0x17	;0,984807753 = cos (10°)

alg_search_z3:
	.EQU	alg_sz3_oldz3	= 1		;save old Z3 here
	.EQU	alg_sz3_truesep	= 9		;true separation from sky coordinates
	.EQU	alg_sz3_maxz3	= 17		;maximum Z3
	.EQU	alg_sz3_minz3	= 25		;minimum Z3
	.EQU	alg_sz3_maxerr	= 33		;separation error from maximum Z3
	.EQU	alg_sz3_minerr	= 41		;separation error from minimum Z3
	.EQU	alg_sz3_error	= 49		;separation error from new Z3 guess
	.EQU	alg_sz3_counter	= 57		;iteration loop counter
	.EQU	alg_sz3_lspace	= 58
	LOCAL	alg_sz3_lspace

	ldi	r16	,0
	sts	alg_sz3_errno	,r16
	f_load	mco_z3			;save old Z3
	f_store_l	alg_sz3_oldz3
	f_const	flt__0			;start with z3 = 0
	f_store	mco_z3

	ldi	r16	,high(alg_az_steps1)	;convert steps to az/alt for star 1
	ldi	r17	,low(alg_az_steps1)
	ldi	r18	,high(alg_az_rad1)
	ldi	r19	,low(alg_az_rad1)
	call	alg_mnt_steps2rad

	ldi	r16	,high(alg_az_steps1)	;convert az/alt to vector for star 1
	ldi	r17	,low(alg_az_steps1)
	ldi	r18	,high(alg_mntvec1x)
	ldi	r19	,low(alg_mntvec1x)
	call	mco_rad2vec

	ldi	r16	,high(alg_az_steps2)	;convert steps to az/alt for star 2
	ldi	r17	,low(alg_az_steps2)
	ldi	r18	,high(alg_az_rad2)
	ldi	r19	,low(alg_az_rad2)
	call	alg_mnt_steps2rad

	ldi	r16	,high(alg_az_steps2)	;convert az/alt to vector for star 2
	ldi	r17	,low(alg_az_steps2)
	ldi	r18	,high(alg_mntvec2x)
	ldi	r19	,low(alg_mntvec2x)
	call	mco_rad2vec

	f_const	flt__0			;make Z=0 to calc azimut difference only
	f_store	alg_mntvec1z
	f_store	alg_mntvec2z

	ldi	r16	,high(alg_mntvec1x)	;make unit vector
	ldi	r17	,low(alg_mntvec1x)
	movw	r19:r18	,r17:r16
	call	flt_vec_makeunit
	brts	alg_sz3_02			;OK ?

	ldi	r16	,1
	sts	alg_sz3_errno	,r16
	f_load_l	alg_sz3_oldz3			;restore old Z3
	f_store	mco_z3
	clt				;error if X=Y=0
	rjmp	alg_sz3_return			;return false

alg_sz3_02:
	ldi	r16	,high(alg_mntvec2x)	;make unit vector
	ldi	r17	,low(alg_mntvec2x)
	movw	r19:r18	,r17:r16
	call	flt_vec_makeunit
	brts	alg_sz3_03			;OK ?

	ldi	r16	,2
	sts	alg_sz3_errno	,r16
	f_load_l	alg_sz3_oldz3			;restore old Z3
	f_store	mco_z3
	clt				;error if X=Y=0
	rjmp	alg_sz3_return			;return false

alg_sz3_03:
	ldi	r16	,high(alg_mntvec1x)	;calc separation
	ldi	r17	,low(alg_mntvec1x)
	ldi	r18	,high(alg_mntvec2x)
	ldi	r19	,low(alg_mntvec2x)
	call	flt_scalar_product		;scalar product = cos(sep)

	f_push				;check if azimut separation is >10°
	f_const	alg_sz3__maxcossep		;scalar_product < maxcossep
	f_sub				;scalar_product - maxcossep must be negative
	tst	AKKU_S
	brmi	alg_sz3_az_sep_ok			;ok if negative

	ldi	r16	,3
	sts	alg_sz3_errno	,r16
	ldi	zl	,low(alg_sz3_error_3_msg<<1)
	ldi	zh	,high(alg_sz3_error_3_msg<<1)
	call	lcd_print_alarm
	f_load_l	alg_sz3_oldz3			;restore old Z3
	f_store	mco_z3
	clt				;return false
	rjmp	alg_sz3_return

alg_sz3_error_3_msg:
	.db	"Error 03"
	.db	"Az Sep  "

alg_sz3_az_sep_ok:
	ldi	r16	,high(alg_skyvec1x)	;calc true separation between stars
	ldi	r17	,low(alg_skyvec1x)	;from sky coordinates
	ldi	r18	,high(alg_skyvec2x)
	ldi	r19	,low(alg_skyvec2x)
	call	flt_scalar_product		;scalar product = cos(true_sep)
	f_store_l	alg_sz3_truesep
	f_store	alg_truesep

	clr	AKKU_S			;check if true separation is >10° and <170°
	f_push				;abs(scalar_product) < maxcossep
	f_const	alg_sz3__maxcossep		;scalar_product - maxcossep must be negative
	f_sub
	tst	AKKU_S
	brmi	alg_sz3_true_sep_ok		;ok if negative

	ldi	r16	,4
	sts	alg_sz3_errno	,r16
	ldi	zl	,low(alg_sz3_error_4_msg<<1)
	ldi	zh	,high(alg_sz3_error_4_msg<<1)
	call	lcd_print_alarm
	f_load_l	alg_sz3_oldz3			;restore old Z3
	f_store	mco_z3
	clt				;return false
	rjmp	alg_sz3_return

alg_sz3_error_4_msg:
	.db	"Error 04"
	.db	"True Sep"

alg_sz3_true_sep_ok:
	f_load	alg_alt_rad1			;check which star is higher in altitude,
	f_push				;then find min and max guesses
	f_load	alg_alt_rad2			;for Z3
	f_sub
	tst	AKKU_S
	brmi	alg_sz3_star1_lower		;alt1 - alt2 negative: alt1 smaller

	f_const	flt__pi2			;Star 1 is higher
	f_push				;calc z3 max guess to
	f_load	alg_alt_rad1			;bring higher star to zenit
	f_sub
	f_store_l	alg_sz3_maxz3
	f_store	alg_maxz3

	f_const	flt__0			;Star 2 is lower
	f_push				;calc z3 min guess to
	f_load	alg_alt_rad2			;bring lower star to horizon
	f_sub
	f_store_l	alg_sz3_minz3
	f_store	alg_minz3

	rjmp	alg_sz3_guesses_set

alg_sz3_star1_lower:
	f_const	flt__pi2			;Star 2 is higher
	f_push				;calc z3 max guess to
	f_load	alg_alt_rad2			;bring higher star to zenit
	f_sub
	f_store_l	alg_sz3_maxz3
	f_store	alg_maxz3
				
	f_const	flt__0			;Star 1 is lower
	f_push				;calc z3 min guess to
	f_load	alg_alt_rad1			;bring lower star to horizon
	f_sub
	f_store_l	alg_sz3_minz3
	f_store	alg_minz3

alg_sz3_guesses_set:
	f_load_l	alg_sz3_minz3			;Calc separation error for minimum Z3
	f_store	mco_z3			;set Z3 for test

	rcall	alg_sz3_calc_separation
	f_store	alg_minsep

	f_push				;sep_error = cos(sep)-cos(true_sep)
	f_load_l	alg_sz3_truesep
	f_sub
	f_store_l	alg_sz3_minerr

	f_load_l	alg_sz3_maxz3			;Calc separation error for maximum Z3
	f_store	mco_z3			;set Z3 for test

	rcall	alg_sz3_calc_separation
	f_store	alg_maxsep

	f_push				;sep_error = cos(sep)-cos(true_sep)
	f_load_l	alg_sz3_truesep
	f_sub
	f_store_l	alg_sz3_maxerr

	push	AKKU_S			;Error of both Z3 guesses must have
	f_load_l	alg_sz3_minerr			;opposite signs !
	pop	r16
	eor	r16	,AKKU_S
	tst	r16
	brmi	alg_sz3_iteration			;opposite signs, OK

	ldi	r16	,5
	sts	alg_sz3_errno	,r16
	f_load_l	alg_sz3_oldz3			;Something wrong here !
	f_store	mco_z3			;restore old Z
	ldi	zl	,low(alg_sz3_error_5_msg<<1)
	ldi	zh	,high(alg_sz3_error_5_msg<<1)
	call	lcd_print_alarm
	clt				;return false
	rjmp	alg_sz3_return

alg_sz3_error_5_msg:
	.db	"Error 05"
	.db	"????????"

alg_sz3_start_iteration:
	ldi	r16	,12		;12 iteration to come from 90 deg
	std	Y+alg_sz3_counter	,r16		;to 1 arcmin

alg_sz3_iteration:
	f_load_l	alg_sz3_minz3			;simple iteration
	f_push				;Calc mid_Z3 between minz3 and maxz3
	f_load_l	alg_sz3_maxz3
	f_add
	;f_subexp	r16	,1
	ldi	r16	,low(1)	;exponent -1
	sub	AKKU_E2	,r16
	ldi	r16	,high(1)
	sbc	AKKU_E1	,r16
	f_store	mco_z3			;set Z3 for test

	rcall	alg_sz3_calc_separation

	f_push				;sep_error = cos(sep)-cos(true_sep)
	f_load_l	alg_sz3_truesep
	f_sub
	f_store_l	alg_sz3_error

	push	AKKU_S			;check sign of separation error
	f_load_l	alg_sz3_minerr
	pop	r16
	eor	r16	,AKKU_S
	tst	r16
	brmi	alg_sz3_not_new_minz3

	f_load	mco_z3			;sep_error * min_err > 0
	f_store_l	alg_sz3_minz3			;take mid_Z3 as new min_Z3
	f_load_l	alg_sz3_error			;save error as new max_err
	f_store_l	alg_sz3_minerr			;for better regula falsi iteration (later)
	rjmp	alg_sz3_01

alg_sz3_not_new_minz3:
	f_load	mco_z3			;sep_error * min_err < 0
	f_store_l	alg_sz3_maxz3			;take mid_Z3 as new max_Z3
	f_load_l	alg_sz3_error			;save error as new max_err
	f_store_l	alg_sz3_maxerr			;for better regula falsi iteration (later)

alg_sz3_01:
	ldd	r16	,Y+alg_sz3_counter	;decrement counter
	dec	r16
	std	Y+alg_sz3_counter	,r16
	breq	alg_sz3_return_true		;counter = 0, return true

	rjmp	alg_sz3_iteration			;not finished

alg_sz3_return_true:
	ldi	r16	,99
	sts	alg_sz3_errno	,r16
	set

alg_sz3_return:
	ENDLOCAL	alg_sz3_lspace
	ret

;****************************************************************************************

alg_sz3_calc_separation:

	ldi	r16	,high(alg_az_steps1)	;convert steps to az/alt for star 1
	ldi	r17	,low(alg_az_steps1)
	ldi	r18	,high(alg_az_rad1)
	ldi	r19	,low(alg_az_rad1)
	call	alg_mnt_steps2rad

	ldi	r16	,high(alg_az_steps1)	;convert az/alt to vector for star 1
	ldi	r17	,low(alg_az_steps1)
	ldi	r18	,high(alg_mntvec1x)
	ldi	r19	,low(alg_mntvec1x)
	call	mco_rad2vec

	ldi	r16	,high(alg_az_steps2)	;convert steps to az/alt for star 2
	ldi	r17	,low(alg_az_steps2)
	ldi	r18	,high(alg_az_rad2)
	ldi	r19	,low(alg_az_rad2)
	call	alg_mnt_steps2rad

	ldi	r16	,high(alg_az_steps2)	;convert az/alt to vector for star 2
	ldi	r17	,low(alg_az_steps2)
	ldi	r18	,high(alg_mntvec2x)
	ldi	r19	,low(alg_mntvec2x)
	call	mco_rad2vec

	ldi	r16	,high(alg_mntvec1x)	;calc separation
	ldi	r17	,low(alg_mntvec1x)
	ldi	r18	,high(alg_mntvec2x)
	ldi	r19	,low(alg_mntvec2x)
	call	flt_scalar_product		;scalar product = cos(sep)

	ret


;****************************************************************************************
;cos_d = cos (fDecl);
;rkt =  fRectRad - fTimeElapsedIRQs * rad_per_irq;

;m_vSky[0] = cos_d * cos (rkt);
;m_vSky[1] = cos_d * sin (rkt);
;m_vSky[2] = sin ( RAD (m_fDeclDegrees));

alg__rad_per_irq:	.db	0x3E,0xA3,0x1D,0xA7,0xF6,0x87,0x27,0x2C	;1.002737908*2*pi/(24*60*60*128)

alg_sky_to_vec:
	.EQU	alg_s2v_sptr	= 1
	.EQU	alg_s2v_vptr	= 3
	.EQU	alg_s2v_cosd	= 5
	.EQU	alg_s2v_rkt	= 13
	.EQU	alg_s2v_lspace	= 21
	LOCAL	alg_s2v_lspace
	
	std	Y+alg_s2v_sptr	,r16		;ptr > fRect,fDecl,lTime
	std	Y+alg_s2v_sptr+1	,r17
	std	Y+alg_s2v_vptr	,r18		;ptr > X,Y,Z
	std	Y+alg_s2v_vptr+1	,r19

	ldd	xh	,Y+alg_s2v_sptr		;cos_d = cos(fDecl);
	ldd	xl	,Y+alg_s2v_sptr+1
	adiw	xh:xl	,8		;fDecl
	call	flt_load_x
	f_cos
	f_store_l	alg_s2v_cosd			; = cos_d

	;rkt =  fRectRAD - m_fTimeIRQ * rad_per_irq
	ldd	xh	,Y+alg_s2v_sptr
	ldd	xl	,Y+alg_s2v_sptr+1
	adiw	xh:xl	,16		;lTime in IRQs (128 per second)
	ld	AKKU_2	,X+
	ld	AKKU_3	,X+
	ld	AKKU_4	,X+
	ld	AKKU_5	,X+
	call	flt_convert_long			;make double
	f_push
	f_const	alg__rad_per_irq
	f_mul				;convert to radians
	f_chs	r16
	f_push
	ldd	xh	,Y+alg_s2v_sptr
	ldd	xl	,Y+alg_s2v_sptr+1
	call	flt_load_x			; = fRect
	f_add
	f_store_l	alg_s2v_rkt

	f_cos				;X = cos(rkt) * cos_d
	f_push
	f_load_l	alg_s2v_cosd
	f_mul
	ldd	xh	,Y+alg_s2v_vptr
	ldd	xl	,Y+alg_s2v_vptr+1
	call	flt_store_x			; = X
	
	f_load_l	alg_s2v_rkt			;Y = sin(rkt) * cos_d
	f_sin
	f_push
	f_load_l	alg_s2v_cosd
	f_mul
	ldd	xh	,Y+alg_s2v_vptr
	ldd	xl	,Y+alg_s2v_vptr+1
	adiw	xh:xl	,8
	call	flt_store_x			; = Y

	ldd	xh	,Y+alg_s2v_sptr		;Z = sin(fDecl)
	ldd	xl	,Y+alg_s2v_sptr+1
	adiw	xh:xl	,8
	call	flt_load_x			;decl
	f_sin
	ldd	xh	,Y+alg_s2v_vptr
	ldd	xl	,Y+alg_s2v_vptr+1
	adiw	xh:xl	,16
	call	flt_store_x			;Z
	

	ENDLOCAL	alg_s2v_lspace
	ret


;****************************************************************************************
;Convert 	Motor steps into azimut,altitude
;
;r16:r17	az_steps	long
;	alt_steps	long
;
;r18:r19	az_rad	double
;	alt_rad	double
;****************************************************************************************

alg_mnt_steps2rad:
	.EQU	alg_s2r_sptr	= 1
	.EQU	alg_s2r_rptr	= 3
	.EQU	alg_s2r_lspace	= 5
	LOCAL	alg_s2r_lspace
	
	std	Y+alg_s2r_sptr	,r16		;ptr > lAz,lAlt
	std	Y+alg_s2r_sptr+1	,r17
	std	Y+alg_s2r_rptr	,r18		;ptr > fAz,fAlt
	std	Y+alg_s2r_rptr+1	,r19

	ldd	xh	,Y+alg_s2r_sptr		;fAz = lAz * AzRadPerStep
	ldd	xl	,Y+alg_s2r_sptr+1
	ld	AKKU_2	,X+
	ld	AKKU_3	,X+
	ld	AKKU_4	,X+
	ld	AKKU_5	,X+
	call	flt_convert_long			;make double
	f_push
	f_load	mco_az_per_step
	f_mul

alg_s2r_1:
	tst	AKKU_S			;if negative
	brpl	alg_s2r_2

	f_push				;add one turn (2*PI = 360°)
	f_const	flt__2pi
	f_add
	rjmp	alg_s2r_1

alg_s2r_2:
	f_store_array	alg_s2r_rptr	,0		;az [radians]

alg_s2r_3:
	f_push				;subtract one revolution
	f_const	flt__2pi			;save result if still positive
	f_sub
	tst	AKKU_S
	brmi	alg_s2r_4

	f_store_array	alg_s2r_rptr	,0		;az [radians]
	rjmp	alg_s2r_3

alg_s2r_4:
	ldd	xh	,Y+alg_s2r_sptr		;fAlt = lAlt * AltRadPerStep
	ldd	xl	,Y+alg_s2r_sptr+1
	adiw	xh:xl	,4
	ld	AKKU_2	,X+
	ld	AKKU_3	,X+
	ld	AKKU_4	,X+
	ld	AKKU_5	,X+
	call	flt_convert_long			;make double
	f_push
	f_load	mco_alt_per_step
	f_mul
	f_push
	f_load	mco_alt_start
	f_add
	f_push
	f_load	mco_z3
	f_add
	f_store_array	alg_s2r_rptr	,1		;alt [radians]

	ENDLOCAL	alg_s2r_lspace
	ret

;****************************************************************************************
;x13 = x21 * x32 - x31 * x22;
;x23 = x31 * x12 - x11 * x32;
;x33 = x11 * x22 - x21 * x12;
;len = sqrt (x13 * x13 + x23 * x23 + x33 * x33);
;x13 /= len;
;x23 /= len;
;x33 /= len;
;
;r16:r17	adress of matrix
;****************************************************************************************

alg_third_column:
	.EQU	alg_tco_ptr1	= 1
	.EQU	alg_tco_ptr2	= 3
	.EQU	alg_tco_ptr3	= 5
	.EQU	alg_tco_len	= 7
	.EQU	alg_tco_lspace	= 15
	LOCAL	alg_tco_lspace
	
	std	Y+alg_tco_ptr1	,r16		;ptr > first column
	std	Y+alg_tco_ptr1+1	,r17
	mov	xh	,r16
	mov	xl	,r17
	adiw	xh:xl	,24
	std	Y+alg_tco_ptr2	,xh		;ptr > second column
	std	Y+alg_tco_ptr2+1	,xl
	adiw	xh:xl	,24
	std	Y+alg_tco_ptr3	,xh		;ptr > third column
	std	Y+alg_tco_ptr3+1	,xl

	;x13 = x21 * x32 - x31 * x22;
	f_load_array	alg_tco_ptr1	,1
	f_push
	f_load_array	alg_tco_ptr2	,2
	f_mul
	f_push
	f_load_array	alg_tco_ptr1	,2
	f_push
	f_load_array	alg_tco_ptr2	,1
	f_mul
	f_sub
	f_store_array	alg_tco_ptr3	,0		; = X

	;x23 = x31 * x12 - x11 * x32;
	f_load_array	alg_tco_ptr1	,2
	f_push
	f_load_array	alg_tco_ptr2	,0
	f_mul
	f_push
	f_load_array	alg_tco_ptr1	,0
	f_push
	f_load_array	alg_tco_ptr2	,2
	f_mul
	f_sub
	f_store_array	alg_tco_ptr3	,1		; = Y

	;x33 = x11 * x22 - x21 * x12;
	f_load_array	alg_tco_ptr1	,0
	f_push
	f_load_array	alg_tco_ptr2	,1
	f_mul
	f_push
	f_load_array	alg_tco_ptr1	,1
	f_push
	f_load_array	alg_tco_ptr2	,0
	f_mul
	f_sub
	f_store_array	alg_tco_ptr3	,2		; = Z

	ldd	r16	,Y+alg_tco_ptr3
	ldd	r17	,Y+alg_tco_ptr3+1
	mov	r18	,r16
	mov	r19	,r17
	call	flt_scalar_product
	f_sqrt
	f_store_l	alg_tco_len

	f_load_array	alg_tco_ptr3	,0
	f_push
	f_load_l	alg_tco_len
	f_div
	f_store_array	alg_tco_ptr3	,0

	f_load_array	alg_tco_ptr3	,1
	f_push
	f_load_l	alg_tco_len
	f_div
	f_store_array	alg_tco_ptr3	,1

	f_load_array	alg_tco_ptr3	,2
	f_push
	f_load_l	alg_tco_len
	f_div
	f_store_array	alg_tco_ptr3	,2

	ENDLOCAL	alg_tco_lspace
	ret


;****************************************************************************************
;r16	1 = save alignment position 1
;	2 = save alignment position 2
;	0 = save as next position for RMS error calculation
;****************************************************************************************
.CSEG
alg_ana_save:
	lds	r0	,alg_eep_open
	tst	r0
	brne	alg_ans_00

	ret
alg_ans_00:
	dec	r16
	brpl	alg_ans_01

	lds	r16	,alg_eep_counter
	subi	r16	,-2		;add 2
alg_ans_01:
	ldi	zl	,low(eep_ana_array)	;calculate adress for new analysis record
	ldi	zh	,high(eep_ana_array)	;(r1:r0 will be used later)
	ldi	r17	,ALG_ADJ_SIZE
	mul	r16	,r17
	add	zl	,r0
	adc	zh	,r1

	ldi	r17	,1

	cpi	r16	,0		;alignment pos 1 ?
	brne	alg_ans_no_align1			;no

	e_rw_long	alg_az_steps1
	e_rw_long	alg_alt_steps1
	e_rw_long	alg_time1
	e_rw_word	alg_rectint1
	e_rw_word	alg_declint1
	rcall	alg_ans_objname
	rjmp	alg_ans_end

alg_ans_no_align1:
	cpi	r16	,1		;alignment pos 2
	brne	alg_ans_no_align2			;no

	e_rw_long	alg_az_steps2
	e_rw_long	alg_alt_steps2
	e_rw_long	alg_time2
	e_rw_word	alg_rectint2
	e_rw_word	alg_declint2
	rcall	alg_ans_objname
	rjmp	alg_ans_end

alg_ans_no_align2:
	cpi	r16	,2+ALG_ADJ_MAX		;analysis array full ?
	brge	alg_ans_end			;yes

	e_rw_long	alg_az_steps3
	e_rw_long	alg_alt_steps3
	e_rw_long	alg_time3
	e_rw_word	alg_rectint3
	e_rw_word	alg_declint3
	rcall	alg_ans_objname

	adiw	zh:zl	,12		;Z -> RA of next dataset
	ldi	r16	,0xFF
	call	eep_write			;0xFFFF = illegal RA = end of list
	adiw	zh:zl	,1
	call	eep_write

	lds	r16	,alg_eep_counter
	inc	r16			;no of records ++
	sts	alg_eep_counter	,r16

alg_ans_end:
	ret

alg_ans_objname:
	ldi	xl	,low(obj_ds_name)		;object name
	ldi	xh	,high(obj_ds_name)
	ldi	r17	,1		;1=write
	ldi	r16	,8		;8 bytes
	jmp	eep_rw_block			;write eeprom


	

;****************************************************************************************
;Calculate RMS position errors of up to 32 reference stars
;
;After Z1 or Z2 have been changed, the alignment is done again on the first two stars
;Then this subroutine calculates the errors of up to 32 more positions
;
;alg_adj_array	base adress of analysis arrays having records of the form:
;	az_steps	4 byte  precision 1 microstep = 1/64 fullstep)
;	alt_steps	4 byte
;	time	4 byte  precision 1/128 second) 
;	rightascension	2 byte  precision 20 arcseconds)
;	declination	2 byte            10 arcseconds)
;	object name	8 byte
;****************************************************************************************
.CSEG
alg_analysis:
	.EQU	alg_ana_dummy	= 1
	.EQU	alg_ana_ptr	= 2

	.EQU	alg_ana_rectrad	= 4
	.EQU	alg_ana_declrad	= 12
	.EQU	alg_ana_time	= 20

	.EQU	alg_ana_azst	= 24
	.EQU	alg_ana_altst	= 28
	.EQU	alg_ana_azrad	= 32
	.EQU	alg_ana_altrad	= 40
	.EQU	alg_ana_objname	= 48

	.EQU	alg_ana_mntvec1x	=  56
	.EQU	alg_ana_mntvec1y	=  64
	.EQU	alg_ana_mntvec1z	=  72
	.EQU	alg_ana_skyvec1x	=  80
	.EQU	alg_ana_skyvec1y	=  88
	.EQU	alg_ana_skyvec1z	=  96
	.EQU	alg_ana_skyvec2x	= 104
	.EQU	alg_ana_skyvec2y	= 112
	.EQU	alg_ana_skyvec2z	= 120
	.EQU	alg_ana_crossvecx	= 128
	.EQU	alg_ana_crossvecy	= 136
	.EQU	alg_ana_crossvecz	= 144
	.EQU	alg_ana_lspace	= 152
	LOCAL	alg_ana_lspace

	f_const	flt__0			;error sum = 0
	f_store	alg_ana_errsum
	ldi	zh	,high(eep_ana_array+2*ALG_ADJ_SIZE)	;start of analysis array
	ldi	zl	,low(eep_ana_array+2*ALG_ADJ_SIZE)
	std	Y+alg_ana_ptr	,zh
	std	Y+alg_ana_ptr+1	,zl

alg_ana_loop:
	ldd	zh	,Y+alg_ana_ptr
	ldd	zl	,Y+alg_ana_ptr+1
	adiw	zh:zl	,12		;Z -> Rightascension
	call	eep_read
	cpi	r16	,0xFF
	brne	alg_ana_01

	adiw	zh:zl	,1
	call	eep_read
	cpi	r16	,0xFF		;0xFFFF = illegal RA = end of list
	brne	alg_ana_01

	rjmp	alg_ana_exit

alg_ana_01:
	ldd	zh	,Y+alg_ana_ptr
	ldd	zl	,Y+alg_ana_ptr+1

	movw	xh:xl	,yh:yl
	adiw	xh:xl	,alg_ana_azst		;X = RAM adress
	ldi	r17	,0		;0=read
	ldi	r16	,4		;4 bytes
	call	eep_rw_block			;read eeprom
	
	movw	xh:xl	,yh:yl
	adiw	xh:xl	,alg_ana_altst
	ldi	r17	,0
	ldi	r16	,4
	call	eep_rw_block
	
	movw	xh:xl	,yh:yl
	adiw	xh:xl	,alg_ana_time
	ldi	r17	,0
	ldi	r16	,4
	call	eep_rw_block
	
	clr	AKKU_2
	clr	AKKU_3
	call	eep_read			;alg_rectint
	mov	AKKU_4	,r16
	adiw	zh:zl	,1
	call	eep_read			;alg_rectint+1
	mov	AKKU_5	,r16
	adiw	zh:zl	,1

	std	Y+alg_ana_ptr	,zh
	std	Y+alg_ana_ptr+1	,zl

	call	flt_convert_long
	f_push
	f_const	alg__db2rect
	f_mul
	f_store_l	alg_ana_rectrad

	ldd	zh	,Y+alg_ana_ptr
	ldd	zl	,Y+alg_ana_ptr+1

	call	eep_read			;alg_declint
	mov	AKKU_2	,r16
	adiw	zh:zl	,1
	call	eep_read			;alg_declint+1
	mov	AKKU_3	,r16
	adiw	zh:zl	,1

	movw	xh:xl	,yh:yl
	adiw	xh:xl	,alg_ana_objname		;object name
	ldi	r17	,0
	ldi	r16	,8
	call	eep_rw_block

	std	Y+alg_ana_ptr	,zh
	std	Y+alg_ana_ptr+1	,zl

	call	flt_convert_word
	f_push
	f_const	alg__db2decl
	f_mul
	f_store_l	alg_ana_declrad

	ldi	r16	,0		;convert az/alt steps to radians
	ldi	r17	,alg_ana_azst		;including start altitude and Z3
	add	r17	,yl
	adc	r16	,yh
	ldi	r18	,0
	ldi	r19	,alg_ana_azrad
	add	r19	,yl
	adc	r18	,yh
	call	alg_mnt_steps2rad

	f_load_l	alg_ana_azrad
	f_load_l	alg_ana_altrad


	ldi	r16	,0		;convert az/alt into mount vector
	ldi	r17	,alg_ana_azst		;including Z1 and Z2
	add	r17	,yl
	adc	r16	,yh
	ldi	r18	,0
	ldi	r19	,alg_ana_mntvec1x
	add	r19	,yl
	adc	r18	,yh
	call	mco_rad2vec

	ldi	r16	,high(alg_mat_mnt2sky)	;convert mount vector into sky vector 1
	ldi	r17	,low(alg_mat_mnt2sky)
	ldi	r18	,0
	ldi	r19	,alg_ana_mntvec1x
	add	r19	,yl
	adc	r18	,yh
	ldi	r20	,0
	ldi	r21	,alg_ana_skyvec1x
	add	r21	,yl
	adc	r20	,yh
	call	flt_mat_vec_mul

	ldi	r16	,0		;convert RA,DEC,time into sky vector 2
	ldi	r17	,alg_ana_rectrad
	add	r17	,yl
	adc	r16	,yh
	ldi	r18	,0
	ldi	r19	,alg_ana_skyvec2x
	add	r19	,yl
	adc	r18	,yh
	call	alg_sky_to_vec

	ldi	r16	,0		;length(cross product) = sin (error angle)
	ldi	r17	,alg_ana_skyvec1x
	add	r17	,yl
	adc	r16	,yh
	ldi	r18	,0
	ldi	r19	,alg_ana_skyvec2x
	add	r19	,yl
	adc	r18	,yh
	ldi	r20	,0
	ldi	r21	,alg_ana_crossvecx
	add	r21	,yl
	adc	r20	,yh
	call	flt_cross_product

	ldi	r16	,0		;scalar product = length ** 2
	ldi	r17	,alg_ana_crossvecx
	add	r17	,yl
	adc	r16	,yh
	ldi	r18	,0
	ldi	r19	,alg_ana_crossvecx
	add	r19	,yl
	adc	r18	,yh
	call	flt_scalar_product
	f_sqrt				;length

	f_push
	f_const	flt__rad2arcmin			;convert to arc minutes
	f_mul

	ldi	r17	,8
	movw	xh:xl	,yh:yl
	adiw	xh:xl	,alg_ana_objname
alg_ana_02:
	ld	r16	,X+
	call	putc_func
	dec	r17
	brne	alg_ana_02

	PUTC	' '
	PUTC	':'
	PUTC	' '
	call	flt_print
	PUTC	13
	f_push
	f_mul				;square
	f_push
	f_load	alg_ana_errsum
	f_add
	f_store	alg_ana_errsum

	rjmp	alg_ana_loop

alg_ana_exit:
	clr	AKKU_2
	lds	AKKU_3	,alg_eep_counter
	tst	AKKU_3
	breq	alg_ana_end

	call	flt_convert_word
	f_push
	f_load	alg_ana_errsum
	f_xy
	f_div
	f_sqrt
	f_store	alg_ana_errsum
	PUTC	13
	PUTS_ROM	msg_alg_ana_sum
	call	flt_print
	PUTC	13

alg_ana_end:
	ENDLOCAL	alg_ana_lspace
	set
	ret

;****************************************************************************************