// computation of the extrema of s
function [maxS,minS] = maxloc(s)
 maxS = [];
 minS = [];

 for k=2:length(s)-1,
  if (s(k) > s(k+1))&(s(k) > s(k-1))
    maxS = [maxS k];
  end
  if (s(k) < s(k+1))&(s(k) < s(k-1))
    minS = [minS k];
  end
 end
endfunction

//symmetrization of the original signal with respect to the first and
// the last extremum

function [ind_ext,ind_head,k_first,k_last,snew,tnew] = Maxlocsym(s,pas)
  
 // snew: the symmetrized signal
 // tnew: the corresponding times
 // ind_head: number of points added at the beginning of the signal 
 // k_first: index of the first extremum of the old signal in the symmetrized one dans le nouveau signal
 // k_last: index of the last extremum of the old signal in the symmetrized one
 // ind_ext: indices of the extrema 
  
  l_s = length(s);
  [maxS,minS] = maxloc(s);
  ind_ext     = [maxS minS];
  ind_ext     = sort(ind_ext); //we sort the extrema in descending order
  l_ext       = length(ind_ext);
  ind_ext     = ind_ext(l_ext:-1:1); //we sort the extrema in ascending order
   
  //computation of the indices of the extrema
  //we symmetrize the signal with respect to the first and the last extrema   
  
  l_snew = l_s;
 
  if (ind_ext(2)-ind_ext(1) >= ind_ext(1)-1)
   l_snew   = l_snew - 2*ind_ext(1)+ind_ext(2)+1;    
   ind_head = ind_ext(2)-2*ind_ext(1)+1; // number of points added at the 
                                         // beginning of the signal
   k_first  = 2;
   
   //we symmetrize the signal with respect to the last extremum
   if (ind_ext(l_ext) - ind_ext(l_ext-1) >= l_s - ind_ext(l_ext))    
    l_snew = l_snew + 2*ind_ext(l_ext) - ind_ext(l_ext-1) - l_s; 
    printf(%io(2),l_snew)
    snew = zeros(1,l_snew);
    snew(ind_head+ind_ext(1):ind_head+ind_ext(l_ext)) = ...
       s(ind_ext(1):ind_ext(l_ext));
    snew(ind_ext(1)+ind_head-1:-1:1) = s(ind_ext(1)+1:ind_ext(2));
    snew(l_snew:-1:ind_head+ind_ext(l_ext)+1) = ...
       s(ind_ext(l_ext-1):ind_ext(l_ext)-1);
   else 
    k=2;
    //in case the symmetrized signal is shorter than this original one
    while (ind_ext(l_ext) - ind_ext(l_ext-k) < l_s - ind_ext(l_ext)) 
     k = k+1;
    end 
    l_snew = l_snew + 2*ind_ext(l_ext) - ind_ext(l_ext-k)- l_s; 
   
    snew = zeros(1,l_snew);
    snew(ind_head+ind_ext(1):ind_head+ind_ext(l_ext)) = ...
       s(ind_ext(1):ind_ext(l_ext));
    snew(ind_ext(1)+ind_head-1:-1:1)  = s(ind_ext(1)+1:ind_ext(2));
    snew(l_snew:-1:ind_head+ind_ext(l_ext)+1) = ...
       s(ind_ext(l_ext-k):ind_ext(l_ext)-1);
   end
   k_last = (k_first-1)+l_ext;
  else
   k=3;
   while (ind_ext(k)-ind_ext(1) < ind_ext(1)-1) 
    k = k+1;
   end 
   k_first  = k;
   l_snew   = l_snew - 2*ind_ext(1)+ind_ext(k)+1;    
   //number of indices that we add at the beginning of the signal
   ind_head = ind_ext(k)-2*ind_ext(1)+1; 
   
   if (ind_ext(l_ext) - ind_ext(l_ext-1) >= l_s - ind_ext(l_ext))    
    l_snew = l_snew + 2*ind_ext(l_ext) - ind_ext(l_ext-1) - l_s;
    
    snew = zeros(1,l_snew);
    snew(ind_head+ind_ext(1):ind_head+ind_ext(l_ext)) = ...
       s(ind_ext(1):ind_ext(l_ext));
    snew(ind_ext(1)+ind_head-1:-1:1) = s(ind_ext(1)+1:ind_ext(k));
    snew(l_snew:-1:ind_head+ind_ext(l_ext)+1) = ...
       s(ind_ext(l_ext-1):ind_ext(l_ext)-1);
   else 
    k1 = 2;
    while (ind_ext(l_ext) - ind_ext(l_ext-k) < l_s - ind_ext(l_ext)) 
     k1 = k1+1;
    end 
    l_snew = l_snew + 2*ind_ext(l_ext) - ind_ext(l_ext-k1)-l_s;
    
    snew = zeros(1,l_snew);
    snew(ind_head+ind_ext(1):ind_head+ind_ext(l_ext)) = ...
       s(ind_ext(1):ind_ext(l_ext));
    snew(ind_ext(1)+ind_head-1:-1:1)  = s(ind_ext(1)+1:ind_ext(k));
    snew(l_snew:-1:ind_head+ind_ext(l_ext)+1) = ...
       s(ind_ext(l_ext-k1):ind_ext(l_ext)-1);
   end
   k_last = (k_first-1) + l_ext;
  end
  
  tnew=zeros(1,l_snew);
  tnew=0:pas:(length(tnew)-1)*pas;
  
  //we recompute the extrema of the new signal  
  [maxS,minS] = maxloc(snew);
  ind_ext = [maxS minS];
  ind_ext = sort(ind_ext);
  l_ext = length(ind_ext); 
  ind_ext = ind_ext(l_ext:-1:1); 
  // we add the first and the last points since they should also be extrema
  ind_ext = [1 ind_ext l_snew]; 
  
endfunction

function [B,M1,S1,N1,S2,Q,S3] = calculmatrice(snew,tnew,ind_ext)
 
 //construction of the cost function (B) and of the  
 //matrices of constraints (M1 and N1 are inequality constraints,
 //Q corresponds to equality constraints)
 
 
 l_ext = length(ind_ext);
 H = diff(tnew(ind_ext));

 //construction of the matrix B
 
 B = zeros(2*l_ext);
 
 for k = 0:l_ext-1
  if (k == 0)
   B(2*k+1,2*k+1) = 6/(5*H(k+1));
   B(2*k+1,2*k+2) = 1/10;
   B(2*k+1,2*k+3) = -6/(5*H(k+1));
   B(2*k+1,2*k+4) = 1/10;
   B(2*k+2,2*k+1) = B(2*k+1,2*k+2);
   B(2*k+2,2*k+2) = 2*H(k+1)/15;
   B(2*k+2,2*k+3) = -1/10;
   B(2*k+2,2*k+4) = H(k+1)/30;
  elseif (k == l_ext-1)
   B(2*k+1,2*k-1) = -6/(5*H(k));
   B(2*k+1,2*k)   = -1/10;
   B(2*k+1,2*k+1) = 6/(5*H(k));
   B(2*k+1,2*k+2) = -1/10;
   B(2*k+2,2*k-1) = 1/10;
   B(2*k+2,2*k) = H(k)/30;
   B(2*k+2,2*k+1) = -1/10;
   B(2*k+2,2*k+2) = 2/15*H(k);
  else
   // odd lines
   B(2*k+1,2*k-1) = -6/(5*H(k));
   B(2*k+1,2*k)   = -1/10;...
   B(2*k+1,2*k+1) = 6/5*(1/H(k)+1/H(k+1)); 
   B(2*k+1,2*k+3) = -6/(5*H(k+1));
   B(2*k+1,2*k+4) = 1/10;
         
   // even lines
   B(2*k+2,2*k-1) = 1/10;
   B(2*k+2,2*k)   = H(k)/30;
   B(2*k+2,2*k+2) = 2/15*(H(k)+H(k+1)); 
   B(2*k+2,2*k+3) = -1/10;
   B(2*k+2,2*k+4) = H(k+1)/30;
  end   
 end 

 //Computation of the matrices M1 and N1 and of the vectors S1 and S2
 
 M1 = diag(zeros(1,2*l_ext));
 X1 = 1:2:2*l_ext-1;
 DiagM1(X1) = ones(1,l_ext);
 M1 = diag([DiagM1 0]);
 N1 = -M1;
 
 //the vectors S1 and S2 will be set later
 
 S1 = zeros(1,2*l_ext);
 S2 = zeros(1,2*l_ext);
 
 //equality constraints and computation of S1 and S2
 
 Q = M1; 
 S3 = zeros(1,2*l_ext);
 
 X = ind_ext;
 for j=1:l_ext, 
  if (j > 2) & (j <= l_ext-2),  
   if (snew(X(j)) - snew(X(j-2)))*(snew(X(j+2))-snew(X(j))) >= 0,
    if  ((snew(X(j)) - snew(X(j-2)))*(snew(X(j+1))-snew(X(j-1))) >= 0)... 
       &((snew(X(j+2)) - snew(X(j)))*(snew(X(j+1))-snew(X(j-1))) >= 0),
     S1(2*(j-1)+1) = (max(snew(X(j-1)),snew(X(j+1)))+snew(X(j)))/2;
     S2(2*(j-1)+1) = -(min(snew(X(j-1)),snew(X(j+1)))+snew(X(j)))/2;
    end
    Q(2*(j-1)+1,2*(j-1)+1) = 0;
   else 
    M1(2*(j-1)+1,2*(j-1)+1) = 0;
    N1(2*(j-1)+1,2*(j-1)+1) = 0; 
    //computation of the intersection point 
    Tint = ...
    (-snew(X(j-2))*tnew(X(j-1))*(tnew(X(j+2))-tnew(X(j)))...
     -snew(X(j-1))*(tnew(X(j))-tnew(X(j-2)))*(tnew(X(j+2))-tnew(X(j)))... 
     +snew(X(j))*(tnew(X(j+1))*(tnew(X(j))-tnew(X(j-2)))+...
                  tnew(X(j-1))*(tnew(X(j+2))-tnew(X(j))))...
     +snew(X(j+1))*(tnew(X(j+2))-tnew(X(j)))*(tnew(X(j))-tnew(X(j-2)))...
     -snew(X(j+2))*tnew(X(j+1))*(tnew(X(j))-tnew(X(j-2))))/...
     ((snew(X(j))-snew(X(j-2)))*(tnew(X(j+2))-tnew(X(j))) -...
      (snew(X(j+2))-snew(X(j)))*(tnew(X(j))-tnew(X(j-2))));
     if (Tint > tnew(X(j)))
      S3(2*(j-1)+1) = ... 
        0.5*(snew(X(j))*(2*tnew(X(j))-tnew(X(j-2))-tnew(X(j-1)))/...
                        (tnew(X(j))-tnew(X(j-2)))...
            -snew(X(j-2))*(tnew(X(j))-tnew(X(j-1)))/(tnew(X(j))-tnew(X(j-2)))...
	    +snew(X(j-1)));		              		       
     else
      S3(2*(j-1)+1) = ... 
        0.5*(snew(X(j))*(-2*tnew(X(j))+tnew(X(j+2))+tnew(X(j+1)))/...
                        (tnew(X(j+2))-tnew(X(j)))...
           +snew(X(j+2))*(tnew(X(j))-tnew(X(j+1)))/(tnew(X(j+2))-tnew(X(j)))...
	   +snew(X(j+1)));		              		       
     end
   end
  else
   M1(2*(j-1)+1,2*(j-1)+1) = 0;
   N1(2*(j-1)+1,2*(j-1)+1) = 0; 
   Q(2*(j-1)+1,2*(j-1)+1) = 0;
  end 
 end

endfunction

function [pol] = reconspol(env,ind_ext,tnew,pas)
 //reconstruction of the polynomial using the value of the envelope and 
 //of its derivative given in env
 
 l_ext = length(ind_ext);
 
 pol = [];
 
 for k=1:l_ext-1,
  if k < l_ext-1,
   hjj  = (1-2*(tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k)))/...
           (tnew(ind_ext(k))-tnew(ind_ext(k+1))))...
        .*((tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k+1))).^2/...
           (tnew(ind_ext(k))-tnew(ind_ext(k+1)))^2);
   kjj  = (tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k))).*...
          ((tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k+1))).^2/...
           (tnew(ind_ext(k))-tnew(ind_ext(k+1)))^2);	  
   hj1j = (1-2*(tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k+1)))/...
           (tnew(ind_ext(k+1))-tnew(ind_ext(k))))...
        .*((tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k))).^2/...
          (tnew(ind_ext(k+1))-tnew(ind_ext(k)))^2);
   kj1j  = (tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k+1))).*...
          ((tnew(ind_ext(k):ind_ext(k+1)-1)-tnew(ind_ext(k))).^2/...
           (tnew(ind_ext(k+1))-tnew(ind_ext(k)))^2);	  
   pol = [pol env(2*(k-1)+1)*hjj+env(2*(k-1)+2)*kjj+env(2*(k-1)+3)*hj1j...
              +env(2*(k-1)+4)*kj1j];   
 
  else
   hjj  = (1-2*(tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k)))/...
           (tnew(ind_ext(k))-tnew(ind_ext(k+1))))...
        .*((tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k+1))).^2/...
           (tnew(ind_ext(k))-tnew(ind_ext(k+1)))^2);
   kjj  = (tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k))).*...
          ((tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k+1))).^2/...
           (tnew(ind_ext(k))-tnew(ind_ext(k+1)))^2);	  
   hj1j = (1-2*(tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k+1)))/...
           (tnew(ind_ext(k+1))-tnew(ind_ext(k))))...
        .*((tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k))).^2/...
          (tnew(ind_ext(k+1))-tnew(ind_ext(k)))^2);
   kj1j  = (tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k+1))).*...
          ((tnew(ind_ext(k):ind_ext(k+1))-tnew(ind_ext(k))).^2/...
           (tnew(ind_ext(k+1))-tnew(ind_ext(k)))^2);	  
   pol = [pol env(2*(k-1)+1)*hjj+env(2*(k-1)+2)*kjj+env(2*(k-1)+3)*hj1j...
              +env(2*(k-1)+4)*kj1j];
  end	      
 end      
endfunction

function [pol,pol1,snew,t,tnew,ind_ext] = calcul_enveloppe(f,T,pas)

   //f : the signal
   //T : the signal duration
   //pas : the time span
   
   t = 0:pas:T;
   s = feval(t,f); // we compute the function f at each time t
   [ind_ext,ind_head,k_first,k_last,snew,tnew] = Maxlocsym(s,pas);
   [B,M1,S1,N1,S2,Q,S3] = calculmatrice(snew,tnew,ind_ext);
   //construction of the matrix of constraints  
   C = [Q;M1;N1];
   b = [S3';S1';S2'];
   [env,lagr,f0]=quapro(B,zeros(2*length(ind_ext),1),C,b,[],[],2*length(ind_ext),'g');
   pol1 = reconspol(env,ind_ext,tnew,pas);
   //restriction of the envelope to the indices of the old signal
   pol = pol1(ind_head+1:length(s)+ind_head);
   
endfunction 
