Non-diegetic support for Ambisonics Mapping 254.

Signed-off-by: Jean-Marc Valin <jmvalin@jmvalin.ca>
diff --git a/src/opus_multistream_encoder.c b/src/opus_multistream_encoder.c
index 2c4dca2..98e174b 100644
--- a/src/opus_multistream_encoder.c
+++ b/src/opus_multistream_encoder.c
@@ -132,6 +132,29 @@
    return (opus_val32*)(void*)ptr;
 }
 
+#ifdef ENABLE_EXPERIMENTAL_AMBISONICS
+static int validate_ambisonics(int nb_channels, int *nb_streams, int *nb_coupled_streams)
+{
+   int order_plus_one;
+   int acn_channels;
+   int nondiegetic_channels;
+
+   order_plus_one = isqrt32(nb_channels);
+   acn_channels = order_plus_one * order_plus_one;
+   nondiegetic_channels = nb_channels - acn_channels;
+
+   if (order_plus_one < 1 || order_plus_one > 15 ||
+       (nondiegetic_channels != 0 && nondiegetic_channels != 2))
+      return 0;
+
+   if (nb_streams)
+      *nb_streams = acn_channels + (nondiegetic_channels != 0);
+   if (nb_coupled_streams)
+      *nb_coupled_streams = nondiegetic_channels != 0;
+   return 1;
+}
+#endif
+
 static int validate_encoder_layout(const ChannelLayout *layout)
 {
    int s;
@@ -423,8 +446,8 @@
 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
    } else if (mapping_family==254)
    {
-      nb_streams=channels;
-      nb_coupled_streams=0;
+      if (!validate_ambisonics(channels, &nb_streams, &nb_coupled_streams))
+         return 0;
 #endif
    } else
       return 0;
@@ -467,7 +490,17 @@
    st->variable_duration = OPUS_FRAMESIZE_ARG;
    for (i=0;i<st->layout.nb_channels;i++)
       st->layout.mapping[i] = mapping[i];
-   if (!validate_layout(&st->layout) || !validate_encoder_layout(&st->layout))
+   if (!validate_layout(&st->layout))
+      return OPUS_BAD_ARG;
+   if (mapping_type == MAPPING_TYPE_SURROUND &&
+       !validate_encoder_layout(&st->layout))
+      return OPUS_BAD_ARG;
+#ifdef ENABLE_EXPERIMENTAL_AMBISONICS
+   if (mapping_type == MAPPING_TYPE_AMBISONICS &&
+       !validate_ambisonics(st->layout.nb_channels, NULL, NULL))
+      return OPUS_BAD_ARG;
+#endif
+   )
       return OPUS_BAD_ARG;
    ptr = (char*)st + align(sizeof(OpusMSEncoder));
    coupled_size = opus_encoder_get_size(2);
@@ -564,10 +597,12 @@
    } else if (mapping_family==254)
    {
       int i;
-      *streams=channels;
-      *coupled_streams=0;
-      for(i=0;i<channels;i++)
-         mapping[i] = i;
+      if (!validate_ambisonics(channels, streams, coupled_streams))
+         return OPUS_BAD_ARG;
+      for(i = 0; i < (*streams - *coupled_streams); i++)
+         mapping[i] = i + (*coupled_streams * 2);
+      for(i = 0; i < *coupled_streams * 2; i++)
+         mapping[i + (*streams - *coupled_streams)] = i;
 #endif
    } else
       return OPUS_UNIMPLEMENTED;
@@ -748,14 +783,19 @@
       )
 {
    int i;
-   int non_mono_rate;
    int total_rate;
+   int directional_rate;
+   int nondirectional_rate;
+   int leftover_bits;
 
-   /* The mono channel gets (rate_ratio_num / rate_ratio_den) times as many bits
-    * as all other channels */
+   /* Each nondirectional channel gets (rate_ratio_num / rate_ratio_den) times
+    * as many bits as all other ambisonics channels.
+    */
    const int rate_ratio_num = 4;
    const int rate_ratio_den = 3;
-   const int num_channels = st->layout.nb_streams;
+   const int nb_channels = st->layout.nb_streams + st->layout.nb_coupled_streams;
+   const int nb_nondirectional_channels = st->layout.nb_coupled_streams * 2 + 1;
+   const int nb_directional_channels = st->layout.nb_streams - 1;
 
    if (st->bitrate_bps==OPUS_AUTO)
    {
@@ -763,25 +803,52 @@
          (Fs+60*Fs/frame_size) + st->layout.nb_streams * 15000;
    } else if (st->bitrate_bps==OPUS_BITRATE_MAX)
    {
-      total_rate = num_channels * 320000;
-   } else {
+      total_rate = nb_channels * 320000;
+   } else
+   {
       total_rate = st->bitrate_bps;
    }
 
-   /* Let y be the non-mono rate and let p, q be integers such that the mono
-    * channel rate is (p/q) * y.
+   /* Let y be the directional rate, m be the num of nondirectional channels
+    *   m = (s + 1)
+    * and let p, q be integers such that the nondirectional rate is
+    *   m_rate = (p / q) * y
     * Also let T be the total bitrate to allocate. Then
-    *   (n - 1) y + (p/q) y = T
-    *   y = (T q) / (qn - q + p)
+    *   T = (n - m) * y + m * m_rate
+    * Solving for y,
+    *   y = (q * T) / (m * (p - q) + n * q)
     */
-   non_mono_rate =
-         total_rate * rate_ratio_den
-         / (rate_ratio_den*num_channels + rate_ratio_num - rate_ratio_den);
+   directional_rate =
+      total_rate * rate_ratio_den
+      / (nb_nondirectional_channels * (rate_ratio_num - rate_ratio_den)
+       + nb_channels * rate_ratio_den);
 
-   rate[0] = total_rate - (num_channels - 1) * non_mono_rate;
-   for (i=1;i<st->layout.nb_streams;i++)
+   /* Calculate the nondirectional rate.
+    *   m_rate = y * (p / q)
+    */
+   nondirectional_rate = directional_rate * rate_ratio_num / rate_ratio_den;
+
+   /* Calculate the leftover from truncation error.
+    *   leftover = T - y * (n - m) - m_rate * m
+    * Place leftover bits in omnidirectional channel.
+    */
+   leftover_bits = total_rate
+      - directional_rate * nb_directional_channels
+      - nondirectional_rate * nb_nondirectional_channels;
+
+   /* Calculate rates for each channel */
+   for (i = 0; i < st->layout.nb_streams; i++)
    {
-      rate[i] = non_mono_rate;
+      if (i < st->layout.nb_coupled_streams)
+      {
+         rate[i] = nondirectional_rate * 2;
+      } else if (i == st->layout.nb_coupled_streams)
+      {
+         rate[i] = nondirectional_rate + leftover_bits;
+      } else
+      {
+         rate[i] = directional_rate;
+      }
    }
 }
 #endif /* ENABLE_EXPERIMENTAL_AMBISONICS */