Merge pull request #1831 from jluhrs/feature/SEQNG-1268

SEQNG-1268 Fixed comparison of current and demanded offset.

modules/server/src/main/scala/seqexec/server/tcs/TcsController.scala

@@ -232,10 +232,6 @@ object TcsController {
       tag[OffsetQ](((x * -1 * iaa.sin) - y * iaa.cos) * FOCAL_PLANE_SCALE)
     )
 
-    def ~=(other: FocalPlaneOffset): Boolean =
-      math.pow((x - other.x).toMillimeters, 2) + math.pow((y - other.y).toMillimeters,
-                                                          2
-      ) <= FocalPlaneOffset.ToleranceSquared
   }
 
   object FocalPlaneOffset {
@@ -244,7 +240,6 @@ object TcsController {
     def fromInstrumentOffset(o: InstrumentOffset, iaa: Angle): FocalPlaneOffset =
       o.toFocalPlaneOffset(iaa)
 
-    val ToleranceSquared: Double = 1e12
   }
 
   trait OffsetP

modules/server/src/main/scala/seqexec/server/tcs/TcsControllerEpicsCommon.scala

@@ -28,6 +28,8 @@ import seqexec.server.EpicsCodex.encode
 import seqexec.server.EpicsCommand
 import seqexec.server.SeqexecFailure
 import seqexec.server.tcs.TcsController._
+import squants.Length
+import squants.space.LengthConversions._
 import squants.time.TimeConversions._
 
 /**
@@ -251,14 +253,14 @@ object TcsControllerEpicsCommon {
       .map(_.withDebug(s"$name($current =!= $demand"))
 
   def applyParam[F[_]: Applicative, T, C](
-    used:    Boolean,
-    current: T,
-    demand:  T,
-    act:     T => F[Unit],
-    lens:    Lens[C, T],
-    comp:    (T, T) => Boolean
-  )(name:    String): Option[WithDebug[C => F[C]]] =
-    (used && comp(current, demand))
+    used:     Boolean,
+    current:  T,
+    demand:   T,
+    act:      T => F[Unit],
+    lens:     Lens[C, T],
+    equalish: (T, T) => Boolean
+  )(name:     String): Option[WithDebug[C => F[C]]] =
+    (used && !equalish(current, demand))
       .option((c: C) => act(demand) *> lens.set(demand)(c).pure[F])
       .map(_.withDebug(s"$name($current =!= $demand"))
 
@@ -625,15 +627,16 @@ object TcsControllerEpicsCommon {
           o.toFocalPlaneOffset((l ^|-> BaseEpicsTcsConfig.iaa).get(current)),
           setTelescopeOffset,
           l ^|-> BaseEpicsTcsConfig.offset,
-          (u: FocalPlaneOffset, v: FocalPlaneOffset) => u ~= v
+          offsetNear
         )("Offset")
       ),
       tc.wavelA.flatMap(
         applyParam(subsystems.contains(Subsystem.Mount),
                    (l ^|-> BaseEpicsTcsConfig.wavelA).get(current),
                    _,
                    setWavelength,
-                   l ^|-> BaseEpicsTcsConfig.wavelA
+                   l ^|-> BaseEpicsTcsConfig.wavelA,
+                   wavelengthNear
         )("Wavelenght")
       )
     ).flattenOption
@@ -789,4 +792,17 @@ object TcsControllerEpicsCommon {
   val DefaultTimeout: FiniteDuration = FiniteDuration(10, SECONDS)
   val ConfigTimeout: FiniteDuration  = FiniteDuration(60, SECONDS)
 
+  val OffsetTolerance: Length = 1e-6.millimeters
+
+  def offsetNear(offset: FocalPlaneOffset, other: FocalPlaneOffset): Boolean =
+    math.pow((offset.x - other.x).toMillimeters, 2) + math.pow((offset.y - other.y).toMillimeters,
+                                                               2
+    ) <= math.pow(OffsetTolerance.toMillimeters, 2)
+
+  // Wavelength status gives value as Angstroms, with no decimals
+  val WavelengthTolerance: Length = 0.5.angstroms
+
+  def wavelengthNear(wavel: Wavelength, other: Wavelength): Boolean =
+    math.abs(wavel.length.toAngstroms - other.length.toAngstroms) <= WavelengthTolerance.toAngstroms
+
 }

modules/server/src/test/scala/seqexec/server/tcs/TcsArbitraries.scala

@@ -11,6 +11,7 @@ import org.scalacheck.Gen
 import shapeless.tag
 import shapeless.tag.@@
 import squants.Angle
+import squants.space.AngleConversions._
 import squants.space.Degrees
 
 trait TcsArbitraries {
@@ -28,16 +29,20 @@ trait TcsArbitraries {
   implicit val tcsNodChopCogen: Cogen[TcsController.NodChop]   =
     Cogen[(TcsController.Beam, TcsController.Beam)].contramap(x => (x.nod, x.chop))
 
-  private val angleMMGen                                             = Gen.posNum[Double].map(Degrees(_))
+  private def rangedAngleGen(minVal: Angle, maxVal: Angle) =
+    Gen.choose(minVal.toDegrees, maxVal.toDegrees).map(Degrees(_))
+
+  private val offsetLimit: Angle = 120.arcseconds
+
   implicit val offsetPArb: Arbitrary[Angle @@ TcsController.OffsetP] = Arbitrary(
-    angleMMGen.map(tag[TcsController.OffsetP].apply)
+    rangedAngleGen(-offsetLimit, offsetLimit).map(tag[TcsController.OffsetP].apply)
   )
   implicit val offsetPCogen: Cogen[Angle @@ TcsController.OffsetP]   =
     Cogen[Double].contramap(_.value)
-  implicit val offsetYArb: Arbitrary[Angle @@ TcsController.OffsetQ] = Arbitrary(
-    angleMMGen.map(tag[TcsController.OffsetQ].apply)
+  implicit val offsetQArb: Arbitrary[Angle @@ TcsController.OffsetQ] = Arbitrary(
+    rangedAngleGen(-offsetLimit, offsetLimit).map(tag[TcsController.OffsetQ].apply)
   )
-  implicit val offsetYCogen: Cogen[Angle @@ TcsController.OffsetQ]   =
+  implicit val offsetQCogen: Cogen[Angle @@ TcsController.OffsetQ]   =
     Cogen[Double].contramap(_.value)
   implicit val fpoArb: Arbitrary[TcsController.InstrumentOffset]     = Arbitrary {
     for {
@@ -50,6 +55,9 @@ trait TcsArbitraries {
       (x.p, x.q)
     )
 
+  implicit val iaaArb: Arbitrary[Angle] = Arbitrary(rangedAngleGen(-90.degrees, 270.degrees))
+  implicit val iaaCogen: Cogen[Angle]   = Cogen[Double].contramap(_.toDegrees)
+
   implicit val crFollowArb: Arbitrary[CRFollow] = Arbitrary {
     Gen.oneOf(CRFollow.On, CRFollow.Off)
   }

modules/server/src/test/scala/seqexec/server/tcs/TcsControllerEpicsCommonSpec.scala

@@ -42,6 +42,7 @@ import seqexec.server.tcs.TcsController.{
 import shapeless.tag
 import squants.space.{ Arcseconds, Length, Microns, Millimeters }
 import org.scalatest.flatspec.AnyFlatSpec
+import seqexec.server.keywords.USLocale
 import seqexec.server.tcs.TestTcsEpics.{ ProbeGuideConfigVals, TestTcsEvent }
 import squants.space.AngleConversions._
 import squants.space.LengthConversions._
@@ -809,6 +810,161 @@ class TcsControllerEpicsCommonSpec extends AnyFlatSpec with PrivateMethodTester
 
   }
 
+  // This function simulates the loss of precision in the EPICS record
+  def epicsTransform(prec: Int)(v: Double): Double =
+    s"%.${prec}f".formatLocal(USLocale, v).toDouble
+
+  it should "apply an offset if it is not at the right position" in {
+
+    val offsetDemand  = InstrumentOffset(tag[OffsetP](10.arcseconds), tag[OffsetQ](-5.arcseconds))
+    val offsetCurrent =
+      InstrumentOffset(tag[OffsetP](10.00001.arcseconds), tag[OffsetQ](-5.arcseconds))
+    val iaa           = 33.degrees
+    val wavelength    = Wavelength(440.nanometers)
+    val recordPrec    = 14
+
+    val dumbEpics = buildTcsController[IO](
+      TestTcsEpics.defaultState.copy(
+        xoffsetPoA1 = tag[OffsetX](
+          epicsTransform(recordPrec)(offsetCurrent.toFocalPlaneOffset(iaa).x.toMillimeters)
+        ),
+        yoffsetPoA1 = tag[OffsetX](
+          epicsTransform(recordPrec)(offsetCurrent.toFocalPlaneOffset(iaa).y.toMillimeters)
+        ),
+        instrAA = epicsTransform(recordPrec)(iaa.toDegrees),
+        sourceAWavelength = epicsTransform(recordPrec)(wavelength.length.toAngstroms)
+      )
+    )
+
+    val config = baseConfig.copy(
+      tc = TelescopeConfig(offsetDemand.some, wavelength.some)
+    )
+
+    val genOut: IO[List[TestTcsEpics.TestTcsEvent]] = for {
+      d <- dumbEpics
+      c  = TcsControllerEpicsCommon(d)
+      _ <- c.applyBasicConfig(TcsController.Subsystem.allButGaos, config)
+      r <- d.outputF
+    } yield r
+
+    val result = genOut.unsafeRunSync()
+
+    assert(
+      result.exists {
+        case TestTcsEvent.OffsetACmd(_, _) => true
+        case _                             => false
+      }
+    )
+
+  }
+
+  it should "not reapply an offset if it is already at the right position" in {
+
+    val offset     = InstrumentOffset(tag[OffsetP](10.arcseconds), tag[OffsetQ](-5.arcseconds))
+    val iaa        = 33.degrees
+    val wavelength = Wavelength(440.nanometers)
+    val recordPrec = 14
+
+    val dumbEpics = buildTcsController[IO](
+      TestTcsEpics.defaultState.copy(
+        xoffsetPoA1 =
+          tag[OffsetX](epicsTransform(recordPrec)(offset.toFocalPlaneOffset(iaa).x.toMillimeters)),
+        yoffsetPoA1 =
+          tag[OffsetX](epicsTransform(recordPrec)(offset.toFocalPlaneOffset(iaa).y.toMillimeters)),
+        instrAA = epicsTransform(recordPrec)(iaa.toDegrees),
+        sourceAWavelength = epicsTransform(recordPrec)(wavelength.length.toAngstroms)
+      )
+    )
+
+    val config = baseConfig.copy(
+      tc = TelescopeConfig(offset.some, wavelength.some)
+    )
+
+    val genOut: IO[List[TestTcsEpics.TestTcsEvent]] = for {
+      d <- dumbEpics
+      c  = TcsControllerEpicsCommon(d)
+      _ <- c.applyBasicConfig(TcsController.Subsystem.allButGaos, config)
+      r <- d.outputF
+    } yield r
+
+    val result = genOut.unsafeRunSync()
+
+    assert(
+      !result.exists {
+        case TestTcsEvent.OffsetACmd(_, _) => true
+        case _                             => false
+      }
+    )
+
+  }
+
+  it should "apply the target wavelength if it changes" in {
+
+    val wavelengthDemand  = Wavelength((2000.0 / 7.0).nanometers)
+    val wavelengthCurrent = Wavelength((2000.0 / 7.0).nanometers + 1.angstroms)
+    val recordPrec        = 0
+
+    val dumbEpics = buildTcsController[IO](
+      TestTcsEpics.defaultState.copy(
+        sourceAWavelength = epicsTransform(recordPrec)(wavelengthCurrent.length.toAngstroms)
+      )
+    )
+
+    val config = baseConfig.copy(
+      tc = TelescopeConfig(None, wavelengthDemand.some)
+    )
+
+    val genOut: IO[List[TestTcsEpics.TestTcsEvent]] = for {
+      d <- dumbEpics
+      c  = TcsControllerEpicsCommon(d)
+      _ <- c.applyBasicConfig(TcsController.Subsystem.allButGaos, config)
+      r <- d.outputF
+    } yield r
+
+    val result = genOut.unsafeRunSync()
+
+    assert(
+      result.exists {
+        case TestTcsEvent.WavelSourceACmd(_) => true
+        case _                               => false
+      }
+    )
+
+  }
+
+  it should "not reapply the target wavelength if it is already at the right value" in {
+
+    val wavelength = Wavelength((2000.0 / 7.0).nanometers)
+    val recordPrec = 0
+
+    val dumbEpics = buildTcsController[IO](
+      TestTcsEpics.defaultState.copy(
+        sourceAWavelength = epicsTransform(recordPrec)(wavelength.length.toAngstroms)
+      )
+    )
+
+    val config = baseConfig.copy(
+      tc = TelescopeConfig(None, wavelength.some)
+    )
+
+    val genOut: IO[List[TestTcsEpics.TestTcsEvent]] = for {
+      d <- dumbEpics
+      c  = TcsControllerEpicsCommon(d)
+      _ <- c.applyBasicConfig(TcsController.Subsystem.allButGaos, config)
+      r <- d.outputF
+    } yield r
+
+    val result = genOut.unsafeRunSync()
+
+    assert(
+      !result.exists {
+        case TestTcsEvent.WavelSourceACmd(_) => true
+        case _                               => false
+      }
+    )
+
+  }
+
 }
 
 object TcsControllerEpicsCommonSpec {

modules/server/src/test/scala/seqexec/server/tcs/TestTcsEpics.scala

@@ -42,6 +42,7 @@ class TestTcsEpics[F[_]: Sync](
       mountGuideCmd,
       offsetACmd,
       offsetBCmd,
+      wavelSourceA,
       pwfs1Park,
       pwfs1ProbeFollowCmd,
       pwfs1ProbeGuideCmd,
@@ -156,9 +157,17 @@ class TestTcsEpics[F[_]: Sync](
     override def setY(v: Double): F[Unit] = Applicative[F].unit
   }
 
-  override val wavelSourceA: TargetWavelengthCmd[F] = new DummyCmd[F] with TargetWavelengthCmd[F] {
-    override def setWavel(v: Double): F[Unit] = Applicative[F].unit
-  }
+  override val wavelSourceA: TargetWavelengthCmd[F] =
+    new TestEpicsCommand1[F, State, TestTcsEvent, Double](State.wavelSourceACmd, state, out)
+      with TargetWavelengthCmd[F] {
+      override def setWavel(v: Double): F[Unit] = setParameter1(v)
+
+      override protected def event(st: State): TestTcsEvent =
+        TestTcsEvent.WavelSourceACmd(st.wavelSourceACmd.param1)
+
+      override protected def cmd(st: State): State =
+        st.copy(sourceAWavelength = st.wavelSourceACmd.param1)
+    }
 
   override val wavelSourceB: TargetWavelengthCmd[F] = new DummyCmd[F] with TargetWavelengthCmd[F] {
     override def setWavel(v: Double): F[Unit] = Applicative[F].unit
@@ -863,6 +872,7 @@ object TestTcsEpics {
     pwfs2ProbeFollowCmd:      TestEpicsCommand1.State[String],
     oiwfsProbeFollowCmd:      TestEpicsCommand1.State[String],
     offsetACmd:               TestEpicsCommand2.State[Double, Double],
+    wavelSourceACmd:          TestEpicsCommand1.State[Double],
     pwfs1ParkCmd:             TestEpicsCommand0.State,
     pwfs2ParkCmd:             TestEpicsCommand0.State,
     oiwfsParkCmd:             TestEpicsCommand0.State,
@@ -971,6 +981,7 @@ object TestTcsEpics {
       nodbchopb: String
     )                               extends TestTcsEvent
     final case class OffsetACmd(p: Double, q: Double) extends TestTcsEvent
+    final case class WavelSourceACmd(w: Double) extends TestTcsEvent
     final case class Pwfs1ProbeFollowCmd(state: String) extends TestTcsEvent
     final case class Pwfs2ProbeFollowCmd(state: String) extends TestTcsEvent
     final case class OiwfsProbeFollowCmd(state: String) extends TestTcsEvent
@@ -1146,6 +1157,7 @@ object TestTcsEpics {
     pwfs2ProbeFollowCmd = TestEpicsCommand1.State[String](false, ""),
     oiwfsProbeFollowCmd = TestEpicsCommand1.State[String](false, ""),
     offsetACmd = TestEpicsCommand2.State[Double, Double](false, 0.0, 0.0),
+    wavelSourceACmd = TestEpicsCommand1.State[Double](false, 0.0),
     pwfs1ParkCmd = false,
     pwfs2ParkCmd = false,
     oiwfsParkCmd = false,