//============================================================
// 
//             Bureau d'études :  UNIVERSPACE
//             ============================== 
//
//============================================================  
// Script scilab pour :
//  -calculer les paramètres orbitaux pour une orbite héliosynchrosne 
//   phasée et gelée (orbites phasées 1 et 2 jour). Un groupe peut tracer 
//   l orbite un jour l autre 2 jours (orbites Mistigri)
//  -calculer les caracteristiques des orbites
//  -extrapoler les orbites
//  -tracer les orbites
//  -estmimer la consommation d ergol
//
//
//              CNES - DCT/SB/MS
//
//============================================================
// Historique
// V1   Version initiale 03/03/2015
//============================================================

//============================================================
//    Caractéristiques des orbites phasées hélio gelées
//============================================================

// Données d'entrée pour une mission heliosynchrone phasee :
t0 = CL_dat_cal2cjd(2016,7,9); 
// Recherche des orbites héliosynchrones phasées 1 ou 2 jours entre 500 et 800km
alt_min = 500.e3; //Zone de recherche 
alt_max = 800.e3;
Qmin = 1; //jour
Qmax = 2; //jours
ecc = 0.001; //Excentricité gelée
sso = 1; //Sun Synchronous Orbit
phasage = CL_op_searchRepeatOrbits(%CL_eqRad+alt_min,%CL_eqRad+alt_max,Qmin,Qmax,ecc,sso);

// Calcul de l'ascension droite pour que le plan de l'orbite ait la bonne heure locale
hloc = 13; // heure locale désirée
gom = CL_op_locTime(t0, 'mlh', hloc, 'ra'); // hloc en heure

//Calcul de l'excentricité gelée (optionnel)
dga = phasage(:,1)';
inc = phasage(:,3)';
[ecc, pom] = CL_op_frozenOrbit(dga, inc);

//anomalie moyenne
anm = [-%pi/2, -%pi/2];


//============================================================
//  Calcul des caractéristiques de l'orbite keplerienne
//============================================================

//Calcul de la période képlerienne
per = CL_kp_params('per',dga);

//Transformation anomalie moyenne vers anomalie vraie 
v = CL_kp_M2v(ecc,anm);

//Caractéristiques de l'orbite képlerienne
res = CL_kp_characteristics(dga, ecc,v);

//============================================================
//  Calcul des caractéristiques de l'orbite kepler + J2
//============================================================

//Intertrace orbitale (J2) et periode nodale (J2) 
[lgap, nodper] = CL_op_paramsJ2(['lgap', 'nodper'],dga, ecc, inc);

//============================================================
//         Extrapolation des orbites
//============================================================

step = 30 / 86400; // pas de propagation 
t = t0:step:t0+2;
kep_t1 = CL_ex_secularJ2(t0, [dga(1), ecc(1), inc(1), pom(1), gom(1), anm(1)]', t); // propagation d orbite

pos_eci1 = CL_oe_kep2car(kep_t1);      // position cartesienne en repere inertiel
pos_ecf1 = CL_fr_convert("ECI", "ECF", t, pos_eci1); // position en repere terrestre 


kep_t2 = CL_ex_secularJ2(t0, [dga(2), ecc(2), inc(2), pom(2), gom(1), anm(2)]', t); // orbit propagation

pos_eci2 = CL_oe_kep2car(kep_t2);      // position cartesienne en repere inertie
pos_ecf2 = CL_fr_convert("ECI", "ECF", t, pos_eci2); // position en repere terrestre
						     
//============================================================
//         Tracé des orbites
//============================================================						     
						     						     
scf();
CL_plot_ephem(pos_ecf1, color_id = 3);
CL_plot_earthMap();
a = gca();
a.title.text = "Orbite phasée 2 jours";

scf();
CL_plot_ephem(pos_ecf2, color_id = 5);
CL_plot_earthMap();
a = gca();
a.title.text = "Orbite phasée 1 jour";

//============================================================
//            Ecriture des résultats
//============================================================

entete = [  "Eléments orbitaux [km, deg]"
            "dga, exx, inc, argp, adna anom"  ];
params =[];
//Convertion de format pour écriture 
params(1,:) = dga/1e3; //km
params(2,:) = ecc; //km  
params(3,:) = inc * %CL_rad2deg; // deg 
params(4,:) = pom * %CL_rad2deg; // deg  
params(5,:) = gom * %CL_rad2deg; // deg  
params(6,:) = anm * %CL_rad2deg; // deg  
str = [ msprintf("\n%6.6f %6.6f\n", params) ];
  
str = [entete; str];
nomfic = "orbites.txt";  
mputl(str, nomfic);
//disp(str)

//============================================================
//           Budget d'ergol
//============================================================
//Correction des erreurs d injection
Ddga = [10e3, 10e3];
[deltav_mip, dv_mip, anv] = CL_man_dvSma(dga, ecc ,dga + Ddga);

//Cout du maintien à poste (demi grand axe)
alt = dga - %CL_eqRad;
vel = [sqrt(%CL_mu./ dga); zeros(alt); zeros(alt)];
rho = CL_mod_atmUS76Density(alt);
surface = 0.95; //m2
mass = 200; //kg
coefd = 2.7 * surface ./ mass;
acc = CL_fo_dragAcc(vel, rho, [coefd, coefd]);
//Incrément de vitesse = accélération * temps (3 ans)
deltav_map = CL_norm(acc) * 86400 * 365 * 3;

//Manoeuvre de désorbitation
//Orbite rentrant en 25 ans (calcul STELA)
//6991.82750145 0.00120477 98.2693 90.0005859052 122.152359 270
[deltav_desorb, dv_desorb, anv] = CL_man_dvSma(dga(1), 0, 6991.82750145*1000);
deltav_desorb = [deltav_desorb, 0];

//Delta V total (m/s)
deltav_tot = deltav_mip + deltav_map + deltav_desorb;

//Calcul de la consommation (kg)
conso_tot = CL_man_consumption('dm', deltav_tot, mass, 200);

//============================================================
//             Couverture et ouverture senseur
//============================================================
dlon = CL_op_paramsJ2('lgap',dga,ecc,inc); //Ecart entre 2 traces consécutives
dlon(1) = dlon(1)/2; //Orbite phasée 2 jours
//Angle au centre pour couvrir dlon/2
cen = CL_op_equatorialSwath("dlon2cen", dga, inc, dlon/2);
//ouverture instrument
sat = CL_gm_visiParams(dga, ones(dga).*%CL_eqRad,'cen', cen,'sat');
//Vérifier avec la démo
CL_rad2deg(sat)

//============================================================
//             Enchainement
//============================================================
//============================================================
//              Simulation CIC (exemple)
//============================================================  
// Script d'exemple illustrant l'utilisation des fonctions 
// de celestLab en vue de générer les fichiers nécessaires
// pour les sessions d'ingéniérie concourrante (CIC)
// Ce script ne produit pas tous les fichiers nécessaires mais
// uniquement quelques exemples sélectionnées :
//  - fichier position vitesse
//  - fichier quaternions d'attitude
//  - fichier de visibilité géométrique des stations
//  - fichier d'éclipse (visibilité soleil)
//  - fichier direction soleil en repère sat
//  - fichier direction Terre en repère sat
//  - fichier coordonnées géographiques
//
//  Se placer dans le répertoire demo_CIC.sce
//  CL_init()
//  exec('demo_CIC.sce');
//
//
//              CNES - DCT/SB/MS
//
// Historique
//============
//
// V1   Version historique
// V2   06/06/2014 Mise à jour pour CelestLab V3
//============================================================

//----------------------------------------------------------
// Chargement des constantes et des fonctions 
//----------------------------------------------------------

///////////CL_init();  // Chargement des constantes de CelestLab (cf help CL_init()
exec("cic_ecrire.sce");  // Chargement des fonctions d'écriture

//----------------------------------------------------------
// Structures qui contiennent des données et les résultats 
//----------------------------------------------------------

data = struct();
res = struct();

//-----------------------------------------------------------
// Période de simulation
//-----------------------------------------------------------

// Date de debut, pas, durée de simu
tdeb = CL_dat_cal2cjd(2016,7,9);         //Converti date en jour julien 1950

pas = 10; // secondes
duree = 1; // jour

// dates de simulation
data.t = t0 + [0 : pas/86400 : duree];

//-----------------------------------------------------------
// Stations  
//-----------------------------------------------------------
noms = ["Aussaguel", "Kourou" "Svalbard"];  

// nom, coordonnées géodésiques, elevmin
data.stations.name = ["Aussaguel", "Kourou", "Svalbard"];
data.stations.coord = [[1.499* %CL_deg2rad; 43.43* %CL_deg2rad; 154.0] , [-52.64* %CL_deg2rad; 5.1* %CL_deg2rad; 94.0], [15.38* %CL_deg2rad; 78.21* %CL_deg2rad; 200.0]];   

data.stations.elevmin = [0, 0, 0]; //Masques des 3 stations


//-----------------------------------------------------------
// Soleil  
//-----------------------------------------------------------
[pos_sun, vel_sun] = CL_eph_sun(data.t);         //The default output frame is ECI. 
M = CL_fr_convertMat("ECI", "EME2000", data.t);    //Matrice de transformation de ECI à EME2000

data.pos_sun=M*pos_sun;

//-----------------------------------------------------------
// Orbite  
//-----------------------------------------------------------
////////kep = [ 7098088.4;     // demi grand axe (m)            Bulletin en repère Veis
////////        0.0010400;     // excentricité
////////        1.7151228;     // inclinaison (rad)
////////        1.5707963;     // petit omega (rad)
////////        2.1319608;     // raan (rad)  
////////       -1.5707963 ];   // anomalie moyenne (rad)
////////       
kep = [ dga(1);     // demi grand axe (m)            Bulletin en repère Veis
        ecc(1);     // excentricité
        inc(1);     // inclinaison (rad)
        pom(1);     // petit omega (rad)
        gom(1);     // raan (rad)  
        anm(1) ];   // anomalie moyenne (rad)
////////

[pos,vel]=CL_oe_kep2car(kep);                                           //Passage de képlérien à cartésien
[pos,vel] = CL_fr_convert("Veis", "EME2000", tdeb, pos,vel);  //Changement de repère de Veis à EME2000

cir = CL_oe_car2cir(pos,vel);           //Passage de cartésien à circulaire

[cir_mean,cir_osc] = CL_ex_eckHech(tdeb,cir,data.t); 
[res.pos,res.vel]  = CL_oe_cir2car(cir_osc);        

//-----------------------------------------------------------
// Attitude  
//-----------------------------------------------------------
// Attitude 1 : Z pointé terre, X vers la vitesse.
M = CL_fr_locOrbMat(res.pos,res.vel,"sWQ"); //matrice de transformation vers le repere orbital local 
q1 = CL_rot_matrix2quat(M);                 // Création du quaternion (de rotation) équivalent à la matrice

// Attitude 2 : Z pointé terre, X 'au mieux' vers le Soleil
// (On pointe Z vers '-pos' et on pointe X au mieux vers (pos_sun-pos))
[q2, Inok] = CL_rot_defRotVec([0;0;1], [1;0;0], -res.pos, (data.pos_sun-res.pos));

// Attitude = Attitude nominal pour 4000 pas de temps puis attitude 2 jusqu'a la fin.
res.q = [];
N = size(q1);
res.q = [q1(1:4000) , q2(4001:N) ];

//-----------------------------------------------------------
// Autres calculs géométriques 
//-----------------------------------------------------------
// Direction de soleil/terre dans satellite frame :
//Conventions pour la définition de M et q opposées ==> res.q' (voir conventions et aides)
res.dir_sun_sat =  CL_rot_rotVect(res.q',CL_unitVector(data.pos_sun - res.pos));
res.dir_earth_sat =  CL_rot_rotVect(res.q',CL_unitVector( - res.pos));

// Coordonnées géographiques et altitude géodésiques (long lat alt)

pos_ter = CL_fr_convert("EME2000", "ECF", data.t, res.pos);
res.coord_geo = CL_co_car2ell(pos_ter);

// Visibilité géométrique de la station (au dessus d'une elev min) (0 = non , 1 = oui)
res.visi_sta = list();
nb_sta = size(data.stations.name, 2);
for k = 1 : nb_sta
  coord = data.stations.coord(:,k);
  elev = CL_gm_stationPointing(coord, pos_ter,"elev");
  I2 = find(elev > data.stations.elevmin(k));
  res.visi_sta(k) = zeros(data.t);
  res.visi_sta(k)(I2) = 1;

end


// Visibilite du Soleil (% de Soleil non caché par la Terre)
%CL_radiusSun=CL_dataGet("body.Sun.eqRad");
res.sun_visibility = 1 - CL_gm_eclipseCheck(res.pos,data.pos_sun,[0;0;0],%CL_radiusSun,%CL_eqRad);

// --------------------------------
// Ecriture des fichiers de sorties
// --------------------------------

// Date : conversion de jours juliens CNES (sur une variable) en 
//        jours juliens modifés (sur deux variables : jours / secondes)
format_date = "%d %.5f ";
//t_mjd = CL_dat_cjd2mjd(data.t);
t_mjd = CL_dat_convert("cjd","mjd",data.t);
jour_mjd = floor(t_mjd);
sec_mjd = (t_mjd - jour_mjd) * 86400.0 ;

// Repertoire ou ecrire les fichiers
rep = ".";

////////////////////////////prefixe = "NANOSAT";
prefixe = "CIC-SAT";

// Fichier _POSITION_VELOCITY.TXT (OEM)
write_fic("OEM", rep, prefixe, format_date, jour_mjd, sec_mjd, "POSITION_VELOCITY", [res.pos ; res.vel] / 1000, "%.3f %.3f %.3f %.6f %.6f %.6f") ;

// Fichier _QUATERNION.TXT (AEM)

write_fic("AEM", rep, prefixe, format_date, jour_mjd, sec_mjd, "QUATERNION", [res.q.r ; res.q.i] , "%.6f %.6f %.6f %.6f" , "ICRF" , "SC_BODY_1") ;

// Fichier _SUN_DIRECTION-SATELLITE_FRAME.TXT (MEM)
write_fic("MEM", rep, prefixe, format_date, jour_mjd, sec_mjd, "SUN_DIRECTION-SATELLITE_FRAME" , res.dir_sun_sat , "%.6f %.6f %.6f") ;

// Fichier _EARTH_DIRECTION-SATELLITE_FRAME.TXT (MEM)
write_fic("MEM", rep, prefixe, format_date, jour_mjd, sec_mjd, "EARTH_DIRECTION-SATELLITE_FRAME" , res.dir_earth_sat , "%.6f %.6f %.6f") ;

// Fichier _GEOGRAPHICAL_COORDINATES.TXT (MEM)
write_fic("MEM", rep, prefixe, format_date, jour_mjd, sec_mjd, "GEOGRAPHICAL_COORDINATES" , res.coord_geo(1:2,:) * %CL_rad2deg, "%.6f %.6f") ;

// Fichiers _GEOMETRICAL_VISIBILITY_GROUND_STATION_X.TXT (MEM)
for k = 1 : lstsize(res.visi_sta)
  write_fic("MEM", rep, prefixe, format_date, jour_mjd, sec_mjd, "GEOMETRICAL_VISIBILITY_GROUND_STATION_"+string(k) , res.visi_sta(k) , "%d");
end

// Fichier _SUN_VISIBILITY.TXT (MEM)
write_fic("MEM", rep, prefixe, format_date, jour_mjd, sec_mjd, "SUN_VISIBILITY" , res.sun_visibility * 100 , "%.2f") ;


printf('Fin de simulation CIC.\n');