%%Justin Rice, April 30, 2005
%Cornell University
%jkr25@cornell.edu
%please do not reproduce code with out acknowledgement


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%user changeable values:
klimit=7;% number of zoom ins
alpha=5;%grid size
residuallimit=0.000000015;%maximum allowable least squares fitting error
%%%%%%%%%%%%%%%%%%%%%%%%%%end user changeable values





residual_error=0;
data=datafortest;
tstep=1;     %hour
starttime=0;
startA=0*pi/180;
startB=-1;
endA=90*pi/180;
endB=1;
endtime=24;
A=startA;
B=startB;
goodB=0;
goodA=90*pi/180;
T=0;
i=1;
k=1;
previousrangeA=endA-startA;
previousrangeB=endB-startB;
while(k<=klimit)
    
    while(A<=endA)%search through A's
        B=startB;
        T=starttime;
        while(B<=endB)%search through B's
            T=starttime;
            
            while(T<=endtime)%search for starting time
                %(T is starting time of data)
                j=1;
                f=f+1;
                while(j<=length(data))
                    pAltitude(j) = asin(sin(B) * sin(A) + cos(B) * cos(A) * cos((15 * (T+tstep*(j-1) - 12)) * (pi / 180)));
                    pAzimuth(j) = (acos((cos(A) * sin(B) - cos(B) * sin(A) * cos((15 * (T+tstep*(j-1) - 12)) * (pi / 180))) / cos(pAltitude(j))));
               
                    if(T+tstep*(j-1)>12)
                        pAzimuth(j)=-pi+pAzimuth(j);
                    else
                        pAzimuth(j)=pi-pAzimuth(j);
                    end
                    j=j+1;
            
                end
            
                j=1;
                serror=0;
                azerror=0;
                alterror=-0;
                while(j<=length(data))
                    Altitudeerrorsquared=(pAltitude(j)-data(j,2))^2;
                    Azimutherrorsquared=(pAzimuth(j)-data(j,3))^2;
                    totalerror=Altitudeerrorsquared+Azimutherrorsquared;
                    j=j+1;
                    serror=serror+totalerror;
                    alterror=Altitudeerrorsquared+alterror;
                    azerror=Azimutherrorsquared+azerror;
                end
                results(i,1)=[i];
                results(i,2)=[A];
                results(i,3)=[B];
                results(i,4)=[T];
                results(i,5)=[serror];
                results(i,6)=alterror;
                results(i,7)=azerror;
            
            
                T=T+1;
                i=i+1;
            end
            B=B+(endB-startB)/alpha;
        end
        A=A+(endA-startA)/alpha;
    end
    %%search results

    low=1000;
    i=1;
    while(i<=length(results))
        if(results(i,5)<low)
            low=results(i,5);
            goodA=results(i,2);
            goodB=results(i,3);
            goodtime=results(i,4);
            j=i;
        end
    i=i+1;
    end
    previousrangeA=endA-startA;
    previousrangeB=endB-startB;
    goodtime=goodtime;
    goodA=goodA;
    goodB=goodB;
    starttime=goodtime-2;
    endtime=goodtime+2;
    startA=goodA-(previousrangeA/alpha)/2;
    endA=goodA+(previousrangeA/alpha)/2;
    startB=goodB-(previousrangeB/alpha)/2;
    endB=goodB+(previousrangeB/alpha)/2;
    residual_error=results(j,5);
    A=startA;
    B=startB;
    kerror(k)=residual_error;
    if(residual_error>=residuallimit)
        k=k+1;
    else
        break
    end
end
%south is 0 degrees azimuth
fDeclination=goodB;
latitude=goodA;
time=[0:1:24];
i=1;
while(time(i)<24)
    fAltitude = asin(sin(fDeclination) * sin(latitude) + cos(fDeclination) * cos(latitude) * cos((15 * (time(i) - 12)) * (pi / 180)));
    fAzimuth = (acos((cos(latitude) * sin(fDeclination) - cos(fDeclination) * sin(latitude) * cos((15 * (time(i) - 12)) * (pi / 180))) / cos(fAltitude)));
    Altitudefound(i)=fAltitude*180/pi;
    if(time(i)>12)
        Azimuthfound(i)=-180+fAzimuth*180/pi;
    else
        Azimuthfound(i)=180-fAzimuth*180/pi;
    end
    i=i+1;
end
Altitudefound(i)=Altitudefound(i-1);
Azimuthfound(i)=Azimuthfound(i-1);
i=1;
while(i<=length(datafortest))
data(i, 1)=datafortest(i, 1);
data(i, 2)=datafortest(i, 2)*180/pi;
data(i, 3)=datafortest(i, 3)*180/pi;
i=i+1;
end
%now predict your next couple of positions:
i=1;
time=[0:1:24];
while(time(i+goodtime+length(data))<=(goodtime+length(data)+4))
    fAltitude = asin(sin(fDeclination) * sin(latitude) + cos(fDeclination) * cos(latitude) * cos((15 * (time(i+goodtime+length(data)) - 12)) * (pi / 180)));
    fAzimuth = (acos((cos(latitude) * sin(fDeclination) - cos(fDeclination) * sin(latitude) * cos((15 * (time(i+goodtime+length(data)) - 12)) * (pi / 180))) / cos(fAltitude)));
    Altitudenext(i)=fAltitude*180/pi;
   
    if(time(i+goodtime+length(data) ) >12)
        Azimuthnext(i)=-180+fAzimuth*180/pi;
    else
        Azimuthnext(i)=180-fAzimuth*180/pi;
    end
    i=i+1;
end
Altitudenext=Altitudenext;
Azimuthnext=Azimuthnext;
goodtime=goodtime;
foundlatitude=goodA;
plot(Azimuth, Altitude, 'b', Azimuthfound,Altitudefound, 'r', data(:,3), data(:,2), 'r*' , Azimuthnext, Altitudenext, 'm+')
Xlabel('Azimuth')
Ylabel('Altitude')
Legend('actual curve of sun', 'curve fit path of sun', 'data points used for curve fit', 'predicted points')
%now find predicted errors:
i=1;
while(i<=length(Altitudenext))
error(i,1)=abs(Azimuth(goodtime+length(data)+i)-Azimuthnext(i));
error(i,2)=abs(Altitude(goodtime+length(data)+i)-Altitudenext(i));
i=i+1;
end
error

number_of_search_points=f
k=1:1:length(kerror);
pause
semilogy(k, kerror)
ylabel('residual error')
xlabel('number of zoom ins(k)')