code: /*******************************************************************************
   * Copyright (c) 2017 Attila Kovacs <attila[AT]sigmyne.com>.
   * All rights reserved. 
   * 
   * This file is part of crush.
   * 
   *     crush is free software: you can redistribute it and/or modify
   *     it under the terms of the GNU General Public License as published by
   *     the Free Software Foundation, either version 3 of the License, or
   *     (at your option) any later version.
   * 
   *     crush is distributed in the hope that it will be useful,
   *     but WITHOUT ANY WARRANTY; without even the implied warranty of
   *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   *     GNU General Public License for more details.
   * 
   *     You should have received a copy of the GNU General Public License
   *     along with crush.  If not, see <http://www.gnu.org/licenses/>.
   * 
   * Contributors:
   *     Attila Kovacs <attila[AT]sigmyne.com> - initial API and implementation
   ******************************************************************************/
  
  package crush.telescope.sofia;
  
  import java.util.ArrayList;
  
  import crush.CRUSH;
  import jnum.Unit;
  import jnum.Util;
  import jnum.astro.CoordinateEpoch;
  import jnum.astro.EquatorialCoordinates;
  import jnum.math.Range;
  import jnum.math.Vector2D;
  
  
  public class GyroDrifts extends ArrayList<GyroDrifts.Datum> {
      
      
      /**
       * 
       */
      private static final long serialVersionUID = -3751194338137621151L;
  
      private SofiaScan<?> scan;
  
      public GyroDrifts(SofiaScan<?> scan) {
          this.scan = scan;
      } 
      
      protected void validate() {
          double fromUTC = scan.getFirstIntegration().getFirstFrame().utc;
          for(int i=0; i<size(); i++) {
              Datum drift = get(i);
              drift.utcRange.setMin(fromUTC);
              fromUTC = drift.nextUTC;
          }    
      }
      
      public void correct(SofiaIntegration<?> integration) {
          if(isEmpty()) {
              integration.warning("Skipping gyro drift correction. No data...");
              return;
          }
          
          if(integration.hasOption("gyrocorrect.max")) {
              double limit = integration.option("gyrocorrect.max").getDouble() * Unit.arcsec;
              if(getMax() > limit) {
                  integration.warning("Skipping gyro drift correction. Drifts are too large...");
                  return;
              }
          }
          
          integration.info("Correcting for gyro drifts.");
                  
          validate();
          
          int k = 0;
          Datum drift = get(0);
          boolean isExtrapolated = false;
          
          for(int i=0; i<integration.size(); i++) {
              SofiaFrame frame = integration.get(i);
              if(frame == null) continue;
                      
              if(!isExtrapolated) while(!drift.utcRange.contains(frame.utc)) {
                  drift = ++k < size() ? get(k) : null;
                  if(drift == null) break;
              }
           
              if(drift == null) {
                  integration.warning("Extrapolated drift correction after frame " + i);
                  drift = get(k-1);
                  isExtrapolated = true;
              }
              
              Vector2D offset = drift.delta.copy();
              
              double x = (frame.utc - drift.utcRange.min()) / drift.utcRange.span();
              offset.scale(x);
              
              frame.equatorial.addOffset(offset);
              
              frame.equatorialToHorizontal(offset);
              frame.horizontalOffset.add(offset);
              frame.horizontal.addOffset(offset);
          }
      }
      
      public double getMax() {
          if(isEmpty()) return Double.NaN;
          double max = 0.0;
          for(Datum drift : this) if(drift.delta.length() > max) max = drift.delta.length();
          return max;
      }
      
      public double getRMS() {
          if(isEmpty()) return Double.NaN;
          double sum = 0.0;
          for(Datum drift : this) sum += drift.delta.absSquared();
          return Math.sqrt(sum / size());
      }
      
      
      
      public void parse(SofiaHeader header) {
          for(int i=0; ; i++){
              try { if(!add(header, i)) return; }
              catch(IllegalStateException e) {}            
          }
      } 
   
      protected boolean add(SofiaHeader header, int index) throws IllegalStateException {
          Datum drift = new Datum();
          
          if(drift.parse(header, index)) {
              add(drift);
              CRUSH.detail(scan, " drift " + index + ": " 
                      + Util.f1.format(drift.delta.length() / Unit.arcsec) + " arcsec.");
          }
          else return false;
          
          return true;
      }
      
      protected class Datum implements Comparable<Datum> {
          int index;
          public Range utcRange = new Range();
          public double nextUTC;
          public Vector2D delta;
          
          public boolean parse(SofiaHeader header, int index) throws IllegalStateException {
              this.index = index;
              
              if(!header.containsKey("DBRA" + index)) return false;
              if(!header.containsKey("DBDEC" + index)) return false;
              if(!header.containsKey("DARA" + index)) return false;
              if(!header.containsKey("DADEC" + index)) return false;
              
              CoordinateEpoch epoch = CoordinateEpoch.J2000;
              
              String spec = header.getString("EQUINOX", null);
              if(spec != null) if(SofiaHeader.isValid(spec)) epoch = CoordinateEpoch.forString(spec);
              
              EquatorialCoordinates before = new EquatorialCoordinates(
                      header.getHMSTime("DBRA" + index) * Unit.timeAngle,
                      header.getDMSAngle("DBDEC" + index) * Unit.deg,
                      epoch
              );
                
              EquatorialCoordinates after = new EquatorialCoordinates(
                      header.getHMSTime("DARA" + index) * Unit.timeAngle,
                      header.getDMSAngle("DADEC" + index) * Unit.deg,
                      epoch
              );
                      
              delta = after.getOffsetFrom(before);
              if(Double.isNaN(delta.length())) throw new IllegalStateException("NaN drift value.");   
              
              utcRange.setMax(header.getDouble("DBTIME" + index));
              nextUTC = header.getDouble("DATIME" + index);
  
              
              return true;
          }
  
  
          @Override
          public int compareTo(Datum o) {
              return Double.compare(utcRange.max(), o.utcRange.max());
          }
          
      }
      
  
  }
  