PRO RANSAC4
;-------Last changed on March 19, 2009 by Shelby Frisch.

Common Block1,a0,a1,b1,vrr,theta ;The a's are the coeff  of the model, a0 +a1*sin(theta)+b1*cos(theta)
COMMON Block2,flag_thresh,rnsc_win_width ; flag_thresh is the Vr thresh above which data is good
       ; rnsc_win_width is the width of the window in m/s that data is accepted about the sin cos fit

COMMON Block3,err,nmpts ; err is the std dev of the accepted data from the fit of the model
COMMON Block4,flag ; if not enough good data points are found, flag=0
COMMON Block5,ht,outfile_1 ; ht is the height of the range gate , outfile_1 is the output data file path
COMMON Block7,count ; count is the number of points used in the final ransac output,
COMMON Block8,el ; this is the elevation angle of the scan


Height=ht

;Flag is set 0, no calculations were made, flag=1 coef were ok
;--------------flag_thresh---------------
	;the data flag is for known bad data the flag_thresh should be less than this

	flag_thresh=-20.
;	dzzthreshold=-20.0

;---------------rnsc_win_width------------------
	; This should be some fraction of nyquist,

	rnsc_win_width=1.0


	;----------------err-------------------
	;This is the rms difference between the fitted curve and the actual ransac selected data
;------Set flag =0 initially, if the calculations are made for the coef, then flag will be set to 1.
flag=0
err=0
nmpts=0

	seed_value1=4L
	seed_value2=4L
	seed_value3=4L
	;--- this is an initial guess for the difference between the coef fit and selected data

	diff1 =10000

	diff=FLTARR(360)
	buffer=FLTARR(360)
	y=FLTARR(360)
	ymax=FLTARR(360)
	ymin=FLTARR(360)
	v=FLTARR(360)
	theta=INDGEN(360)*.017444
	theta1=INDGEN(360)
	ab0=FLTARR(1000)
	ab1=FLTARR(1000)
	ab2=FLTARR(1000)
	aa0=FLTARR(1000)
	aa1=FLTARR(1000)
	bb1=FLTARR(1000)
	vr=FLTARR(360)

	msum=FLTARR(1000)


	vr=vrr
; the plot here is just to see the data as its being processed, not necessary for the calculations, just handy for reviewing data
plot,theta,vr, MIN_VALUE=-20,MAX_VALUE=20
;-------------find good data

	index4=WHERE(vr GT flag_thresh,count)



IF (count GT 100 ) THEN BEGIN

		thtsmpl1=0
		thtsmpl2=0
		thtsmpl3=0
;
seed_value1=1.25

; --------Start the random selection of 3 azimuth angles and window the data for k=0,n times, select the minimum error
FOR k=0,200 DO BEGIN

	index0=WHERE(vr GT flag_thresh,count	)
		ia=RANDOMU(seed_value1)*(count)
		thtsmpl1=Index0(ia)

	index1=WHERE(vr GT flag_thresh,count)
		ib=RANDOMU(seed_value1)*(count)
		 thtsmpl2=Index1(ib)

	index2=WHERE(vr GT flag_thresh,count)
		ic=RANDOMU(seed_value1)*(count)
		 thtsmpl3=Index2(ic)

;-----The 3 angles must all be different to solve for a0,a1 and b1

 IF (thtsmpl1 NE thtsmpl2 AND thtsmpl1 NE thtsmpl3 AND thtsmpl3 NE thtsmpl2 ) THEN BEGIN

;----------------------------------------------------------------------


	kk=0

; can print the angles for the fun of it------ or comment the printing  out------
;	print,thtsmpl1,thtsmpl2,thtsmpl3

;-The next steps are to simplify the program statements by calculating the sin and cos of the samples

   	alpha1=sin(theta[thtsmpl1])
   	alpha2=sin(theta[thtsmpl2])
	alpha3=sin(theta[thtsmpl3])

	beta1=cos(theta[thtsmpl1])
	beta2=cos(theta[thtsmpl2])
	beta3=cos(theta[thtsmpl3])

	v1=vr[thtsmpl1]
	v2=vr[thtsmpl2]
	v3=vr[thtsmpl3]



;--------Compute the coef from the 3 samples

	AA=(alpha2*beta3-beta2*alpha3)
	BB=(alpha2*beta1-beta2*alpha1)

	ab0(k)=((v3*alpha2-v2*alpha3)*BB-(v1*alpha2-V2*alpha1)*AA)/($
		(alpha2-alpha3)*BB-(alpha2-alpha1)*AA)

	numerater1=(v1-v2)/(beta1-beta2)-(v3-v2)/(beta3-beta2)
	denominator1=(alpha1-alpha2)/(beta1-beta2)-(alpha3-alpha2)/(beta3-beta2)

	ab1(k)=numerater1/denominator1

	numerater2=(v1-v2)/(alpha1-alpha2)-(v3-v2)/(alpha3-alpha2)
	denominator2=(beta1-beta2)/(alpha1-alpha2)-(beta3-beta2)/(alpha3-alpha2)

	ab2(k)=numerater2/denominator2


;--------------------------

;;------compute the model for Vr (The center of the window)

	FOR j=0,359 DO BEGIN
		y(j)=ab0(k)+ab1(k)*sin(theta(j))+ab2(k)*cos(theta(j))
	ENDFOR

;-----------set up the window width---------

		ymax=y+rnsc_win_width/2.0
		ymin=y-rnsc_win_width/2.0

;---------Find the points in the window-------that is the window centered about the current model y

index=WHERE(vr LT ymax AND vr GT ymin, count)
;----------count is the number of points within the window to be used in the next calculation


	;-------------------------------Fit the windowed data set to the model--

		sum0=0
		sum1=0
		sum2=0
 IF count GT 3 THEN BEGIN
	;------get aa0----------------------------------
	FOR j=0,count-1 DO BEGIN
		sum0=sum0+vr[index[j]]
	ENDFOR
		aa0(k)=sum0/count
	FOR j=0,count-2 DO BEGIN

		sum1=sum1+0.32*(vr[index[j]]+vr[index[j+1]])*(theta[index[j+1]]-theta[index[j]])*$
		(sin(theta[index[j]])+sin(theta[index[j+1]]))/4.0


		sum2=sum2+0.32*(vr[index[j]]+vr[index[j+1]])*(theta[index[j+1]]-theta[index[j]])*$
		(cos(theta[index[j]])+cos(theta[index[j+1]]))/4.0


	ENDFOR

	;------------------compute the coeff of sin and cos to pass on to the vad program after min
		;----------the differences from all the trials

		aa1(k)=sum1+0.32*(vr[index[count-1]]+vr[index[0]])*(vr[index[count-1]]-vr[index[0]])*$
		(sin(theta[index[0]])+sin(theta[index[count-1]]))/4.0
		bb1(k)=sum2+0.32*(vr[index[count-1]]+vr[index[0]])*(vr[index[count-1]]-vr[index[0]])*$
		(cos(theta[index[0]])+cos(theta[index[count-1]]))/4.0

;	print,'aa0=',aa0(k),'aa1=',aa1(k),'bb1=',bb1(k)

	FOR j=0,359 DO BEGIN
		V(j)=aa0(k)+aa1(k)*sin(theta(j))+bb1(k)*cos(theta(j))
	ENDFOR
;----------comput the squared difference between the windowed data and the model computed from the windowed data
		diff[index]=(V(index)-vr(index))^2
;---------find the mean sum of the difference
		msum(k)=MEAN(diff(index))
;

ENDIF
ENDIF
ENDFOR
;--------------Find the smallest difference in all the tials between the windowed model and the actual data in the window.

	index_min=WHERE(msum GT 0, count_min)
	result=MIN(msum[index_min])
	index_a=WHERE(msum EQ result)

;------index_a is the tial number of the minimum difference



	;OPLOT,theta,v







;------output values---------------The coef (a's) are selected from the minimum of the fit of the data
; They are returned to the main program for calculating the u and v components of velocity for 1 range gate

	err=sqrt(msum[index_a])

	a0=aa0[index_a]
	a1=aa1[index_a]
	b1=bb1[index_a]
	flag=1
ENDIF
; --------This section is for plotting out the ransac vr and fit for diagnositcs ---can be removed----------


	IF Flag EQ 1 THEN BEGIN
			ndim=SIZE(a0)
		IF ndim[1] GT 1 THEN BEGIN
			a0=a0[0]
			a1=a1[0]
			b1=b1[0]
		ENDIF

	FOR j=0,359 DO BEGIN
		V(j)=a0+a1*sin(theta(j))+b1*cos(theta(j))
	ENDFOR

	;;------compute the model for Vr (The center of the window)

	FOR j=0,359 DO BEGIN
		y(j)=a0+a1*sin(theta(j))+b1*cos(theta(j))
	ENDFOR

;-----------set up the window width---------

		ymax=y+rnsc_win_width
		ymin=y-rnsc_win_width
;---------Find the points in the window-------

		index=WHERE(vr LT ymax AND vr GT ymin,count)

IF COUNT GT 2 THEN BEGIN

		ymax=MAX(vr(index))
		ymin=MIN(vr(index))

		Height=STRING(ht)
		elev=STRING(el)


 	Set_Plot, 'ps'
  	device, filename=outfile_1+' '+STRING(Height)+'.ps'

;---------plotting

label=STRING(outfile_1)+''+'height='+STRING(ht)+'el ang='+elev
;label='height='+STRING(ht)+'el ang='+elev
sublabel='error='+STRING(result)+'   '+'elevation='+elev

   	PLOT,CHARSIZE=0.5,theta(index),vr(index), MIN_VALUE=-20,MAX_VALUE=20,$
   		xtitle='azimuth(radians)',yTitle='Vr(m/s)', Title=label,$
   		SUBTITLE=sublabel,PSYM=2,YRANGE=[ymin,ymax]
   	OPLOT,theta,v
   	OPLOT,theta(index4),vr(index4), PSYM=4

;-------end of plotting

  	device,/close
	Set_Plot,'WIN'

ENDIF

ENDIF



END