o
    hȱ                     @   s   d dl Z d dlZd dlmZ ddlmZ eej	Z
eejZdd Zdd Zd	d
 Zdd Zdd Zdd Zdd Zdd ZdS )    N)qr   )get_arrays_tolc           6      K   s  |rft | tjr| jdksJ t|| sJ t |tjr%|j| jks'J t |tjr3|j| jks5J t |ts<J t |t	sCJ t
||}t||ksQJ t|| ks[J t|rd|dksfJ t|d}t|d}| j}t| } t| }	|dk | dk B |dk| dkB @ }
t| }t|}| |
  ||
< d}d}d}|t|
k r6| | }|dt | tdtj|  krŐnqzt|||}W n
 ty   Y n_w | | d| krnS||}|| }|tt| krt| | d}ntj}t||}| |d| |   d| krn |tj k|t t||  k @ }|tjk |tt||  k@ }t|tj}t|tj}t|| ||  ||  d||< t|| ||  ||  d||< t|}t|}t||}t||}|dkrt||
 |||
   ||
 ||
 ||
< | || 7 } |||d| |   8 }|t||k r| |
 ||
  | }|||
  | |
  ||
< d||
 < |d7 }n]||k r||krt|}|| ||< nt|}|| ||< d|
|< | |
  ||
< d||
 < d}n)||krt |}|| ||< d|
|< ||kr,t |}|| ||< d|
|< d	}n|t|
k s|!d
d	r<|r<t|}|	| d| ||  } t|
dkr+||
 ||
  }!| |
 | |
  }"| |
 ||
  }#t"t|!|" |#d  d }|#||
  |!| |
   ||
< d||
 < |d| kst#|t t||
  krn||
  |   < t$|}$t$|}%||
 d ||
 d  ||
 d  |$|
< ||
 d ||
 d  ||
 d  |%|
< t"|$|$dk ||$dk  |$|$dk< t"|%|%dk ||%dk  |%|%dk< t|| d}&t|| d}'|$t|& k}|%t|' k}t%|}(t%|})t|(| |&| |$|  |(|< t|)| |'| |%|  |)|< t|(}*t|)}+t|*|+},||}-||}||- }.|| }|| }/d}0t&|0d |, d }0t'|,|0 |,|0}1d|1 d|1d   }2|2|#|1 | |1|.  |2|1|/ d||.      }3t|3dkrnut(|3}4d|1|4 d  d|1|4 d   }5|5||
  |2|4 ||
   ||
< | |5d |- |2|4 |  7 } ||3|4 7 }|,dk r"|4|0d kr"|*|,krt |(}|| ||< d|
|< |+|,kr!t |)}|| ||< d|
|< nnt|
dksY|	| d| ||  | kr<|}|r`t||ksIJ t||ksSJ tj|d| k s`J |S )ad  
    Minimize approximately a quadratic function subject to bound constraints in
    a trust region.

    This function solves approximately

    .. math::

        \min_{s \in \mathbb{R}^n} \quad g^{\mathsf{T}} s + \frac{1}{2}
        s^{\mathsf{T}} H s \quad \text{s.t.} \quad
        \left\{ \begin{array}{l}
            l \le s \le u\\
            \lVert s \rVert \le \Delta,
        \end{array} \right.

    using an active-set variation of the truncated conjugate gradient method.

    Parameters
    ----------
    grad : `numpy.ndarray`, shape (n,)
        Gradient :math:`g` as shown above.
    hess_prod : callable
        Product of the Hessian matrix :math:`H` with any vector.

            ``hess_prod(s) -> `numpy.ndarray`, shape (n,)``

        returns the product :math:`H s`.
    xl : `numpy.ndarray`, shape (n,)
        Lower bounds :math:`l` as shown above.
    xu : `numpy.ndarray`, shape (n,)
        Upper bounds :math:`u` as shown above.
    delta : float
        Trust-region radius :math:`\Delta` as shown above.
    debug : bool
        Whether to make debugging tests during the execution.

    Returns
    -------
    `numpy.ndarray`, shape (n,)
        Approximate solution :math:`s`.

    Other Parameters
    ----------------
    improve_tcg : bool, optional
        If True, a solution generated by the truncated conjugate gradient
        method that is on the boundary of the trust region is improved by
        moving around the trust-region boundary on the two-dimensional space
        spanned by the solution and the gradient of the quadratic function at
        the solution (default is True).

    Notes
    -----
    This function implements Algorithm 6.2 of [1]_. It is assumed that the
    origin is feasible with respect to the bound constraints and that `delta`
    is finite and positive.

    References
    ----------
    .. [1] T. M. Ragonneau. *Model-Based Derivative-Free Optimization Methods
       and Software*. PhD thesis, Department of Applied Mathematics, The Hong
       Kong Polytechnic University, Hong Kong, China, 2022. URL:
       https://theses.lib.polyu.edu.hk/handle/200/12294.
               r   F      $      ?:0yE>      ?Timprove_tcg       @:0yE      皙?))
isinstancenpndarrayndiminspect	signaturebindshapefloatboolr   allisfiniteminimummaximumsizecopy
zeros_likecount_nonzeroEPSmaxlinalgnorm	_alpha_trZeroDivisionErrorTINYabsinfmin	full_likeclipargmin_argmingetsqrtanyzerosonesintlinspaceargmax)6grad	hess_prodxlxudeltadebugkwargstoln	grad_origfree_bdstepsdkreductboundary_reachedgrad_sdalpha_trhess_sdcurv_sd
alpha_quadalphai_xli_xuall_alpha_xlall_alpha_xualpha_xlalpha_xualpha_bdbetai_new	step_basestep_comparatorstep_sqgrad_sq	grad_steptemp_xltemp_xudist_xldist_xuall_t_xlall_t_xut_xlt_xut_bd	hess_step	curv_stepcurv_step_sd	n_samples	t_samples
sin_values
all_reducti_max	cos_value ro   V/var/www/vscode/kcb/lib/python3.10/site-packages/scipy/_lib/cobyqa/subsolvers/optim.pytangential_byrd_omojokun   sr  @


 

"
 $ 













`



""









hrq   c	           H      K   sJ  |rt | tjr| jdksJ t|| sJ t |tjr%|j| jks'J t |tjr3|j| jks5J t |tjrH|jdkrH|jd | jksJJ t |tjr]|jdkr]|j|jd ks_J t |tjrr|jdkrr|jd | jkstJ t |t	s{J t |t
sJ t||}
t||
ksJ t||
 ksJ t||
 ksJ t|r|dksJ t|d}t|d}t|d}| j}t| } t| }|dk | dk B }|dk| dkB }|dk||  dkB }t|||||\}}t| }|dd|df  |dd|df j|   }t|}d}d}d}||| k r#| | }|dt | tdtj|  kr9nzt|||}W n tyL   Y nw | | d	| krYn||}|| }|tt| krst| | d}ntj}t||}| |d
| |   d	| krn||tj k@ |t t||  k @ }||tjk @ |tt||  k@ }t|tj} t|tj}!t|| ||  ||  d| |< t|| ||  ||  d|!|< t| }"t|!}#t|"|#}$|| }%||%tt| k@ }&t|tj}'||& |%|&  |'|&< tj|'tjd}(t||$|(}|dkrNt|||  ||}| || 7 } td|||%  }|||d
| |   8 }|t||$|(k r|dd|df |dd|df j|   })|)| | }*|*| |) }|d7 }n||k r|"|krt | }+||+ ||+< d||+< n|#|krt |!}+||+ ||+< d||+< n	t |'}+d||+< t|||||\}}|dd|df  |dd|df j|   }d}n@|"|krt!| }+||+ ||+< d||+< |#|krt!|!}+||+ ||+< d||+< |(|krt!|'}+d||+< t|||||\}}d}n||| k s!|	"ddr|r||k rt|},||k r|dd|df |dd|df j|  }-|dd|df |dd|df j|   })|-|- }.|)|) }/|)|- }0t#t|.|/ |0d  d }|dd|df |dd|df j|0| |.|     }|d| kst$|t t| krn	||  }t%|}1t%|}2t|| d}3t|| d}4|| d |3| |3| d|-|     |1|< || d |4| |4| d|-|     |2|< t#|1|1dk ||1dk  |1|1dk< t#|2|2dk ||2dk  |2|2dk< |1t|3 k}|2t|4 k}t&|}5t&|}6t|5| |3| |1|  |5|< t|6| |4| |2|  |6|< t|5}7t|6}8t|7|8}9t|}:||- };|| }%|%| d || || d|;|     |:|< t#|:|:dk |%|:dk  |:|:dk< |:t| k}&t'|}<t|<|& ||& |:|&  |<|&< tj|<dd}=t|9|=}>||-}?||}|-|? }@|| }|-| }Ad}Bt(|Bd |> d }Bt)|>|B |>|B}Cd|C d|Cd   }D|D|0|C | |Dd
|Cd  |@ d|C |A  d
|     }Et|Edkr$nt*|E}Fd|C|F d  d|C|F d   }Gt||Gd |-  |D|F |  ||}| |Gd |? |D|F |  7 } td||Gd |;  |D|F |%  }||E|F 7 }|>dk r|F|Bd kr|7|>krt!|5}+||+ ||+< d||+< |8|>krt!|6}+||+ ||+< d||+< |=|>krt!|<}+d||+< t|||||\}}nn||k s<|| d
| ||  ||, d
|, ||,  kr|,}|r#t||}
t||ksJ t||ksJ t|| ||
 ksJ tt|| |
ksJ tj|d| k s#J |S )af
  
    Minimize approximately a quadratic function subject to bound and linear
    constraints in a trust region.

    This function solves approximately

    .. math::

        \min_{s \in \mathbb{R}^n} \quad g^{\mathsf{T}} s + \frac{1}{2}
        s^{\mathsf{T}} H s \quad \text{s.t.} \quad
        \left\{ \begin{array}{l}
            l \le s \le u,\\
            A_{\scriptscriptstyle I} s \le b_{\scriptscriptstyle I},\\
            A_{\scriptscriptstyle E} s = 0,\\
            \lVert s \rVert \le \Delta,
        \end{array} \right.

    using an active-set variation of the truncated conjugate gradient method.

    Parameters
    ----------
    grad : `numpy.ndarray`, shape (n,)
        Gradient :math:`g` as shown above.
    hess_prod : callable
        Product of the Hessian matrix :math:`H` with any vector.

            ``hess_prod(s) -> `numpy.ndarray`, shape (n,)``

        returns the product :math:`H s`.
    xl : `numpy.ndarray`, shape (n,)
        Lower bounds :math:`l` as shown above.
    xu : `numpy.ndarray`, shape (n,)
        Upper bounds :math:`u` as shown above.
    aub : `numpy.ndarray`, shape (m_linear_ub, n)
        Coefficient matrix :math:`A_{\scriptscriptstyle I}` as shown above.
    bub : `numpy.ndarray`, shape (m_linear_ub,)
        Right-hand side :math:`b_{\scriptscriptstyle I}` as shown above.
    aeq : `numpy.ndarray`, shape (m_linear_eq, n)
        Coefficient matrix :math:`A_{\scriptscriptstyle E}` as shown above.
    delta : float
        Trust-region radius :math:`\Delta` as shown above.
    debug : bool
        Whether to make debugging tests during the execution.

    Returns
    -------
    `numpy.ndarray`, shape (n,)
        Approximate solution :math:`s`.

    Other Parameters
    ----------------
    improve_tcg : bool, optional
        If True, a solution generated by the truncated conjugate gradient
        method that is on the boundary of the trust region is improved by
        moving around the trust-region boundary on the two-dimensional space
        spanned by the solution and the gradient of the quadratic function at
        the solution (default is True).

    Notes
    -----
    This function implements Algorithm 6.3 of [1]_. It is assumed that the
    origin is feasible with respect to the bound and linear constraints, and
    that `delta` is finite and positive.

    References
    ----------
    .. [1] T. M. Ragonneau. *Model-Based Derivative-Free Optimization Methods
       and Software*. PhD thesis, Department of Applied Mathematics, The Hong
       Kong Polytechnic University, Hong Kong, China, 2022. URL:
       https://theses.lib.polyu.edu.hk/handle/200/12294.
    r   r   r   r   NFr   r   r	   r
   initialTr   r   r   r   r   r   )+r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r    qr_tangential_byrd_omojokunr!   Tr#   r$   r%   r&   r'   r(   r)   r*   r+   r,   r-   r.   r/   r0   r1   r2   r3   r4   r5   	ones_liker6   r7   r8   )Hr9   r:   r;   r<   aubbubaeqr=   r>   r?   r@   rA   rB   free_xlfree_xufree_ubn_actqrD   rE   residrF   rG   rH   rI   rJ   rK   rL   rM   rN   rO   rP   rQ   rR   rS   rT   rU   aub_sdi_uball_alpha_ubalpha_ub	grad_projrV   rW   rX   	step_projrZ   r[   r\   r]   r^   r_   r`   ra   rb   rc   rd   re   temp_ubaub_stepall_t_ubt_ubt_minrf   rg   rh   ri   rj   rk   rl   rm   rn   ro   ro   rp   $constrained_tangential_byrd_omojokunC  s"  S









0
$
 ($



.








0


x

..$

























 
 
r   c           N      K   s  |rt | tjr| jdksJ t |tjr"|jdkr"|j| jd ks$J t |tjr9|jdkr9|jd | jd ks;J t |tjrN|jdkrN|j|jd ksPJ t |tjr_|j| jd fksaJ t |tjrp|j| jd fksrJ t |tsyJ t |tsJ t||}	t	||	ksJ t	||	 ksJ t
|r|dksJ t|d}t|d}| j\}
}tj|j|  td| f }|dk |d| dk B }|dk|d| dkB }|dk }|dk| |d|  ||d  dkB }t| ||||\}}t|| ||d ||d   }t|}|dd|df  |dd|df j|  }|||d  }d}d}d}|||
 | k r|| }|dt | tdtj| kr]nqzt||d| |}W n tyv   tj}Y nw zt|t||d ||d |}W n
 ty   Y nw | | d	| krn-tj|j||d|   ||d f }|| }|tt| krt| | d}ntj}t||}| |d
| |   d	| krn||tj k@ |d| t t||  k @ } ||tjk @ |d| tt||  k@ }!|||d t t||d  k @ }"t|tj}#t|tj}$t|tj}%t||  ||   |d| |   d|#| < t||! ||!  |d| |!  d|$|!< t||d |"  ||d |"  d|%|"< t|#}&t|$}'tj|%tjd}(t|&|'|(})| |d|  ||d  }*||*tt| k@ }+t|tj},||+ |*|+  |,|+< tj|,tjd}-t||)|-}|dkrt|||d|   ||}||| 7 }td|||*  }|||d
| |   8 }|t||)|-k r0|dd|df |dd|df j|  }.|.| | }/|/| |. }|d7 }n||k r|&|krJt|#}0||0 ||0< d||0< n-|'|kr_t|$}0||0 ||0< d||0< n|(|krnt|%}0d||0< n	t|,}0d||0< t| ||||\}}|dd|df  |dd|df j|  }d}n)|&|krt|#}0||0 ||0< d||0< |'|krt|$}0||0 ||0< d||0< d}n	|||
 | k sE| ddrQ|rQt!|}1||@ }2| jt| | | d |j|| |   }t|}t"|2dkr||2 ||2  }3||2 ||2  }4||2 ||2  }5tt|3|4 |5d  d }|5||2  |3||2   ||2< d||2 < |d| ksUt#|t t||2  krWn||2  |   < t|}6t|}7||2 d ||2 d  ||2 d  |6|2< ||2 d ||2 d  ||2 d  |7|2< t|6|6dk ||6dk  |6|6dk< t|7|7dk ||7dk  |7|7dk< t|| d}8t|| d}9|6t|8 k} |7t|9 k}!t$|}:t$|};t|:|  |8|  |6|   |:| < t|;|! |9|! |7|!  |;|!< t|:}<t|;}=t|<|=}>d}?t%|?d |> d }?t&|>|? |>|?}@t| | | d}A|| | }Bt!|}Cd|C|2 < t'|?}Dt(|?D ]F}Ed|@|E  d|@|E d   }Ft||F||@|E |C    ||}Gt| |G | d}H||G | }Id
|A|A |B|B  |H|H  |I|I   |D|E< qEt	|Ddkrnt)|D}Jd|@|J d  d|@|J d   }Kd|@|J  d|@|J d   }F|K||2  |F||2   ||2< | jt| | | d |j|| |   }||D|J 7 }|>dk r|J|?d kr|<|>krt|:}0||0 ||0< d|2|0< |=|>krt|;}0||0 ||0< d|2|0< nnt"|2dkst| | | d}At| |1 | d}L|| | }B||1 | }M|A|A |B|B  |L|L |M|M  krQ|1}|rut	||ks^J t	||kshJ tj|d| k suJ |S )a	  
    Minimize approximately a linear constraint violation subject to bound
    constraints in a trust region.

    This function solves approximately

    .. math::

        \min_{s \in \mathbb{R}^n} \quad \frac{1}{2} \big( \lVert \max \{
        A_{\scriptscriptstyle I} s - b_{\scriptscriptstyle I}, 0 \} \rVert^2 +
        \lVert A_{\scriptscriptstyle E} s - b_{\scriptscriptstyle E} \rVert^2
        \big) \quad \text{s.t.}
        \quad
        \left\{ \begin{array}{l}
            l \le s \le u,\\
            \lVert s \rVert \le \Delta,
        \end{array} \right.

    using a variation of the truncated conjugate gradient method.

    Parameters
    ----------
    aub : `numpy.ndarray`, shape (m_linear_ub, n)
        Matrix :math:`A_{\scriptscriptstyle I}` as shown above.
    bub : `numpy.ndarray`, shape (m_linear_ub,)
        Vector :math:`b_{\scriptscriptstyle I}` as shown above.
    aeq : `numpy.ndarray`, shape (m_linear_eq, n)
        Matrix :math:`A_{\scriptscriptstyle E}` as shown above.
    beq : `numpy.ndarray`, shape (m_linear_eq,)
        Vector :math:`b_{\scriptscriptstyle E}` as shown above.
    xl : `numpy.ndarray`, shape (n,)
        Lower bounds :math:`l` as shown above.
    xu : `numpy.ndarray`, shape (n,)
        Upper bounds :math:`u` as shown above.
    delta : float
        Trust-region radius :math:`\Delta` as shown above.
    debug : bool
        Whether to make debugging tests during the execution.

    Returns
    -------
    `numpy.ndarray`, shape (n,)
        Approximate solution :math:`s`.

    Other Parameters
    ----------------
    improve_tcg : bool, optional
        If True, a solution generated by the truncated conjugate gradient
        method that is on the boundary of the trust region is improved by
        moving around the trust-region boundary on the two-dimensional space
        spanned by the solution and the gradient of the quadratic function at
        the solution (default is True).

    Notes
    -----
    This function implements Algorithm 6.4 of [1]_. It is assumed that the
    origin is feasible with respect to the bound constraints and that `delta`
    is finite and positive.

    References
    ----------
    .. [1] T. M. Ragonneau. *Model-Based Derivative-Free Optimization Methods
       and Software*. PhD thesis, Department of Applied Mathematics, The Hong
       Kong Polytechnic University, Hong Kong, China, 2022. URL:
       https://theses.lib.polyu.edu.hk/handle/200/12294.
    r   r   r   r   NFr   r   r	   r
   rr   Tr   r   r   r   r   r   )*r   r   r   r   r   r   r   r   r   r   r   r   r   r_ru   qr_normal_byrd_omojokunr2   r4   r#   r$   r%   r&   r'   r(   r+   r,   r)   r*   r-   r.   r/   r0   r1   r    r"   r3   r5   r6   r7   emptyranger8   )Nrw   rx   ry   beqr;   r<   r=   r>   r?   r@   m_linear_ubrA   r9   rz   r{   
free_slackr|   r}   r~   delta_slackrD   rE   r   rF   rG   rH   rI   rJ   rK   rL   rM   rN   rO   rP   i_slackrQ   rR   all_alpha_slackrS   rT   alpha_slackrU   r   r   r   r   r   rV   rW   rX   rC   rZ   r[   r\   r]   r^   r_   r`   ra   rb   rc   rd   re   ri   rj   resid_ubresid_eqr   rl   i	sin_valuestep_altresid_ub_altresid_eq_altrm   rn   resid_ub_baseresid_eq_basero   ro   rp   normal_byrd_omojokun  s  C





""

 (&
0$
&(
 0,( 


.












0

{





""














or   c                 C   s   |j }t|}tt|g| | d d f g|| d d f  g|| d d f ggjdd\}}}	ttt|dt	 | tj
j|d t|jd t|jf dd k}
|
|fS NT)pivotingg      $@r   )axis)r   r   eyer   blockru   r"   r*   diagr#   r%   r&   r,   r   )rw   ry   rz   r{   r|   rA   identityr~   r_r}   ro   ro   rp   rt   c  s2   
.rt   c                 C   s6  | j \}}t|}t|}tt| | d d f || d d f  gt|t| |f|| d d f  g|| d d f  t|t| |fg|| d d f t|t| |fggjdd\}	}
}ttt	|
dt
 ||  tjj|
d t|
j d t|
j f dd k}||	fS r   )r   r   r   r   r   r4   r"   ru   r*   r   r#   r%   r&   r,   )rw   rz   r{   r   r|   r   rA   
identity_n
identity_mr~   r   r   r}   ro   ro   rp   r   {  sD   


.r   c                 C   s   | | }|| }|d | |   }t t|d ||  d}|dkr7|tt||  kr7t|| | d}|S t|| t| krLt|||  d}|S t)Nr   r   )r   r2   r$   r)   r*   r(   )rD   rE   r=   step_sdsd_sq
dist_tr_sqtemprJ   ro   ro   rp   r'     s   r'   c                 C   s   t | t | kS N)r   flatnonzeror$   xro   ro   rp   _argmax     r   c                 C   s   t | t | kS r   )r   r   r,   r   ro   ro   rp   r0     r   r0   )r   numpyr   scipy.linalgr   utilsr   finfor   tinyr)   epsr#   rq   r   r   rt   r   r'   r   r0   ro   ro   ro   rp   <module>   s*      8   &  %