%This function determines which state we are in
%right now.
function S=TheState(phi,psi)
%rad_phi = phi*180/3.1416;
%rad_psi = psi*180/3.1416;
rad_phi = phi;
rad_psi = psi;
if rad_phi > 180
    rad_phi = rad_phi - 360;
end
if rad_psi > 180
    rad_psi = rad_psi - 360;
end
S=0;
if (rad_phi>-180) && (rad_phi<-60)
    if (rad_psi>110) && (rad_psi<180)%60
        S=1; %state C7eq
    end
    if (rad_psi>-180) && (rad_psi<-150)%(rad_psi>-180) && (rad_psi<-130)
        S=1; %state C7eq
    end
end
if (rad_phi>-110) && (rad_phi<-60)
    if (rad_psi>45) && (rad_psi<180)%60
        S=1; %state C7eq
    end
end
if (rad_phi>160) && (rad_phi<180)
    if (rad_psi>-180) && (rad_psi<-150)%60
        S=1; %state C7eq
    end
end
if (rad_phi>-110) && (rad_phi<-60)
    if (rad_psi>-70) && (rad_psi<-35)%(rad_psi>-30) && (rad_psi<0)
        S=2; %state alpha_R
    end
end