/**
   * 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 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;
  }