package Km;
use strict;

use Math::Trig;
use af;

my ($pi,$deg,$AU,$np,$LARGE,$INVALID);
$pi=2*asin(1.0);
$deg=$pi/180.0;
$AU=1.5e8;
$np=1.0e-12; # num. prec.
$LARGE=1.0e30;
$INVALID=1.0e30;
##############################
#
# construct an orbit
# $a  in km 
# $e  in rad
# $I  in rad, not used here
# $L  in rad, mean long at time t_L
# $om
# $no
# $t_L
# $mu in km^3/s^2 
#### $tp in s
#
##############################
sub new {
 my ($class) = shift; 
 my ($a,$e,$I,$L,$om,$no,$t_L,$mu)=@_;
 my ($M,$op,$t);
 
 my $this = {
   _e  => undef,
   _a  => undef,
   _om => undef,
   _tp => undef,
   _mu => undef
 };
# af::dumpvars("a e I L om no t_L mu",($a,$e,$I,$L,$om,$no,$t_L,$mu));
 bless $this, $class;
 $M=$L-$om; # mean anomaly at time t_L
 $op=2*$pi*sqrt($a*$a*$a)/sqrt($mu);   # orb. period in seconds
 $t=$M*$op/(2.0*$pi);
# af::dumpvars("M L om op t",($M,$L,$om,$op,$t));
 $this->{_e} = $e;
 $this->{_a} = $a; 
 $this->{_om} = $no+$om+$pi;      # $pi because the position is opposite
 $this->{_tp} = $t_L*86400-$t;    # to the direction to the sun
 $this->{_mu} = $mu;
 return $this;
}

##############################
sub P {
 my ($ob) = shift; 
 my ($a,$mu,$P);
 
 $a=$ob->{_a};
 $mu=$ob->{_mu};
 $P=2*$pi*sqrt($a*$a*$a)/sqrt($mu);
}

################################
#
# returns absolute coordinates x,y of orbiting body at time t
# t: time (seconds, not epoch)
# returns array (x,y) in km
################################
sub posn_abs {
 my $ME="posn_abs";
 my ($ob) = shift;
 my ($t,$CF)=@_;
 my ($e,$a,$tp,$om,$mu); 
 $e=$ob->{_e}; $a=$ob->{_a}; $tp=$ob->{_tp}; $om=$ob->{_om}; $mu=$ob->{_mu}; 
 my @ar;
 my ($P,$Psi,$xi,$yi,$x,$y,@res);   
 
 if($e<-1e10) {af::die($ME." e = ".$e,$CF);}
 if($a<0) {af::die($ME." a = ".$a,$CF);}
 if($tp==$INVALID) {af::die($ME." tp = ".$tp,$CF);}
 if($om<0) {af::die($ME." om =".$om,$CF);}
 if($mu<0) {af::die($ME." mu =".$mu,$CF);}
 $P=2.0*$pi*sqrt($a*$a*$a)/sqrt($mu);
 @ar=$ob->posn_orb($t,$ME." ".$CF);                # (E,M,Th,or,R)
 $Psi=$ar[0];
# print "__Psi = ".$ar[0]."\n";
 $xi=$a*cos($Psi)-$e*$a;
 $yi=$a*sqrt(1.0-$e*$e)*sin($Psi);
 $x=$xi*cos($om)-$yi*sin($om);
 $y=$yi*cos($om)+$xi*sin($om);
 @res=($x,$y);
}

################################
#
# Calculate all kinds of stuff on the transfer ellipse
#
# Input:
# $r1:		r_1
# $r2:		r_2
# $Th:		\Theta
# $dth:		\Delta\Theta
# $t:		t
# $beta:
# calculate:
# e: eccentricity
# a: semimajor axis
# tt: transit time
# returns tt
################################
sub transfer_orbit {
 my ($ob) = shift;
 my ($r1,$r2,$Th,$dth,$t,$beta,$CF)=@_;
 my ($e,$a,$tp,$om,$mu);
 my ($tt,$op,$mm,$Psi1,$Psi2,$td1,$td2,$tp);
 my @test;

 if(($mu=$ob->{_mu})<0) {ems("undefined orbit");};
 if($Th>2*$pi) {$Th-=2*$pi*int($Th/(2*$pi));}
 if($Th<0) {$Th+=2*$pi*(1+int(-$Th/(2*$pi)));}
 if(($Th<0)||($Th>2*$pi)) {die "bug";}
 $e=($r1-$r2)/($r2*cos($Th+$dth) - $r1*cos($Th));     # ecc., see text
 if($e<0) {$tt=$INVALID; $tp=0;}
 $a=$r1*(1+$e*cos($Th))/(1-$e*$e); 		      # semimajor axis
 if(($e<0.99)&&($e>=0.0)) {
   $op=2*$pi*sqrt($a*$a*$a)/sqrt($mu);
   $mm=2*$pi/$op;
   $Psi1=2*atan(tan(0.5*$Th)*sqrt(1-$e)/sqrt(1+$e));    # ecc. anomaly
   if($Psi1<0) {$Psi1+=2*$pi;}
   $Psi2=2*atan(tan(0.5*($Th+$dth))*sqrt(1-$e)/sqrt(1+$e));    # ecc. anomaly
   if($Psi2<0) {$Psi2+=2*$pi;} 
   $td1=($Psi1-$e*sin($Psi1))/$mm;                    # Kepler equation
   $tp=$t-$td1;
   $td2=($Psi2-$e*sin($Psi2))/$mm;                    # Kepler equation
   $tt=$td2-$td1;
   if($tt<0) {$tt+=$op;}
 }
 else {$tt=$INVALID; $tp=0;} # slap a penalty on subelliptic and hyperbolic traj.
# print " e = ".$e."\n";
 $ob->{_e}=$e;
 $ob->{_a}=$a; 
 $ob->{_tp}=$tp;
 $ob->{_om}=$beta;
 if(($e<0.99)&&($e>=0.0)) {   
   @test=$ob->posn_orb($t);
   if(abs($test[0]-$Psi1)>1.0) {print "!! ".$test[0]."\t".$Psi1."\t".$beta."\n";} 
 }
 $tt;
}

################################
#
# $t
# returns array: (E,M,Th,or,R)
# $E:	  ecc. anomaly
# $M:	  mean anomaly
# $Th:	  Theta
# $om:	  orientation (copied from object data)
# $R:
#
################################
sub posn_orb {
 my $ME="posn_orb";
 my ($ob) = shift;
 my ($t,$CF)=@_;
 my ($e,$a,$tp,$om,$mu); 
 $e=$ob->{_e};
 $a=$ob->{_a};
 $tp=$ob->{_tp}; $om=$ob->{_om}; $mu=$ob->{_mu}; 
 my ($b,$op,$tr,$M,$E,$Th,$lh,$rf,@res);
 my $m;

 $t-=$tp;                              # time diff to peri. time
 if($a<0) {af::die("a = ".$a,$CF);}
 $op=2*$pi*sqrt($a*$a*$a)/sqrt($mu);   # orb. period
 if($t<0) {$t+=$op*(1+int(-$t/$op));}  # kludge
# print "t = ".$t."\n";
 $tr=$t-$op*int($t/$op);               # reduced time within one period
 $M=2*$pi*$tr/$op;		       # mean anomaly
 $E=$ob->E_from_M($M,$CF);
# my $test=$M-($E-$e*sin($E));
# af::dumpvars("M E test",($M,$E,$test));
 $Th=2.0*atan(tan(0.5*$E)*sqrt(1+$e)/sqrt(1-$e)); 
 $rf=$a*(1.0-$e*$e)/(1.0+$e*cos($Th)); # dist. from focus
 @res=($E,$M,$Th,$om,$rf);
}

################################
#
#
################################
sub V {
 my $ME="V";
 my ($ob) = shift;
 my ($t,$CF)=@_;
 my ($e,$a,$tp,$om,$mu); 
 $e=$ob->{_e}; $a=$ob->{_a}; $tp=$ob->{_tp}; $om=$ob->{_om}; $mu=$ob->{_mu}; 
 my (@po,$Th,$r1,$r2,$aca,$al,$nu,$vp,$rp,$Ekin,$vabs,$vx,$vy,$vxr,$vyr,@res);

 @po=$ob->posn_orb($t,$ME." ".$CF);
 $Th=$po[2];
 $r1=$po[4];
 $r2=2.0*$a-$r1;
 $aca=($r1*$r1+$r2*$r2-4.0*$e*$e*$a*$a)/(2.0*$r1*$r2);
 if(abs($aca-1)>1.0e-9) { # numeric inaccuracies for alpha ~ 0
   if(abs($aca)>1) {af::dumpvars("a e Th r1 r2 aca",($a,$e,$Th,$r1,$r2,$aca));}
   $al=acos(($r1*$r1+$r2*$r2-4.0*$e*$e*$a*$a)/(2.0*$r1*$r2));
   if($Th<0) {$Th+=2*$pi;} if($Th>=2*$pi) {$Th-=2*$pi;}
   if($Th<$pi) {$nu=$Th-0.5*$al+0.5*$pi;} 
   else        {$nu=$Th+0.5*$al+0.5*$pi;} 
 }
 else {$nu=$Th+0.5*$pi;}
 $rp=$a*(1-$e);            # km
 $vp=sqrt(($e+1)*$mu/$rp); # km/s
 $Ekin=0.5*$vp*$vp-$mu/$rp;#
 if($Ekin+$mu/$r1<0) {af::dumpvars("Ekin r1 vp mu",$Ekin,$r1,$vp,$mu);}
 $vabs=sqrt(2.0*($Ekin+$mu/$r1));
 $vx=$vabs*cos($nu);
 $vy=$vabs*sin($nu);
 $vxr=$vx*cos($om)-$vy*sin($om);
 $vyr=$vy*cos($om)+$vx*sin($om);
 @res=($vx,$vy);
 @po=$ob->posn_abs($t,$ME." ".$CF);
 @res=($vxr,$vyr);
# @res=($po[0],$po[1]);
}

################################
#
# determination of initial bounds is kludgey
# 
#
################################
sub E_from_M {
 my $ME="E_from_M";
 my ($ob) = shift;
 my ($M,$CF)=@_;
 my ($e,$El,$Eg,$Em,$lh,$nl,$test);
 $e=$ob->{_e};
 
# $Eg=100*$M;			       # upper bound ecc. anomaly
# $El=0.01*$M;			       # lower bound
# higher eccentricities require larger factor
 $Eg=10000*$M;			       # upper bound ecc. anomaly
 $El=0;  			       # lower bound
 $nl=0;
 do {	                               # bisect to find E
   $Em=($Eg+$El)/2;
   $lh=$Em-$e*sin($Em);
   if($lh>$M) {$Eg=$Em;}
   else {$El=$Em;}
   $nl+=1;
   if($nl>1000) {
     af::dumpvars("e M E",($e,$M,$Em));
     af::die($ME.", iteration does not converge",$CF);
   }
 } while (abs($lh-$M)>$M*1.0e-12);
# $test=abs($M-($Em-$e*sin($Em)));                    # Kepler equation
 if($test>1.0e-10) {af::dumpvars("M Em",($M,$Em));}
 $Em;
# print $M."\t".$Em."\n";
}
1; # return value