(* Phase space approach for pure harm. osc. *)
Off[General::"spell1", General::"spell"];
Clear["@"];
<< "D:\\sergeev\\PURDUE\\MATH\PACKAGES\\printing.m";
<< "D:\\sergeev\\bgu\\math\\packages\\harm.m";
<< LinearAlgebra`Orthogonalization`;
dir = "D:\\sergeev\\bgu\\math\\zilberg";
SetDirectory[dir];
inpfile = "displace.mx";
Get[inpfile];
aulengthinangstrom = 0.529169;
auenergyininvcm = 219475.;
uinelmass = 1822.89;
(* Transformation to atomic units *)
{freqs1, freqs2} = {freqs1, freqs2}/auenergyininvcm;
displ = displ/aulengthinangstrom Sqrt[uinelmass]; (* mass scaled coordinates *)
en1 = Plus @@ (1/2 freqs1);
en2 = Plus @@ (1/2 freqs2);
Print["Ground state energy: lower = ", en1, "   upper = ", en2];
envert = Plus @@ (freqs1^2 displ^2/2);
Print["Energy for allowed tr. = ", envert];
(* Renumeration of modes of excited state *)
freqs2old = freqs2; displold = displ;
freqs2 = displ = Table[Null, {nmodes}];
dush0 = Table[
      {amax, nmax} = 
        First[Sort[
            Transpose[{dush[[m]], Range[nmodes]}], #1[[1]]^2 > #2[[1]]^2 &]];
      sign = Sign[amax];
      freqs2[[m]] = freqs2old[[nmax]];
      displ[[m]] = sign displold[[nmax]];
      (*Print[m, " -> ", sign nmax];*)
      , {m, nmodes}];
(* Testing that everything was defined *)
Do[If[freqs2[[m]] === Null, Print["Element No. ", m, " is not defined!"]], {m,
       nmodes}];


(* Dropping the first mode *)
nmodes = nmodes - 1;
freqs1 = Drop[freqs1, 1];
freqs2 = Drop[freqs2, 1];
displ = Drop[displ, 1];

iprint = False;
{enmin, enmax, enstep} = {0.1, 1.01, 0.1};
Do[
    q = Table[Symbol["q" <> ToString[n]], {n, nmodes}];
    p = Table[Symbol["p" <> ToString[n]], {n, nmodes}];
    qp = Join[q, p];
    ham = Plus @@ (p^2 + freqs1^2 q^2)/2;
    wig = Plus @@ (p^2/freqs2 + freqs2(q - displ)^2)/2;
    dham = D[ham, #] & /@ qp;
    dwig = D[wig, #] & /@ qp;
    alpha = Join[freqs2/freqs1^2, 1/freqs2];
    qpa = Join[freqs1 displ , Table[0, {nmodes}]];
    {qpstar, lastar, wstar} = harm`harm0[alpha, qpa, en];
    mult = If[harm`private`x01 == 0, 2, 1];
    If[iprint, Print["X0 = ", harm`private`x01,
        "   multiplier = ", mult]];
    s = {Table[
            {q[[n]] -> qpstar[[n]]/freqs1[[n]],
              p[[n]] -> qpstar[[nmodes + n]]},
            {n, nmodes}], la -> lastar} // Flatten;
    wigstar = wig /. s;
    (*Testing*)
    If[iprint, Print["Test: ", (dwig - la dham) /. s, (ham /. s) - en]];
    (* New coordinates, orthogonal to the gradient *)
    newqp = 
      Transpose[
          Sort[Transpose[{qp, dham^2 /. s}], #1[[2]] > #2[[2]] &]][[1]]; 
    If[iprint, Print["Coordinates: ", newqp]];
    dwig = D[wig, #] & /@ newqp;
    dham = D[ham, #] & /@ newqp;
    gradham = Sqrt[Plus @@ (dham^2)];
    ddwig = D[dwig, #] & /@ newqp;
    ddham = D[dham, #] & /@ newqp;
    matm = (ddwig - la ddham) /. s;
    mata = IdentityMatrix[2 nmodes];
    mata[[1]] = dham /. s;
    mata = LinearAlgebra`Orthogonalization`GramSchmidt[mata];
    mata = MatrixPower[mata, -1];
    matb = Transpose[mata].matm.mata;
    matb = Take[matb, -(2*nmodes - 1)];
    matb = Transpose[Take[Transpose[matb], -(2*nmodes - 1)]];
    eig1 = Eigenvalues[matb];
    ifneg = False;
    Do[If[eig1[[n]] < 10.^-6, ifneg = True; Break[]], {n, 2*nmodes - 1}];
    If[ifneg,
      Print["The point (q-star,p-star) is not a minimum!"];
      Print["Eigenvalues of F-matrix:", eig1]];
    (*If[iprint, Print["Modified eigenvalues: ", MatrixForm[eig1]]];*)
    cgrad = gradham /. s;
    If[iprint, Print["|Grad| = ", cgrad]];
    det0 = Times @@ eig1;
    If[iprint, Print["Det F = ", det0]];
    If[iprint, Print["Lambda = ", lastar]];
    cexp = Exp[-(2 wigstar)];
    If[iprint, Print["W* = ", wigstar, "   exp(-2 W*) = ", cexp]];
    cgauss = 1/Sqrt[Pi det0]/cgrad;
    If[iprint, Print["C-Gauss = ", cgauss]];
    r = mult cgauss cexp;
    w0 = -1/2 Log[r];
    hamv = ham /. Table[p[[n]] -> 0, {n, nmodes}];
    bb = Sum[D[hamv, q[[n]]]^2 +
          D[hamv, {q[[n]], 2}]p[[n]]^2, {n, nmodes}];
    cairy = E^( -la^3 bb/3) /. s;
    If[iprint, Print["C-Airy = ", cairy]];
    r1 = mult cairy cgauss cexp;
    w1 = -1/2 Log[r1];
    Print["En = ", en, "   w0 = ", w0, "   w1 = ", w1];
    (* Saving *)
    outfile = "D:\\temp\\wigout.txt";
    os = OpenAppend[outfile, FormatType -> OutputForm, 
        PageWidth -> Infinity];
    Write[os, printF[en, 7, 2], "  ", printF[w0, 8, 3], " ", 
      printF[w1, 8, 3]];
    Close[os];
    , {en, enmin, enmax, enstep}];