debug needsPackage "NumericalAlgebraicGeometry"

-- input conversion functions
toMathematica = f -> replace("_", "", replace("\\)", "]", replace("\\(", "[", toString f)))
toMatlab = f -> replace("[_\\)\\(,]", "", toString f)

-- Input:  d, the size of matrices
--         DR, a row vector with the entries r_ij, j>=i, followed by d_i of the matrices R and D
-- Output: (D,R), matrices D and R (diagonal and symmetric)
getDandR = method()
getDandR(ZZ,Matrix) := (d,DR) -> (
    K := ring DR;
    a := drop(flatten entries DR, -d);
    R := map(K^d,K^d,(i,j)->if i<j then a#((2*d-i+1)*i//2+j-i) else a#((2*d-j+1)*j//2+i-j));
    D := diagonalMatrix(K, b := take(flatten entries DR, -d));
    (D,R)	 
    )

-- Nuij operators
Txs = (p,s,d) -> if d==0 then p else ((x,y,t) := toSequence drop(gens ring p,1); p':= Txs(p,s,d-1); p' + s*x*diff(t,p'))
Tys = (p,s,d) -> if d==0 then p else ((x,y,t) := toSequence drop(gens ring p,1); p':= Tys(p,s,d-1); p' + s*y*diff(t,p'))
Gs = (p,s) -> ((x,y,t) := toSequence drop(gens ring p,1); sub(p, matrix{{s,s*x,s*y,t}}))
Fs = (p,s,d) -> Txs (Tys (p,s,d),s,d)

-- random Nuij
Trs = (p,s) -> (
    -- c := pi*random RR/2; a := cos c; b := sin c;
--    a := abs promote(random RR,R); b := promote(random RR,R);
    a := abs random RR; b := random RR;
    << "-- random linear form: "<< a << " x + "<< b << " y" << endl;
    (x,y,t) := toSequence drop(gens ring p,1); 
    p+s*(a*x+b*y)*diff(t,p)
    )
Fs'randomized = (p,s,d) -> if d==0 then p else Fs'randomized(Trs(p,s),s,d-1)

-- Fs = (p,s,d) -> ((x,y,t) := toSequence drop(gens ring p,1); Txs (Tys (p,s,d//2+1),s,d//2)) -- bad alternative!!!

-- in: a homogeneous polynomial of degree d in {x,y,t}
-- out: a polynomial in {s,x,y,t}  
makeNuijHomotopy = method(Options=>{Randomized=>true})
makeNuijHomotopy RingElement := o -> p -> (
--    assert isHomogeneous p; -- does not work for parametric coefficients
    d := first degree p;
    R := ring p;
    s := symbol s;
    Rs := (coefficientRing R) [s, gens R];
    (x,y,t) := toSequence drop(gens Rs,1);
    toRs := map(Rs,R,{x,y,t});
    (if o.Randomized then Fs'randomized else Fs) (Gs(toRs p,s),1-s,d)
    )
 
trackNuijHomotopy = method(Options=>{
	StartS=>1_CC, 
	TargetS=>0_CC, 
	NuijPathEndDR=>null,
	NuijPathEndP=>null,
	Tolerance=>0.001,
	Randomized=>true
	})
trackNuijHomotopy (Matrix, Ring, RingElement, Matrix) := o->(DR, XYT, PhiDR, Phi) -> (
    CC'XYT := CC[gens XYT];
    p := sub( sub(PhiDR, vars XYT|DR), CC'XYT );
    p'Nuij := makeNuijHomotopy (
	if o.NuijPathEndDR =!= null then sub( sub(PhiDR, vars XYT|o.NuijPathEndDR), CC'XYT )
        else if o.NuijPathEndP =!= null then sub(o.NuijPathEndP,CC'XYT)
	else p,
	Randomized=>o.Randomized
	);
    sXYT := ring p'Nuij;

    at's := s -> map(CC'XYT,sXYT,matrix{{toCC s}}|vars CC'XYT); -- specialize value of s

    assert ( norm(p - (at's o.StartS) p'Nuij)<2*o.Tolerance );-- should be close to 0
    print (at's 0) p'Nuij; -- should not depend on p

    monoms := rsort first entries basis(first degree p, CC'XYT);
    get'coeffs := p -> ( -- get'coefficients of a polynomials as a row vector
--	print transpose (last coefficients (p,Monomials=>monoms) - last coefficients p);
	transpose matrix{flatten entries last coefficients (p,Monomials=>monoms) / (a->sub(a,CC))}
	);
    least'squares := (A,b) -> transpose solve(transpose A * A, transpose A*b); -- "solve" Ax=b

    J := transpose jacobian transpose Phi;
    X0 := DR;
    PhiX0 := get'coeffs p;
    s0 := o.StartS;    
    ds := (o.TargetS-o.StartS)/100;--!!!
    done := false;
    had'enough := 3;--!!!
    while s0 != o.TargetS do (
	if abs ds < 0.000001 --!!! 
	then (
	    print "--step is too small!!!";
	    break
	    );
	sv := first SVD sub(J,X0);
	<< "--condition number = " << first sv / last sv << endl;
	<< "-- s0 = " << s0 << "; ds = " << ds << endl;
	s1 := if abs(s0+ds - o.StartS) >= abs (o.TargetS - o.StartS) 
	      then o.TargetS 
	      else s0+ds;
	wantedPhiX1 := get'coeffs (at's s1) p'Nuij;
	X1 := X0;
	PhiX1 := PhiX0;
	local v;
    	count := 1;
	while norm(
	    v = wantedPhiX1 - PhiX1
	    ) > o.Tolerance --!!!
	do (
	    if count%10 == 0 then << " -- distance = " << norm v << endl;
	    J0 := sub(J,X0);
	    da := least'squares(J0,v);
	    if count%1 == 0 then << "." << flush;
	    X1 = X1 + da;
	    PhiX1 = sub(Phi,X1);
	    count = count+1;
     	    if count > had'enough then break;
     	    );
	if count > had'enough then (
	    -- failure; shorten the step
	    ds = ds/1.2; --!!!
	    )
	else ( --success; lengthen the step
	    ds = 1.2*ds; --!!!
	    X0 = X1;
	    PhiX0 = PhiX1;
	    s0 = s1;	
	    )
	);
    X0 
    )

makeNuijPathSLP = method(Options=>{Randomized=>true,TypeOfSLP=>preSLP})
makeNuijPathSLP (ZZ,Thing,Number,Number) := o->(d,DRorPolynomial,A,B) -> (
    pre := o.TypeOfSLP === preSLP; -- else GateMatrix
    towardN := (class DRorPolynomial === Matrix);
    PRECISION := precision(if towardN then ring DRorPolynomial else coefficientRing ring DRorPolynomial);
    K := CC_PRECISION;
    setRandomSeed 0;
    xyt := map(K^(binomial(d+2,d)-1), K^3, (i,j)->exp(2*pi*ii*random RR)); -- interpolation points on unit circle
    (C,PhiDR) := determinantalRepresentationSLP(d,xyt); -- C is not used
    if pre then (
	PhiDRxyt := (determinantalRepresentation(d,K))#1;
    	XYT := ring PhiDRxyt;
    	CC'XYT := K[gens XYT];
	);
    hyperPoly := (
	if towardN 
	then sub(sub(PhiDRxyt, vars XYT | sub(DRorPolynomial,XYT)), CC'XYT) -- DR
	else sub(DRorPolynomial,vars CC'XYT) -- Polynomial
	);
    nuij'homotopy := makeNuijHomotopy(hyperPoly,Randomized=>o.Randomized);
    local s;
    (s,x,y,t) = toSequence gens ring nuij'homotopy;
    homotopyRing := K[gens coefficientRing XYT,s];
    
    -- we need to track on the segment [A,B] (note: N-path parameter goes from 1 to 0; the fixed polynomial corresponds to s=0)
    -- the "new" s will change on the segment [0,1]
    sub's := s*B+(1-s)*A; 
    nuij'homotopySLP := stackPreSLPs apply(entries xyt, row->
    	poly2preSLP sub(sub(-nuij'homotopy,matrix{{sub's}}|matrix{row}), homotopyRing) 
    	);
    (   
	homotopyRing,
	prunePreSLP transposePreSLP addPreSLPs {PhiDR,nuij'homotopySLP} -- need a row vector for output 
	) 
    ) 

-- Input: n, a positive integer; 
--        K, a field
-- Output: (0) the ring XYT = K[a_{ij}, d_i] [x,y,t]
-- (1) polynomial det(tI+xD+yR) 
-- (2) map Phi: 
--   from { (D,R) | D diagonal nxn and R symmetric nxn matrices }, 
--   to { polynomials of degree n in {x,y,t} }
-- Phi(D,R) = det(tI+xD+yR)
-- (!!! Note: the constant coefficient of the polynomial in the target can be dropped...)
determinantalRepresentation = method()
determinantalRepresentation (ZZ,Thing) := (n,K) -> ( 
    assert (instance(K,InexactFieldFamily) or instance(K,Ring));
    ind := select((1,1)..(n,n), i->i#0<=i#1);
    a := symbol a;
    d := symbol d;
    C := K[ind/(i->a_i),d_1..d_n]; -- coordinate ring of pairs of matrices (D,R)

     x := symbol x;
     y := symbol y;
     t := symbol t;
     XYT := C[x,y,t];
     
     -- a symmetric matrix with off-diagonal entries a_{ij} (or a_{ji}) and diagonal entries a_{ii}+d_i 
     R := map(XYT^n,XYT^n,(i,j)->if i<j then a_(i+1,j+1) else a_(j+1,i+1)); 
     
     D := diagonalMatrix(XYT, toList(d_1..d_n));
     I := map(XYT^n); -- identity
     --print "before"; 
     PhiDR := det(y*R+x*D+t*I);
     --error "debug me";
     -- !!! would be reasonable to drop constants
     -- matrix{select(flatten entries sub(last coefficients PhiDR,C), f->first degree f > 0)}
     (XYT, PhiDR, (map(C,XYT)) last coefficients PhiDR)
     )

-- Input: n, positive integer
--        xyt, matrix with rows (x,y,t) in  K^3 to be plugged in
-- Output: (1) the ring C = K[a_{ij}, d_i], K = ring xyt
--         (2) SLP that computes polynomial det(tI+xD+yR) 
determinantalRepresentationSLP = method(Options=>{TypeOfSLP=>preSLP})
determinantalRepresentationSLP (ZZ,Matrix) := o -> (n,xyt) -> ( 
    pre := o.TypeOfSLP === preSLP; -- else GateMatrix
    K := ring xyt;
    assert (instance(K,InexactFieldFamily) or instance(K,Ring));   
    ind := select((1,1)..(n,n), i->i#0<=i#1);
    a := symbol a;
    d := symbol d;
    C := if pre then K[ind/(i->a_i),d_1..d_n] -- coordinate ring of pairs of matrices (D,R)
    else (
	for i in ind do a_i = inputGate "a_"|i;
	for i from 1 to n do d_i = inputGate "d_"|i;
	ZZ -- needed only for identity matrix
	);
    PhiDR := (if pre then stackPreSLPs 
	else (L->transpose matrix {L})) apply(entries xyt, row->(
    	    (x,y,t) := toSequence row; 
            -- a symmetric matrix with off-diagonal entries a_{ij} (or a_{ji}) and diagonal entries a_{ii}+d_i 
    	    R := matrix table(n,n,(i,j)->if i<j then a_(i+1,j+1) else a_(j+1,i+1)); 
    	    D := diagonalMatrix(C, toList(d_1..d_n));
    	    I := map(C^n); -- identity 
    	    if pre then detPreSLP matrix2preSLP(y*R+x*D+t*I)
	    else det(y*R+x*D+t*I)
	    ));
    (C, PhiDR)
    )
///
determinantalRepresentationSLP (4,QQ)
///

hyperbolicPolynomial = method()
hyperbolicPolynomial (Matrix,Matrix) := (D,R) -> (
    K := ring D;
    x := symbol x;
    y := symbol y;
    t := symbol t;
    Kxyt := K[x,y,t];
    (x,y,t) = toSequence gens Kxyt;
    I := map(Kxyt^d);
    det(y*sub(R,Kxyt)+x*sub(D,Kxyt)+t*I)
    )

end