/**
   * Returns the state response matrix calculated from the front face of elemFrom to the back face
   * of elemTo. This is a convenience wrapper to the real method in the trajectory class
   *
   * <p>This method was moved here from EnvelopeTrajectory/EnvelopeProbe where it was eliminated
   * since <code>Trajectory</code> was genericized.
   *
   * @param strIdElemFrom String identifying starting lattice element
   * @param strIdElemTo String identifying ending lattice element
   * @return response matrix from elemFrom to elemTo
   * @see EnvelopeTrajectory#computeTransferMatrix(String, String)
   */
  public PhaseMatrix computeTransferMatrix(String strIdElemFrom, String strIdElemTo) {

    Trajectory<TransferMapState> trajectory = this.getTrajectory();

    // find starting index
    int[] arrIndFrom = trajectory.indicesForElement(strIdElemFrom);

    int[] arrIndTo = trajectory.indicesForElement(strIdElemTo);

    if (arrIndFrom.length == 0 || arrIndTo.length == 0)
      throw new IllegalArgumentException("unknown element id");

    int indFrom, indTo;
    indTo = arrIndTo[arrIndTo.length - 1]; // use last state before start element

    TransferMapState stateTo = trajectory.stateWithIndex(indTo);
    PhaseMatrix matTo = stateTo.getTransferMap().getFirstOrder();

    indFrom = arrIndFrom[0] - 1;
    if (indFrom < 0) return matTo; // response from beginning of machine

    TransferMapState stateFrom = trajectory.stateWithIndex(indFrom);
    PhaseMatrix matFrom = stateFrom.getTransferMap().getFirstOrder();

    return matTo.times(matFrom.inverse());
  }
  /**
   * Convenience method for computing the transfer map between two state locations, say
   * <i>S</i><sub>1</sub> and <i>S</i><sub>2</sub>. Let <i>s</i><sub>0</sub> be the axis location of
   * the beamline entrance, <i>s</i><sub>1</sub> the location of state <i>S</i><sub>1</sub>, and
   * <i>s</i><sub>2</sub> the location of state <i>S</i><sub>2</sub>. Each state object
   * <i>S<sub>n</sub></i> contains the transfer map
   * <b>T</b>(<i>s<sub>n</sub></i>,<i>s</i><sub>0</sub>) which takes phases coordinates at the
   * beamline entrance to the position of state <i>S<sub>n</sub></i>. The transfer map
   * <b>T</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>1</sub>) taking phase coordinates
   * <b>z</b><sub>1</sub> (and covariance matrix <b>&sigma;</b><sub>1</sub>) from position
   * <i>s</i><sub>1</sub> to position <i>s</i><sub>2</sub> is then given by <br>
   * <br>
   * &nbsp; &nbsp; <b>T</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>1</sub>) =
   * <b>T</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>0</sub>) &#x2218;
   * <b>T</b>(<i>s</i><sub>1</sub>,<i>s</i><sub>0</sub>)<sup>-1</sup> , <br>
   * <br>
   * where <b>T</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>0</sub>) is the transfer map between the
   * beamline entrance <i>s</i><sub>0</sub> and the position <i>s</i><sub>2</sub> of state
   * <i>S</i><sub>2</sub>, and <b>T</b>(<i>s</i><sub>1</sub>,<i>s</i><sub>0</sub>) is the transfer
   * map between the beamline entrance <i>s</i><sub>0</sub> and the position <i>s</i><sub>1</sub> of
   * state <i>S</i><sub>1</sub>.
   *
   * @param state1 trajectory state <i>S</i><sub>1</sub> of starting location <i>s</i><sub>1</sub>
   * @param state2 trajectory state <i>S</i><sub>2</sub> of final location <i>s</i><sub>2</sub>
   * @return transfer map <b>T</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>1</sub>) between locations
   *     <i>s</i><sub>1</sub> and <i>s</i><sub>2</sub>
   * @author Christopher K. Allen
   * @since Nov 4, 2014
   */
  public static PhaseMap computeTransferMap(TransferMapState state1, TransferMapState state2) {
    PhaseMap mapPhi1 = state1.getTransferMap();
    PhaseMap mapPhi2 = state2.getTransferMap();

    PhaseMap mapPhi1inv = mapPhi1.inverse();
    PhaseMap mapPhi21 = mapPhi2.compose(mapPhi1inv);

    return mapPhi21;
  }
  /**
   * Convenience method for computing the transfer matrix between two state locations, say
   * <i>S</i><sub>1</sub> and <i>S</i><sub>2</sub>. Let <i>s</i><sub>0</sub> be the axis location of
   * the beamline entrance, <i>s</i><sub>1</sub> the location of state <i>S</i><sub>1</sub>, and
   * <i>s</i><sub>2</sub> the location of state <i>S</i><sub>2</sub>. Each state object
   * <i>S<sub>n</sub></i> contains the transfer matrix
   * <b>&Phi;</b>(<i>s<sub>n</sub></i>,<i>s</i><sub>0</sub>) which takes phases coordinates at the
   * beamline entrance to the position of state <i>S<sub>n</sub></i>. The transfer matrix
   * <b>&Phi;</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>1</sub>) taking phase coordinates
   * <b>z</b><sub>1</sub> (and covariance matrix <b>&sigma;</b><sub>1</sub>) from position
   * <i>s</i><sub>1</sub> to position <i>s</i><sub>2</sub> is then given by <br>
   * <br>
   * &nbsp; &nbsp; <b>&Phi;</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>1</sub>) =
   * <b>&Phi;</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>0</sub>)
   * <b>&Phi;</b>(<i>s</i><sub>1</sub>,<i>s</i><sub>0</sub>)<sup>-1</sup> , <br>
   * <br>
   * where <b>&Phi;</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>0</sub>) is the transfer matrix between
   * the beamline entrance <i>s</i><sub>0</sub> and the position <i>s</i><sub>2</sub> of state
   * <i>S</i><sub>2</sub>, and <b>&Phi;</b>(<i>s</i><sub>1</sub>,<i>s</i><sub>0</sub>) is the
   * transfer matrix between the beamline entrance <i>s</i><sub>0</sub> and the position
   * <i>s</i><sub>1</sub> of state <i>S</i><sub>1</sub>.
   *
   * @param state1 trajectory state <i>S</i><sub>1</sub> of starting location <i>s</i><sub>1</sub>
   * @param state2 trajectory state <i>S</i><sub>2</sub> of final location <i>s</i><sub>2</sub>
   * @return transfer matrix <b>&Phi;</b>(<i>s</i><sub>2</sub>,<i>s</i><sub>1</sub>) between
   *     locations <i>s</i><sub>1</sub> and <i>s</i><sub>2</sub>
   * @author Christopher K. Allen
   * @since Jun 23, 2014
   */
  public static PhaseMatrix computeTransferMatrix(
      TransferMapState state1, TransferMapState state2) {
    PhaseMatrix matPhi1 = state1.getTransferMap().getFirstOrder();
    PhaseMatrix matPhi2 = state2.getTransferMap().getFirstOrder();

    PhaseMatrix matPhi1inv = matPhi1.inverse();
    PhaseMatrix matPhi21 = matPhi2.times(matPhi1inv);

    return matPhi21;
  }
  /**
   * This is the phase advance for the given state location.
   *
   * <p>Compute and return the particle phase advance from the trajectory beginning to the given
   * state location.
   *
   * <p>Internally the method calculates the phase advances using the initial and final
   * Courant-Snyder &alpha; and &beta; values for the matched beam at the trajectory beginning and
   * the location of the given state, respectively. These Courant-Snyder parameters are computed as
   * the matched beam envelope at the trajectory beginning and the given state location.
   *
   * <p>Let <b>&Phi;</b> represent the transfer matrix from the initial ring location to the given
   * state location. The computed quantity is the general phase advance of the particle through the
   * transfer matrix <b>&Phi;</b>, and no special requirements are placed upon <b>&Phi;</b> (e.g.,
   * periodicity). One phase advance is provided for each phase plane, i.e.,
   * (&sigma;<sub><i>x</i></sub>, &sigma;<sub><i>z</i></sub>, &sigma;<sub><i>z</i></sub>).
   *
   * <p>The definition of phase advance &sigma; is given by <br>
   * <br>
   * &nbsp; &nbsp; &sigma;(<i>s</i>) &equiv; &int;<sup><i>s</i></sup> [1/&beta;(<i>t</i>)]<i>dt</i>
   * , <br>
   * <br>
   * where &beta;(<i>s</i>) is the Courant-Snyder, envelope function, and the integral is taken
   * along the interval between the initial and final Courant-Snyder parameters.
   *
   * <p>The basic relation used to compute &sigma; is the following: <br>
   * <br>
   * &nbsp; &nbsp; &sigma; = sin<sup>-1</sup>
   * &phi;<sub>12</sub>/(&beta;<sub>1</sub>&beta;<sub>2</sub>)<sup>&frac12;</sup> , <br>
   * <br>
   * where &phi;<sub>12</sub> is the element of <b>&Phi;</b> in the upper right corner of each
   * 2&times;2 diagonal block, &beta;<sub>1</sub> is the initial beta function value (provided) and
   * &beta;<sub>2</sub> is the final beta function value (provided).
   *
   * @param state state containing transfer map and location used in these calculations
   * @see calculatePhaseAdvance(PhaseMatrix, Twiss[], Twiss[])
   * @author Christopher K. Allen
   * @since Aug 14, 2013
   */
  @Override
  public R3 computeBetatronPhase(TransferMapState state) {
    PhaseMatrix matFullTrn = this.calculateFullLatticeMatrixAt(state);
    Twiss[] arrTwsLoc = super.calculateMatchedTwiss(matFullTrn);

    PhaseMatrix matPhiLoc = state.getTransferMap().getFirstOrder();
    R3 vecPhsAdv = super.calculatePhaseAdvance(matPhiLoc, this.arrTwsMch, arrTwsLoc);

    return vecPhsAdv;
  }
  /**
   * Calculates the fixed point (closed orbit) in transverse phase space at the given state location
   * in the presence of dispersion.
   *
   * <p>Let the full-turn map a the state location be denoted <b>&Phi;</b>. The transverse plane
   * dispersion vector <b>&Delta;</b> is defined <br>
   * <br>
   * &nbsp; &nbsp; <b>&Delta;</b><sub><i>t</i></sub> &equiv;
   * -(1/&gamma;<sup>2</sup>)[d<i>x</i>/d<i>z'</i>, d<i>x'</i>/d<i>z'</i>, d<i>y</i>/d<i>z'</i>,
   * d<i>y'</i>/d<i>z'</i>]<sup><i>T</i></sup> . <br>
   * <br>
   * It can be identified as the first 4 entries of the 6<sup><i>th</i></sup> column in the transfer
   * matrix <b>&Phi;</b>. The above vector quantifies the change in the transverse particle phase
   * coordinate position versus the change in particle momentum. The factor -(1/&gamma;<sup>2</sup>)
   * is needed to convert from longitudinal divergence angle <i>z'</i> used by XAL to momentum
   * &delta;<i>p</i> &equiv; &Delta;<i>p</i>/<i>p</i> used in the dispersion definition.
   * Specifically, <br>
   * <br>
   * &nbsp; &nbsp; &delta;<i>p</i> &equiv; &Delta;<i>p</i>/<i>p</i> = &gamma;<sup>2</sup><i>z</i>'
   * <br>
   * <br>
   * As such, the above vector can be better described <br>
   * <br>
   * &nbsp; &nbsp; <b>&Delta;</b><sub><i>t</i></sub> &equiv; [&Delta;<i>x</i>/&delta;<i>p</i>,
   * &Delta;<i>x'</i>/&delta;<i>p</i>, &Delta;<i>y</i>/&delta;<i>p</i>,
   * &Delta;<i>y'</i>/&delta;<i>p</i>]<sup><i>T</i></sup> <br>
   * <br>
   * explicitly describing the change in transverse phase coordinate for fractional change in
   * momentum &delta;<i>p</i>.
   *
   * <p>Since we are only concerned with transverse phase space coordinates, we restrict ourselves
   * to the 4&times;4 upper diagonal block of <b>&Phi;</b>, which we denote take <b>T</b>. That is,
   * <b>T</b> = &pi; &sdot; <b>&Phi;</b> where &pi; : <b>R</b><sup>6&times;6</sup> &rarr;
   * <b>R</b><sup>4&times;4</sup> is the projection operator.
   *
   * <p>This method finds that point <b>z</b><sub><i>t</i></sub> &equiv; (<i>x<sub>t</sub></i>,
   * <i>x'<sub>t</sub></i>, <i>y<sub>t</sub></i>, <i>y'<sub>t</sub></i>) in transvse phase space
   * that is invariant under the action of the ring for a given momentum spread &delta;<i>p</i>.
   * That is, the particle ends up in the same location each revolution. With a finite momentum
   * spread of &delta;<i>p</i> &gt; 0 we require this require that <br>
   * <br>
   * &nbsp; &nbsp; <b>Tz</b><sub><i>t</i></sub> + &delta;<i>p</i><b>&Delta;</b><sub><i>t</i></sub> =
   * <b>z</b><sub><i>t</i></sub> , <br>
   * <br>
   * which can be written <br>
   * <br>
   * &nbsp; <b>z</b><sub><i>t</i></sub> = &delta;<i>p</i>(<b>T</b> -
   * <b>I</b>)<sup>-1</sup><b>&Delta;</b><sub><i>t</i></sub> , <br>
   * <br>
   * where <b>I</b> is the identity matrix. Dividing both sides by &delta;<i>p</i> yields the final
   * result <br>
   * <br>
   * &nbsp; <b>z</b><sub>0</sub> &equiv; <b>z</b><sub><i>t</i></sub>/&delta;<i>p</i> = (<b>T</b> -
   * <b>I</b>)<sup>-1</sup><b>&Delta;</b><sub><i>t</i></sub> , <br>
   * <br>
   * which is the returned value of this method. It is normalized by &delta;<i>p</i> so that we can
   * compute the closed orbit for any given momentum spread.
   *
   * @param state we are calculating the dispersion at this state location
   * @return The closed orbit fixed point <b>z</b><sub>0</sub> for finite dispersion, normalized by
   *     momentum spread. Returned as an array
   *     [<i>x</i><sub>0</sub>,<i>x'</i><sub>0</sub>,<i>y</i><sub>0</sub>,<i>y'</i><sub>0</sub>]/&delta;<i>p</i>
   * @see xal.tools.beam.calc.ISimEnvResults#computeChromDispersion(xal.model.probe.traj.ProbeState)
   * @author Christopher K. Allen
   * @since Nov 8, 2013
   */
  @Override
  public PhaseVector computeChromDispersion(TransferMapState state) {
    PhaseMatrix matFullTn = this.calculateFullLatticeMatrixAt(state);
    double dblGamma = state.getGamma();

    //        double[]    arrDisp   = super.calculateDispersion(matFullTn, dblGamma);
    //        R4          vecDispR4 = new R4(arrDisp);
    R4 vecDispR4 = super.calculateDispersion(matFullTn, dblGamma);
    PhaseVector vecDisp = PhaseVector.embed(vecDispR4);

    return vecDisp;
  }
  /**
   * Computes the chromatic aberration for one pass around the ring starting at the given state
   * location, or from the entrance to state position for a linear machine. The returned vector is
   * the displacement from the closed orbit caused by a unit momentum offset (&delta;<i>p</i> = 1).
   * See the documentation in {@link ISimLocResults#computeChromAberration(ProbeState)} for a more
   * detailed exposition.
   *
   * @see xal.tools.beam.calc.ISimLocResults#computeChromAberration(xal.model.probe.traj.ProbeState)
   * @author Christopher K. Allen
   * @since Nov 15, 2013
   */
  @Override
  public PhaseVector computeChromAberration(TransferMapState state) {
    double dblGamma = state.getGamma();
    //        PhaseMap        mapPhi   = state.getTransferMap();
    //        PhaseMap        mapPhi   = state.getStateTransferMap();
    //      PhaseMatrix     matPhi   = mapPhi.getFirstOrder();
    PhaseMatrix matPhi = this.calculateFullLatticeMatrixAt(state);

    R6 vecDel = super.calculateAberration(matPhi, dblGamma);

    return PhaseVector.embed(vecDel);
  }
  /**
   * Calculates and returns the full lattice matrix for the machine at the given state location. Let
   * <i>S<sub>n</sub></i> be the given state object at location <i>s<sub>n</sub></i>, and let
   * <b>T</b><sub><i>n</i></sub> be the transfer matrix between locations <i>s</i><sub>0</sub> and
   * <i>s<sub>n</sub></i> , where <i>s</i><sub>0</sub> is the location of the full transfer matrix
   * <b>&Phi;</b><sub>0</sub> for this machine (end to end). Then the full turn matrix
   * <b>&Phi;</b><sub><i>n</i></sub> for the machine at location <i>s<sub>n</sub></i> is given by
   * <br>
   * <br>
   * &nbsp; &nbsp; <b>&Phi;</b><sub><i>n</i></sub> = <b>T</b><sub><i>n</i></sub> &sdot;
   * <b>&Phi;</b><sub>0</sub> &sdot; <b>T</b><sub><i>n</i></sub><sup>-1</sup> . <br>
   * <br>
   * That is, we conjugate the full transfer map for this machine by the transfer map for the given
   * state.
   *
   * @param state state object <i>S<sub>n</sub></i> for location <i>s<sub>n</sub></i> containing
   *     transfer matrix <b>T</b><sub><i>n</i></sub>
   * @return the full-turn map <b>&Phi;</b><sub><i>n</i></sub> at the location <i>s<sub>n</sub></i>
   *     of the given state
   * @author Christopher K. Allen
   * @since Oct 28, 2013
   */
  protected PhaseMatrix calculateFullLatticeMatrixAt(TransferMapState state) {
    PhaseMap mapPhiState = state.getTransferMap();
    PhaseMatrix matPhiState = mapPhiState.getFirstOrder();
    PhaseMatrix matPhiStInv = matPhiState.inverse();

    PhaseMatrix matPhiFull = this.mapPhiFull.getFirstOrder();
    PhaseMatrix matFullTnLoc;

    matFullTnLoc = matPhiFull.times(matPhiStInv);
    matFullTnLoc = matPhiState.times(matFullTnLoc);

    return matFullTnLoc;
  }