def solve_for_nearest( px,py,rx,ry ):
  dpx = polyder(px)
  dpy = polyder(py)
  cp = polymul( dpx, px ) + polymul( dpy, py )
  cp = polyadd( cp, -rx*dpx )      
  cp = polyadd( cp, -ry*dpy )
  t = roots(cp)
  t = real(t[isreal(t)])
  t = t[ (t>=0) * (t<=1) ]

  ##tt = linspace(0,1,100)
  ##from pylab import plot
  ##plot( polyval(px,tt), polyval(py,tt), 'k', hold = 0 )
  ##plot( [rx],[ry], 'r.' )
  ##plot( polyval(px,t[isreal(t)*(real(t)>=0)*(real(t)<=1)]),
  ##      polyval(py,t[isreal(t)*(real(t)>=0)*(real(t)<=1)]), 'o' )
  ##pdb.set_trace()

  if len(t):
    if len(t) == 1:
      return t[0]
    else:
      ux = polyval( px, t )
      uy = polyval( py, t )
      d = hypot( ux - rx, uy - ry )
      return t[ d==d.min() ][0]
  else:
    t = array([0.0,1.0])
    ux = polyval( px, t )
    uy = polyval( py, t )
    d = hypot( ux - rx, uy - ry )
    if d[0] < d[1]:
      return 0.0
    else:
      return 1.0

def compute_join_curvature( px, py ):
  from scipy.integrate import quad
  xp  = polyder( px, 1 )
  xpp = polyder( px, 2 )
  yp  = polyder( py, 1 )
  ypp = polyder( py, 2 )
  pn = polyadd( polymul( xp, ypp ), polymul( yp, xpp )) #numerator
  pd = polyadd( polymul( xp, xp ) , polymul( yp, yp ) ) #denominator
  integrand = lambda t:  fabs(polyval( pn, t )/( polyval( pd, t )**(1.5)) ) 
  return quad(integrand, 0, 1) [0]

def mean_curvature(w, side, dx, n=16):
    n = min(n, len(w.x) / 4)
    L = cumulative_path_length(w)
    tt = L / L.max()
    teval = tt[n] if side == 0 else tt[-n]
    px = np.polyfit(tt[n:-n], w.x[n:-n], 2)
    py = np.polyfit(tt[n:-n], w.y[n:-n], 2)
    xp = np.polyder(px, 1)
    xpp = np.polyder(px, 2)
    yp = np.polyder(py, 1)
    ypp = np.polyder(py, 2)
    pn = np.polyadd(np.polymul(xp, ypp), np.polymul(yp, xpp))  # numerator
    pd = np.polyadd(np.polymul(xp, xp), np.polymul(yp, yp))  # denominator
    kappa = lambda t: dx * np.polyval(pn, t) / (np.polyval(pd, t) ** (1.5))  # d Tangent angle/ds * ds/dt
    return quad(kappa, 0, 1, epsrel=1e-3)[0]

def Closed_loop(Kz, Kp, Gz, Gp):
    """
    Return zero and pole polynomial for a closed loop function.

    Parameters
    ----------
    Kz & Gz : list
        Polynomial constants in the numerator.
    Kz & Gz : list
        Polynomial constants in the denominator.

    Returns
    -------
    Zeros_poly : list
        List of zero polynomial for closed loop function.

    Poles_poly : list
        List of pole polynomial for closed loop function.

   """

    # calculating the product of the two polynomials in the numerator
    # and denominator of transfer function GK
    Z_GK = numpy.polymul(Kz, Gz)
    P_GK = numpy.polymul(Kp, Gp)

    # calculating the polynomial of closed loop
    # sensitivity function s = 1/(1+GK)
    Zeros_poly = Z_GK
    Poles_poly = numpy.polyadd(Z_GK, P_GK)
    return Zeros_poly, Poles_poly

    def sweep(self, wireFrameProjection, frame, lines,
              color=[0, 0, 255], sweepThick=5, fullsweepFrame=104):
        # calculate sweep angle
        halfcycle = fullsweepFrame / 2
        position = (frame % fullsweepFrame)
        if position > halfcycle:
            position = fullsweepFrame - position
        sweep = position / halfcycle
        allY = [n * 32 for n in range(int(self.projectedY / 32))]

        # calculate the wireframe positions
        nlanes = len(lines) - 1
        leftPolynomial = np.poly1d(lines[0].currentFit)
        rightPolynomial = np.poly1d(lines[nlanes].currentFit)

        # scanning sweep
        polySweepDiff = np.polysub(
            lines[nlanes].currentFit,
            lines[0].currentFit) * sweep
        sweepPoly = np.polyadd(leftPolynomial, polySweepDiff)
        allX = sweepPoly(allY)
        XYPolyline = np.column_stack((allX, allY)).astype(np.int32)
        cv2.polylines(wireFrameProjection, [XYPolyline], 0, color, sweepThick)
        sweepLane = 0
        for i in range(nlanes):
            leftLine = np.poly1d(lines[i].currentFit)
            rightLine = np.poly1d(lines[i+1].currentFit)
            if (leftLine([self.projectedY])[0] <
                    sweepPoly([self.projectedY])[0] and
                    sweepPoly([self.projectedY])[0] <
                    rightLine([self.projectedY])[0]):
                sweepLane = i
        return sweepLane

def error(plant, sensor=None, entrada=None):
    """Negative feedback connection of plant and sensor.
    If sensor is None, then it is assumed to be 1.
    """
    if not isinstance(plant, signal.lti):
        plant = signal.lti(*plant)

    if sensor is None:
        sensor = signal.lti([1], [1])
    elif not isinstance(sensor, signal.lti):
        sensor = signal.lti(*sensor)
        

    if entrada is None:
        entrada = signal.lti([1], [1])
    elif not isinstance(entrada, signal.lti):
        entrada = signal.lti(*entrada) 
 
        
        
 #   aux = np.polymul(plant.den, sensor.den)
    num = np.polymul(np.polymul(plant.den, sensor.den),entrada.num)
    den = np.polyadd(np.polymul(np.polymul(plant.den, sensor.den),entrada.den),
                     np.polymul(np.polymul(plant.num, sensor.num),entrada.den))
    sys = signal.lti(num, den)
    return sys

def fwhm_polyest_e(delta_energy, two_theta, alpha, F, e_f):
    """ Estimate the fwhm profile in an energy dispersive detector.

    Calculates the fwhm wrt. the measurement accuracy of the detectors,
    the Fano factor contribution and angle variation (due to slit size
    and positioning.

    Args:
        delta_energy (ndarray): Energy resolution wrt. energy (keV)
        two_theta (float): 2theta in radians
        alpha (float): Half the full angular variation.
        F (float): Fano factor (approx. 0.13 for Germanium)
        e_f (float): Energy to make electron-hole pair (approx. 3e-3 keV)

    Returns:
        ndarray: 1d array containing the estimated polynomial (k=2).
    """
    # d_E^2 used to calc FWHM -> calc polynomial such that d_E^2 = A*E + B
    if isinstance(delta_energy, (int, float)):
        res_sq = [0, delta_energy ** 2]
    else:
        e, res = delta_energy[:, 0], delta_energy[:, 1]
        res_sq = np.polyfit(e, [i ** 2 for i in res], 1)

    # Polynomial terms that should fit fwhm squared
    fw_base = [(2 * alpha / np.tan(two_theta)) ** 2, F * e_f * 2.35 ** 2, 0]
    fw_e_sq = np.polyadd(fw_base, res_sq)

    # Conversion factor to put fwhm squared in terms of q
    e_q = 1000 * eV * 4 * np.pi * np.sin(two_theta / 2) / (h * c * 1e10)
    return [fw_e_sq[0], fw_e_sq[1] * e_q, fw_e_sq[2] * e_q ** 2]

    def feedback(self, other=1, sign=-1):
        """Feedback interconnection between two LTI objects."""
        other = _convertToTransferFunction(other)

        if (self.inputs > 1 or self.outputs > 1 or
                other.inputs > 1 or other.outputs > 1):
            # TODO: MIMO feedback
            raise NotImplementedError("TransferFunction.feedback is currently \
only implemented for SISO functions.")

        # Figure out the sampling time to use
        if (self.dt is None and other.dt is not None):
            dt = other.dt       # use dt from second argument
        elif (other.dt is None and self.dt is not None) \
                or (self.dt == other.dt):
            dt = self.dt        # use dt from first argument
        else:
            raise ValueError("Systems have different sampling times")

        num1 = self.num[0][0]
        den1 = self.den[0][0]
        num2 = other.num[0][0]
        den2 = other.den[0][0]

        num = polymul(num1, den2)
        den = polyadd(polymul(den2, den1), -sign * polymul(num2, num1))

        return TransferFunction(num, den, dt)

def compute_join_length( px, py, tlow = 0.0, thigh = 1.0 ):
  from scipy.integrate import quad
  xp  = polyder( px, 1 )
  yp  = polyder( py, 1 )
  xp2 = polymul( xp, xp )
  yp2 = polymul( yp, yp )
  p   = polyadd( xp2, yp2 )
  integrand = lambda t: sqrt( polyval( p, t ) )
  return quad(integrand, tlow, thigh) [0]

def _addSISO(num1, den1, num2, den2):
    """Return num/den = num1/den1 + num2/den2.

    Each numerator and denominator is a list of polynomial coefficients.

    """

    num = polyadd(polymul(num1, den2), polymul(num2, den1))
    den = polymul(den1, den2)

    return num, den

def Closed_loop(Kz, Kp, Gz, Gp):
    """Kz & Gz is the polynomial constants in the numerator
    Kp & Gp is the polynomial constants in the denominator"""

    # calculating the product of the two polynomials in the numerator and denominator of transfer function GK
    Z_GK = numpy.polymul(Kz, Gz)
    P_GK = numpy.polymul(Kp, Gp)

    #calculating the polynomial of closed loop sensitivity function s = 1/(1+GK)
    Zeros_poly = Z_GK
    Poles_poly = numpy.polyadd(Z_GK, P_GK)
    return Zeros_poly, Poles_poly

def invres(r,p,k,tol=1e-3,rtype='avg'):
    """Compute b(s) and a(s) from partial fraction expansion: r,p,k

    If M = len(b) and N = len(a)

            b(s)     b[0] x**(M-1) + b[1] x**(M-2) + ... + b[M-1]
    H(s) = ------ = ----------------------------------------------
            a(s)     a[0] x**(N-1) + a[1] x**(N-2) + ... + a[N-1]

             r[0]       r[1]             r[-1]
         = -------- + -------- + ... + --------- + k(s)
           (s-p[0])   (s-p[1])         (s-p[-1])

    If there are any repeated roots (closer than tol), then the partial
    fraction expansion has terms like

            r[i]      r[i+1]              r[i+n-1]
          -------- + ----------- + ... + -----------
          (s-p[i])  (s-p[i])**2          (s-p[i])**n

    See Also
    --------
    residue, poly, polyval, unique_roots

    """
    extra = k
    p, indx = cmplx_sort(p)
    r = take(r,indx,0)
    pout, mult = unique_roots(p,tol=tol,rtype=rtype)
    p = []
    for k in range(len(pout)):
        p.extend([pout[k]]*mult[k])
    a = atleast_1d(poly(p))
    if len(extra) > 0:
        b = polymul(extra,a)
    else:
        b = [0]
    indx = 0
    for k in range(len(pout)):
        temp = []
        for l in range(len(pout)):
            if l != k:
                temp.extend([pout[l]]*mult[l])
        for m in range(mult[k]):
            t2 = temp[:]
            t2.extend([pout[k]]*(mult[k]-m-1))
            b = polyadd(b,r[indx]*poly(t2))
            indx += 1
    b = real_if_close(b)
    while allclose(b[0], 0, rtol=1e-14) and (b.shape[-1] > 1):
        b = b[1:]
    return b, a

def invresz(r, p, k, tol=1e-3, rtype='avg'):
    """Compute b(z) and a(z) from partial fraction expansion: r,p,k

    If M = len(b) and N = len(a)

            b(z)     b[0] + b[1] z**(-1) + ... + b[M-1] z**(-M+1)
    H(z) = ------ = ----------------------------------------------
            a(z)     a[0] + a[1] z**(-1) + ... + a[N-1] z**(-N+1)

                 r[0]                   r[-1]
         = --------------- + ... + ---------------- + k[0] + k[1]z**(-1) ...
           (1-p[0]z**(-1))         (1-p[-1]z**(-1))

    If there are any repeated roots (closer than tol), then the partial
    fraction expansion has terms like

               r[i]              r[i+1]                    r[i+n-1]
          -------------- + ------------------ + ... + ------------------
          (1-p[i]z**(-1))  (1-p[i]z**(-1))**2         (1-p[i]z**(-1))**n

    See also
    --------
    residuez, poly, polyval, unique_roots

    """
    extra = asarray(k)
    p, indx = cmplx_sort(p)
    r = take(r, indx, 0)
    pout, mult = unique_roots(p, tol=tol, rtype=rtype)
    p = []
    for k in range(len(pout)):
        p.extend([pout[k]] * mult[k])
    a = atleast_1d(poly(p))
    if len(extra) > 0:
        b = polymul(extra, a)
    else:
        b = [0]
    indx = 0
    brev = asarray(b)[::-1]
    for k in range(len(pout)):
        temp = []
        # Construct polynomial which does not include any of this root
        for l in range(len(pout)):
            if l != k:
                temp.extend([pout[l]] * mult[l])
        for m in range(mult[k]):
            t2 = temp[:]
            t2.extend([pout[k]] * (mult[k] - m - 1))
            brev = polyadd(brev, (r[indx] * poly(t2))[::-1])
            indx += 1
    b = real_if_close(brev[::-1])
    return b, a

def Closed_loop (Kz,Kp,Gz,Gp):
    """ Kz and Gz are the polynomial constants in the numerator 
    Kp and Gp are the polynomial constants in the denominator""" 
    
    
    
    # calculating the product of the two polynomials in the numerator and denominator of transfer function GK
    Z_GK         =np.polymul(Kz,Gz)
    P_GK         =np.polymul(Kp,Gp)    
    # calculating the polynomial of closed loop function T=(GK/1+GK)
    Zeros_poly   =Z_GK
    Poles_poly   =np.polyadd(Z_GK,P_GK)   
    return      Zeros_poly,Poles_poly

    def createPolyFitRight(self, curImgFtr, leftLane,
                           faint=1.0, resized=False):
        # create new right line polynomial
        polyDiff = np.polysub(leftLane.lines[leftLane.right].currentFit,
                              leftLane.lines[leftLane.left].currentFit)
        self.currentFit = np.polyadd(
            leftLane.lines[leftLane.right].currentFit, polyDiff)
        polynomial = np.poly1d(self.currentFit)
        self.allY = leftLane.lines[leftLane.right].allY
        self.currentX = polynomial(self.allY)
        self.allX = self.currentX

        if len(self.allY) > 75:
            # We need to increase our pixel count by 2 to get to 100%
            # confidence and maintain the current pixel count to keep
            # the line detection
            self.confidence_based = len(self.allY) * 2
            self.confidence = len(self.allY) / self.confidence_based
            self.detected = True

            # create linepoly
            xy1 = np.column_stack(
                (self.currentX + self.maskDelta, self.allY))
            xy1 = xy1.astype(np.int32)
            xy2 = np.column_stack(
                (self.currentX - self.maskDelta, self.allY))
            xy2 = xy2.astype(np.int32)
            self.linePoly = np.concatenate((xy1, xy2[::-1]), axis=0)

            # create mask
            self.linemask = np.zeros_like(self.linemask)
            cv2.fillConvexPoly(self.linemask, self.linePoly, 64)

            # Add the point at the bottom.
            allY = np.append(self.allY, self.projectedY - 1)
            allX = polynomial(allY)
            self.XYPolyline = np.column_stack((allX, allY))
            self.XYPolyline = self.XYPolyline.astype(np.int32)

            # create the accumulator
            self.bestFit = self.currentFit

            # classify the line
            # print("classifying the right line",self.side)
            self.getLineStats(
                curImgFtr.getRoadProjection(), faint=faint, resized=resized)

            # set bottom of line
            x = polynomial([self.projectedY - 1])
            self.pixelBasePos = x[0]

def Poly_Zeros_T(Poly_z_K, Poly_p_K, Poly_z_G, Poly_p_G):
    """Given the polynomial expansion in the denominator and numerator of the controller function K and G
    then this function return s the poles and zeros of the closed loop transfer function in terms of reference signal

    the arrays for the input must range from the highest order of the polynomial to the lowest"""

    Poly_z = numpy.polymul(Poly_z_K, Poly_z_G)
    Poly_p = numpy.polyadd(numpy.polymul(Poly_p_K, Poly_z_G), numpy.polymul(Poly_p_K, Poly_p_G))

    # return the poles and zeros of T
    Zeros = numpy.roots(Poly_z)
    Poles = numpy.roots(Poly_p)

    return Poles, Zeros

 def __add__(self, other):
     """An operator overload for adding two terms with ``+``."""
     if isinstance(other, Polynomial) and self.transform == other.transform:
         return Polynomial(coefficients=numpy.polyadd(self.coefficients, other.coefficients),
                           transform=self.transform)
     elif isinstance(other, Constant):
         if len(self.coefficients):  # pylint: disable=len-as-condition; coefficients might be a NumPy array, where __nonzero__ is not equivalent to len(.)
             coefficients = list(self.coefficients)
             coefficients[-1] += other.value
             return Polynomial(coefficients=coefficients, transform=self.transform)
         else:
             return -other
     else:
         return super().__add__(other)

def feedback(plant, sensor=None):
    """Negative feedback connection of plant and sensor.
    If sensor is None, then it is assumed to be 1.
    """
    if not isinstance(plant, signal.lti):
        plant = signal.lti(*plant)
    if sensor is None:
        sensor = signal.lti([1], [1])
    elif not isinstance(sensor, signal.lti):
        sensor = signal.lti(*sensor)
    num = np.polymul(plant.num, sensor.den)
    den = np.polyadd(np.polymul(plant.den, sensor.den),
                     np.polymul(plant.num, sensor.num))
    sys = signal.lti(num, den)
    return sys

def State_Space_mats(Gp_n, Gp_d, kc, ti, td):

    if ti == 0:
        Gc_d = 1
        Gc_n = [kc * ti * td, (kc * ti), kc]

    else:
        Gc_n = [kc * ti * td, (kc * ti), kc]
        Gc_d = [ti, 0]

    OL_TF_n = np.polymul(Gp_n, Gc_n)
    OL_TF_d = np.polymul(Gc_d, Gp_d)
    CL_TF_n = OL_TF_n
    CL_TF_d = np.polyadd(OL_TF_d, OL_TF_n)
    OL_mats = signal.tf2ss(OL_TF_n, OL_TF_d)  # Open Loop Transfer Function is converted to State Space
    CL_mats = signal.tf2ss(CL_TF_n, CL_TF_d)  # Closed Loop Transfer Function is converted to State Space

    return OL_mats, CL_mats

    def updatePolyFitRight(self, leftLane):
        # update new right line polynomial
        polyDiff = np.polysub(leftLane.lines[leftLane.right].currentFit,
                              leftLane.lines[leftLane.left].currentFit)
        self.currentFit = np.polyadd(
            leftLane.lines[leftLane.right].currentFit, polyDiff)
        polynomial = np.poly1d(self.currentFit)
        self.allY = leftLane.lines[leftLane.right].allY
        self.currentX = polynomial(self.allY)
        self.allX = self.currentX

        if len(self.allY) > 150:
            # We need to increase our pixel count by 2 to get to 100%
            # confidence and maintain the current pixel count to keep
            # the line detection
            self.confidence = len(self.allY) / self.confidence_based
            if self.confidence > 0.5:
                self.detected = True
                if self.confidence > 1.0:
                    self.confidence = 1.0
            else:
                self.detected = False

            # create linepoly
            xy1 = np.column_stack(
                (self.currentX + self.maskDelta, self.allY)).astype(np.int32)
            xy2 = np.column_stack(
                (self.currentX - self.maskDelta, self.allY)).astype(np.int32)
            self.linePoly = np.concatenate((xy1, xy2[::-1]), axis=0)

            # create mask
            self.linemask = np.zeros_like(self.linemask)
            cv2.fillConvexPoly(self.linemask, self.linePoly, 64)

            # Add the point at the bottom.
            allY = np.append(self.allY, self.projectedY - 1)
            allX = polynomial(allY)
            self.XYPolyline = np.column_stack((allX, allY)).astype(np.int32)

            # create the accumulator
            self.bestFit = self.currentFit