#
#
#
# pragma include once;

include 'quanta.g'
include 'measures.g'
include 'pgplotter.g'


# x = degrees, size of source
# v = deg/sec
otfdemo := function(x=0.2, v=0.2) {
  include 'otfpath.g'
  mypath := otfpath();
  mypath.setup(v=v, x=x, jmax=100, dt=0.001);
  mypath.viewstate();
  results := mypath.docalc();
  print results;
  mypath.doplot(1);
  mypath.doplot(2);
  mypath.doplot(3);
  mypath.doplot(4);
  trajectory := mypath.gettrajectory();
  mypath.done()
  return T;
}


otfdemo2 := function(x=0.5, dt=0.001) {
  include 'otfpath.g'

  vslews := 0.1 * [1:20];
  tslew  := 0 * vslews;
  duty   := 0 * vslews;

  for (ii in ind(vslews)) {
    mypath := otfpath();
    mypath.setup(v=vslews[ii], x=x, jmax=100, dt=dt);
    results := mypath.docalc();
    tslew[ii] := results.tonsource + results.toffsource;
    duty[ii] := results.dutycycle
    mypath.done();
  }

  print 'Source sourcesize = ', x;
  print 'ii   vslew   tslew   duty';
  for (ii in ind(vslews)) {
     print ii, vslews[ii], tslew[ii], duty[ii];
  }
  return T;
}  



#
# Define the OTF Path tool
#

otfpath := subsequence() {

	private := [=];
# all times in s, all distances in degrees	
	private.dt   := 0.0005;
################################### These are limits
	private.jmax := 100;	# jerk; other possible values: 30, 60
	private.amax := 24;	# max acceleration
	private.vmax := 6;	# max velocity
################################### These are what we see in the observations
	private.x0   := 10/60;	# half size of object
	private.v0   := 0.5;	# actual velocity used for ON-SOURCE
	private.a0   := F;	# amplitude of acceleration
	private.j0   := F;	# amplitude of jerk
	private.tdelta := F;	# time for turnaround
	private.xdelta := F;	# distance for turnaround

	private.onoffvector:=F;	# ON or OFF source?
	private.tvector := F;	# data vectors for time
	private.xvector := F;	#	 position
	private.vvector := F;	#	 velocity
	private.avector := F;	#	 acceleration
	private.jvector := F;	#	 jerk

	
  	private.havepg    := F;
  	private.pg        := F;

#  public functions

self.type :=  function() {
   note ('I calculate antenna trajectories for OTF mapping');
   return 'otfpath';
}

self.getprivate := function() {
        return ref private;
}

self.done := function() {
        wider self, private;
        private.donepgplotter();
        self := F;
        val public := F;
        return T;
}

# note:  x input is full size, x0 is half size
self.setup :=  function(v=0.5, x=0.1, jmax=100, dt=0.001) {

	wider private;
	private.v0   := v;
	private.x0   := x/2;
	private.jmax := jmax;
	private.dt   := dt;

	return T;
}

self.viewstate :=  function() {

	print 'X0 = ', private.x0;
	print '2*X0 = ', 2*private.x0;
	print 'V0 = ', private.v0;
	print 'Jmax = ', private.jmax;
	print 'dt = ', private.dt;

	return T;
}

# actually create the trajectory vectors
self.docalc :=  function(dofull=F) {

	wider private;
	results := [=];

################# preliminary calcs #############################
	private.tdelta := pi * sqrt( private.v0 / private.jmax );
	nstraight  := as_integer(2 * private.x0 / private.v0 / private.dt+ 0.5);
	nturn      := as_integer(private.tdelta / private.dt + 0.5);
	private.a0 := pi *  private.v0 / private.tdelta;
	private.j0 := pi *  private.a0 / private.tdelta;
	private.xdelta := private.v0 * private.tdelta / pi;

#################### real calcs ################################
# positive turnaround 
	if (dofull) {
	   t1 := [(-nturn/2):(nturn/2)] * private.dt;
	} else {
	   t1 := [0:(nturn/2)] * private.dt;
	}
	x1 := private.x0 + private.tdelta *  private.v0 / pi *
		cos( pi * t1 / private.tdelta );
	toffset := t1[1];
	t1 := t1 - toffset;
	p1 := 0*t1;
# linear stretch down
	t2 := [1:nstraight] * private.dt;
	x2 := private.x0 - t2 * private.v0;
	t2 := t2 + t1[len(t1)];
	p2 := 0*t2 + 1
# negative turnaround
	if (dofull) {
	   t3 :=  t1 + t2[len(t2)];
	   x3 := -x1;
	} else {
	    t3 := [(-nturn/2):0] * private.dt;
	    x3 := -private.x0 - private.tdelta *  private.v0 / pi *
		cos( pi * t3 / private.tdelta );
	    toffset2 := t2[len(t2)];
	    toffset3 := t3[1];
	    t3 := t3 + toffset2 - toffset3 + private.dt;
	}
	p3 := 0*t3
	private.onoffvector := private.concat3(p1, p2, p3);
	private.tvector := private.concat3(t1, t2, t3);
	private.xvector := private.concat3(x1, x2, x3);

	private.vvector := private.differentiate( x1, private.dt );
	private.avector := private.differentiate( private.vvector, private.dt );
	private.jvector := private.differentiate( private.avector, private.dt );

#################### report results ************************
	results.tonsource := 2 * private.x0 / private.v0 ;
	results.toffsource := private.tdelta;
	results.dutycycle := results.tonsource / 
		(results.tonsource + results.toffsource)  ;
	results.xoffsource := private.xdelta;
	results.xonsource := 2 * private.x0;
	results.vmax := max(abs(private.vvector));
	results.amax := max(abs(private.avector));
	results.jmax := max(abs(private.jvector));
	results.j0   := private.j0;
	results.a0   := private.a0;

	return ref results;
}


# which = 1: position, 2: velocity, 3: accel, 4: jerk
self.doplot :=  function(which=0) {

	private.getpgplotter();
	if (which==1) {
	  private.pg.plotxy(x=private.tvector,                        
            y=private.xvector,
            plotlines=F,
            xtitle='Time',
            ytitle='Position',
            title='Position vs Time');
	} else if (which==2) {
	  private.pg.plotxy(x=private.tvector,                        
            y=private.vvector,
            plotlines=F,
            xtitle='Time',
            ytitle='Velocity',
            title='Velocity vs Time');
	} else if (which==3) {
	  private.pg.plotxy(x=private.tvector,                        
            y=private.avector,
            plotlines=F,
            xtitle='Time',
            ytitle='Acceleration',
            title='Acceleration vs Time');
	} else if (which==4) {
	  private.pg.plotxy(x=private.tvector,                        
            y=private.jvector,
            plotlines=F,
            xtitle='Time',
            ytitle='Position',
            title='Jerk vs Time');
	}

	return T;
}
	

# regrid trajector onto a new integration time dt
# Note: if the this DT is not an integral number of
# the setup's dt's, this will only be approximate
# forward=F gives the "return trip"

self.gettrajectory :=  function(dt=0.01, forward=T) {

	results := [=];
	factor := as_integer(dt / private.dt + 0.5);
	select0 := 
	  [1:(as_integer(len(private.tvector)/factor)+1)] *
	    factor - factor + 1;
        if (select0[len(select0)] > len(private.tvector)) {
             select0[len(select0)] := len(private.tvector);
        }
       tmax := private.tvector[len(private.tvector)];
	select1 := select0[(private.tvector[select0] <= tmax)];

	results.t := private.tvector[select1];

	if (forward) {
		results.x := private.xvector[select1];
	} else {
		results.x := private.xvector[-sort(-select1)];	

	}
	results.phase := private.onoffvector[select1];

	return results;
}








#==================== private functions =========================================

  private.getpgplotter := function() 
  {
    wider self, private;
    if (private.havepg == F) {
       private.pg := pgplotter();
       private.havepg := T;
    }
    return T;
  }

  private.donepgplotter := function() 
  {
    wider self, private;
    if (private.havepg ) {
        private.pg.done();
        private.pg := F;
        private.havepg := F;
    }
    return T;
  }



# returns concatenated vector
  private.concat := function(ref v1, ref v2) {
	n1 := len(v1);
	n2 := len(v2);
	v3 := array(0, (n1+n2));
	v3[1:n1] := v1;
	v3[(n1+1):(n1+n2)] := v2;
	return ref v3;
  }

# returns concatenated vector
  private.concat3 := function(ref v1, ref v2, ref v3) {
	n1 := len(v1);
	n2 := len(v2);
	n3 := len(v3);
	v4 := array(0, (n1+n2+n3));
	v4[1:n1] := v1;
	v4[(n1+1):(n1+n2)] := v2;
	v4[(n1+n2+1):(n1+n2+n3)] := v3;
	return ref v4;
  }

# returns concatenated vector
  private.concat4 := function(ref v1, ref v2, ref v3, ref v4) {
	n1 := len(v1);
	n2 := len(v2);
	n3 := len(v3);
	n4 := len(v4);
	v5 := array(0, (n1+n2+n3+n4));
	v5[1:n1] := v1;
	v5[(n1+1):(n1+n2)] := v2;
	v5[(n1+n2+1):(n1+n2+n3)] := v3;
	v5[(n1+n2+n3+1):(n1+n2+n3+n4)] := v4;
	return ref v5;
  }

# returns concatenated vector
  private.concat5 := function(ref v1, ref v2, ref v3, ref v4, ref v5) {
	n1 := len(v1);
	n2 := len(v2);
	n3 := len(v3);
	n4 := len(v4);
	n5 := len(v5);
	v6 := array(0, (n1+n2+n3+n4+n5));
	v6[1:n1] := v1;
	v6[(n1+1):(n1+n2)] := v2;
	v6[(n1+n2+1):(n1+n2+n3)] := v3;
	v6[(n1+n2+n3+1):(n1+n2+n3+n4)] := v4;
	v6[(n1+n2+n3+n4+1):(n1+n2+n3+n4+n5)] := v5;
	return ref v6;
  }

# differentiates an equally spaced vector; 
# the time axis will be slightly in error
  private.differentiate := function( ref myvec, dt ) {
	diff := (myvec[2:(len(myvec))] - myvec[1:(len(myvec)-1)] ) / dt;
	return ref diff;
  }

}
# end of constructor


# returns noisearray
excercisepath := function() {

 mypath := otfpath();

 jerk := 100;
 sizes := [0.01, 0.02, 0.05, 0.1, 0.2, 0.4, 0.8, 1.6];
 velocities := [0.02, 0.05, 0.1, 0.2, 0.3, 0.4, 0.5];

 noisearray := array(0.0, len(sizes), len(velocities) );

 xround := 1000;

 print 'size[deg]  v=.02 v=.05 v=.1 v=.2 v=.3 v=.4 v=.5'
 for (is in ind(sizes)) {
   size := sizes[is];
   for (iv in ind(velocities)) {
	v := velocities[iv];
        mypath.setup(v=v, x=size, jmax=jerk, dt=0.0005);
        results := mypath.docalc(F);

	noisearray[is, iv] := as_integer(xround*1.0/sqrt(results.dutycycle) +0.5)/
		xround;
   }
   print size, noisearray[is,1],noisearray[is,2],noisearray[is,3],
	noisearray[is,4],noisearray[is,5],noisearray[is,6],noisearray[is,7]
 }
 mypath.done();
 return noisearray;
}