/*
    * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package com.irurueta.navigation.inertial.calibration;
  
  import com.irurueta.navigation.inertial.BodyKinematics;
  import com.irurueta.units.Time;
  import com.irurueta.units.TimeConverter;
  import com.irurueta.units.TimeUnit;
  
  import java.io.Serial;
  import java.io.Serializable;
  import java.util.Objects;
  
  /**
   * Contains a body kinematics measurement (accelerometer + gyroscope) along with the
   * corresponding timestamp when measure was made.
   * Notice that timestamp does not need to be absolute.
   * Usually timestamps are used in sequences of measurements of body kinematics, where
   * the first measurement can have any timestamp value (e.g. zero), and hence the subsequent
   * measurements will have timestamps relative to the first one.
   */
  public class TimedBodyKinematics implements Serializable, Cloneable {
  
      /**
       * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
       * instances.
       */
      @Serial
      private static final long serialVersionUID = 0L;
  
      /**
       * Current body kinematics measurement. Contains accelerometer and gyroscope measurements.
       */
      private BodyKinematics kinematics;
  
      /**
       * Timestamp value expressed in seconds.
       */
      private double timestampSeconds;
  
      /**
       * Constructor.
       */
      public TimedBodyKinematics() {
      }
  
      /**
       * Constructor.
       *
       * @param kinematics current body kinematics measurement.
       */
      public TimedBodyKinematics(final BodyKinematics kinematics) {
          this.kinematics = kinematics;
      }
  
      /**
       * Constructor.
       *
       * @param timestampSeconds timestamp value expressed in seconds.
       */
      public TimedBodyKinematics(final double timestampSeconds) {
          this.timestampSeconds = timestampSeconds;
      }
  
      /**
       * Constructor.
       *
       * @param timestamp timestamp value.
       */
      public TimedBodyKinematics(final Time timestamp) {
          timestampSeconds = convertTime(timestamp);
      }
  
      /**
       * Constructor.
       *
       * @param kinematics       current body kinematics measurement.
       * @param timestampSeconds timestamp value expressed in seconds.
       */
      public TimedBodyKinematics(final BodyKinematics kinematics, final double timestampSeconds) {
          this(kinematics);
          this.timestampSeconds = timestampSeconds;
      }
  
      /**
       * Constructor.
      *
      * @param kinematics current body kinematics measurement.
      * @param timestamp  timestamp value.
      */
     public TimedBodyKinematics(final BodyKinematics kinematics, final Time timestamp) {
         this(timestamp);
         this.kinematics = kinematics;
     }
 
     /**
      * Constructor.
      *
      * @param input instance to copy data from.
      */
     public TimedBodyKinematics(final TimedBodyKinematics input) {
         copyFrom(input);
     }
 
     /**
      * Gets current body kinematics measurement. Contains accelerometer and gyroscope
      * measurements.
      *
      * @return current body kinematics measurement.
      */
     public BodyKinematics getKinematics() {
         return kinematics;
     }
 
     /**
      * Sets current body kinematics measurement. Contains accelerometer and gyroscope
      * measurements.
      *
      * @param kinematics current body kinematics measurement to be set.
      */
     public void setKinematics(final BodyKinematics kinematics) {
         this.kinematics = kinematics;
     }
 
     /**
      * Gets timestamp value expressed in seconds.
      *
      * @return timestamp value expressed in seconds.
      */
     public double getTimestampSeconds() {
         return timestampSeconds;
     }
 
     /**
      * Sets timestamp value expressed in seconds.
      *
      * @param timestampSeconds timestamp value expressed in seconds.
      */
     public void setTimestampSeconds(final double timestampSeconds) {
         this.timestampSeconds = timestampSeconds;
     }
 
     /**
      * Gets timestamp value.
      *
      * @return a new timestamp instance.
      */
     public Time getTimestamp() {
         return new Time(timestampSeconds, TimeUnit.SECOND);
     }
 
     /**
      * Gets timestamp value.
      *
      * @param result instance where result data will be stored.
      */
     public void getTimestamp(final Time result) {
         result.setValue(timestampSeconds);
         result.setUnit(TimeUnit.SECOND);
     }
 
     /**
      * Sets timestamp.
      *
      * @param timestamp timestamp to be set.
      */
     public void setTimestamp(final Time timestamp) {
         timestampSeconds = convertTime(timestamp);
     }
 
     /**
      * Copies data of provided instance into this instance.
      *
      * @param input instance to copy data from.
      */
     public void copyFrom(final TimedBodyKinematics input) {
         if (input.kinematics != null) {
             if (kinematics == null) {
                 kinematics = new BodyKinematics(input.kinematics);
             } else {
                 kinematics.copyFrom(input.kinematics);
             }
         } else {
             kinematics = null;
         }
 
         timestampSeconds = input.timestampSeconds;
     }
 
     /**
      * Copies this instance data into provided instance.
      *
      * @param output destination instance where data will be copied to.
      */
     public void copyTo(final TimedBodyKinematics output) {
         output.copyFrom(this);
     }
 
     /**
      * Computes and returns hash code for this instance. Hash codes are almost unique
      * values that are useful for fast classification and storage of objects in collections.
      *
      * @return Hash code.
      */
     @Override
     public int hashCode() {
         return Objects.hash(kinematics, timestampSeconds);
     }
 
     /**
      * Checks if provided instance has exactly the same contents as this instance.
      *
      * @param other instance to be compared.
      * @return true if both instances are considered to be equal, false otherwise.
      */
     public boolean equals(final TimedBodyKinematics other) {
         return equals(other, 0.0);
     }
 
     /**
      * Checks if provided instance has contents similar to this instance up to provided
      * threshold value.
      *
      * @param other     instance to be compared.
      * @param threshold maximum allowed difference between kinematics and timestamp values.
      * @return true if both instances are considered to be equal (up to provided
      * threshold), false otherwise.
      */
     public boolean equals(final TimedBodyKinematics other, final double threshold) {
         if (other == null) {
             return false;
         }
 
         return ((other.kinematics == null && kinematics == null)
                 || (kinematics != null && kinematics.equals(other.kinematics, threshold)))
                 && Math.abs(other.timestampSeconds - timestampSeconds) <= threshold;
     }
 
     /**
      * Checks if provided object is a TimedBodyKinematics instance having exactly the same
      * contents as this instance.
      *
      * @param obj object to be compared.
      * @return true if both objects are considered to be equal, false otherwise.
      */
     @Override
     public boolean equals(final Object obj) {
         if (this == obj) {
             return true;
         }
         if (obj == null || getClass() != obj.getClass()) {
             return false;
         }
         final var other = (TimedBodyKinematics) obj;
         return equals(other);
     }
 
     /**
      * Makes a copy of this instance.
      *
      * @return a copy of this instance.
      * @throws CloneNotSupportedException if clone fails for some reason.
      */
     @Override
     protected Object clone() throws CloneNotSupportedException {
         final var result = (TimedBodyKinematics) super.clone();
         copyTo(result);
         return result;
     }
 
     /**
      * Converts provided time instance to seconds.
      *
      * @param time timestamp to be converted.
      * @return converted value expressed in seconds.
      */
     private static double convertTime(final Time time) {
         return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
     }
 }