MALedSkit.cpp

/*
 *  This file is part of the AiBO+ project
 *
 *  Copyright (C) 2005-2015 Csaba Kertész (csaba.kertesz@gmail.com)
 *
 *  AiBO+ 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 2 of the License, or
 *  (at your option) any later version.
 *
 *  AiBO+ 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 this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Street #330, Boston, MA 02111-1307, USA.
 *
 */

#include "MALedSkit.hpp"

#include "core/MADevice.hpp"
#include "core/MAValueGenerators.hpp"
#include "MASkitDatabase.hpp"

#include <MCContainers.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/math/constants/constants.hpp>

namespace
{
int GetTime(MCBinaryData& data, int& chunk_length)
{
  int Time = 0;

  while (!data.IsPositionAtEnd() > 0 && chunk_length > 4)
  {
    unsigned char Char = data.GetUChar();

    chunk_length--;
    if (Char < 128)
    {
      Time = (Time << 7)+(int)Char;
      return Time;
    }
    Time = (Time << 7)+(int)Char % 128;
  }
  return Time;
}
}

MALedSkit::MALedSkit() : MASkitBase(), SkitDuration(0)
{
  Animations.resize(256);
}


std::string MALedSkit::GetName() const
{
  return Name;
}


int MALedSkit::GetDuration() const
{
  return SkitDuration;
}


void MALedSkit::Dump() const
{
  MC_LOG("-- %s LED animations --", Name.c_str());
  MC_LOG("Animation duration: %d msec", GetDuration());
  foreach_i (i, animation, Animations)
  {
    if (!animation->empty())
    {
      printf("LED 0x%x: ", i);
    }
    std::string LogMessage;

    for (unsigned int i1 = 0; !Animations[i].empty() && i1 < Animations[i].size()-2; i1 += 3)
    {
      LogMessage += "start "+MCToStr(Animations[i][i1])+" msec - ";
      LogMessage += "duration "+MCToStr(Animations[i][i1+1])+" msec - ";
      LogMessage += "value "+MCToStr((float)Animations[i][i1+2] / 100);
      if (i1 < Animations[i].size()-3)
        LogMessage += " | ";
    }
    if (!LogMessage.empty())
      MC_LOG("%s", LogMessage.c_str());
  }
}


MALedSkit* MALedSkit::DecodeLedData(const MCBinaryData& binary_data, const std::string& name)
{
  MCBinaryData& BinaryData = const_cast<MCBinaryData&>(binary_data);
  int OldPosition = BinaryData.GetPosition();

  BinaryData.SetPosition(0);
  // Test the file signature
  if (BinaryData.GetString(4) != "MThd")
  {
    MC_WARNING("Wrong file signature, can't decode LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  /*
   * Block 0
   */
  // Skip the chunk length (block 0)
  BinaryData.GetInt32(true);
  // Check the format 0 (block 0)
  if (BinaryData.GetInt16(false) != 0)
  {
    MC_WARNING("Not format 0 file, can't decode the LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  // Check the track number (block 0)
  if (BinaryData.GetInt16(false) != 1)
  {
    MC_WARNING("Not a file with one track, can't decode the LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  int TimeDelta = BinaryData.GetInt16(false);

  // Test the chunk signature
  if (BinaryData.GetString(4) != "MTrk")
  {
    MC_WARNING("Wrong chunk signature, can't decode LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  // Get the chunk length (block 0)
  int ChunkLength = BinaryData.GetInt32(false);

  // Test the model signature
  if (BinaryData.GetUChar() != 0x00 || BinaryData.GetUChar() != 0xff || BinaryData.GetUChar() != 0x01 ||
      BinaryData.GetUChar() != 9 || BinaryData.GetString(9, "= ") != "MODEL=  7")
  {
    MC_WARNING("Only Sony ERS-7 model is supported, can't decode LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  // Skip the string closing character (0x00)
  BinaryData.GetUChar();
  ChunkLength -= (4+9+1);
  // Skip the track name
  if (BinaryData.GetUChar() != 0xff || BinaryData.GetUChar() != 0x03)
  {
    MC_WARNING("Track name is not readable, can't decode LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  ChunkLength -= 2;
  int StringLength = BinaryData.GetUChar();

  if (BinaryData.GetString(StringLength).empty())
  {
    MC_WARNING("Track name is not readable, can't decode LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  // Skip the string closing character (0x00)
  BinaryData.GetUChar();
  ChunkLength -= (1+StringLength+1);
  // Check tempo identifier
  if (BinaryData.GetUChar() != 0xff || BinaryData.GetUChar() != 0x51 || BinaryData.GetUChar() != 0x03)
  {
    MC_WARNING("Tempo identifier is invalid, can't decode LED sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  ChunkLength -= 3;
  int KeyframeUsec = (int)BinaryData.GetUChar()*256*256+(int)BinaryData.GetUChar()*256+BinaryData.GetUChar();

  ChunkLength -= 3;
  // Note: This calculation is a bit tricky and does not make sense to me, but
  // that is the way how it should be done.
  const float TimeConversionRate = (float)KeyframeUsec / ((float)TimeDelta*1000 / 1024) / 1000;
  int StartTime = (int)((float)GetTime(BinaryData, ChunkLength)*TimeConversionRate);

  if (ChunkLength > 4)
  {
    if (BinaryData.GetUChar() != 0x90)
    {
      MC_WARNING("Status byte (0x90) is missing");
      BinaryData.SetPosition(OldPosition);
      return nullptr;
    }
    ChunkLength--;
  }
  /*
   * Parse the LED animations
   */
  MC::IntList Devices, StartTimes, Intensities;
  MALedSkit* LedSkit = new MALedSkit;

  LedSkit->Name = name;
  while (ChunkLength > 4)
  {
    unsigned char DeviceChar = BinaryData.GetUChar();
    unsigned char IntensityChar = BinaryData.GetUChar();

    // Correct the LED intensity values to range [0..100]
    if (IntensityChar == 31)
      IntensityChar = 25;
    if (IntensityChar == 63)
      IntensityChar = 50;
    if (IntensityChar == 95)
      IntensityChar = 75;
    if (IntensityChar == 127)
      IntensityChar = 100;
    if (IntensityChar > 0)
    {
      Devices.push_back((int)DeviceChar);
      Intensities.push_back((int)IntensityChar);
      StartTimes.push_back(StartTime);
    }
    if (IntensityChar == 0)
    {
      foreach_i (i, device, Devices)
      {
        if (*device == DeviceChar)
        {
          LedSkit->Animations[(int)DeviceChar].push_back(StartTimes[i]);
          LedSkit->Animations[(int)DeviceChar].push_back(StartTime-StartTimes[i]);
          LedSkit->Animations[(int)DeviceChar].push_back(Intensities[i]);
          Devices.erase(Devices.begin()+i);
          Intensities.erase(Intensities.begin()+i);
          StartTimes.erase(StartTimes.begin()+i);
          break;
        }
      }
    }
    ChunkLength -= 2;
    StartTime += (int)((float)GetTime(BinaryData, ChunkLength)*TimeConversionRate);
    if (ChunkLength == 4)
    {
      LedSkit->SkitDuration = StartTime;
    }
  }
  // cppcheck-suppress duplicateExpression
  if (ChunkLength != 4 || BinaryData.GetUChar() != 0x00 || BinaryData.GetUChar() != 0xff ||
      BinaryData.GetUChar() != 0x2f || BinaryData.GetUChar() != 0x00)
  {
    MC_WARNING("Invalid chunk ending, can't decode LED sequence");
    BinaryData.SetPosition(OldPosition);
    delete LedSkit;
    return nullptr;
  }
  BinaryData.SetPosition(OldPosition);
  MC_LOG("Decoded LED sequence: %s", name.c_str());
  return LedSkit;
}


MAGeneratorContainer* MALedSkit::GetGeneratorContainer(unsigned int device) const
{
  if (device > 255 || Animations[device].empty())
  {
    return nullptr;
  }
  MAGeneratorContainer* Container = new MAGeneratorContainer(*new MASimpleGenerator(0, 0, 0));
  int Time = 0;

  for (unsigned int i = 0; i < Animations[device].size()-2; i += 3)
  {
    float TargetValue = (float)Animations[device][i+2] / 100;
    bool GapBefore = false;

    // Add the initial delay properly
    if ((i == 0 && Animations[device][i] != 0) ||
        (i > 0 && Animations[device][i]-Time > 0))
    {
      // Note: We can use this calculation for both cases because the Time variable holds zero
      // when i == 0.
      Container->AddGenerator(*new MASimpleGenerator(0, 0, Animations[device][i]-Time));
      Time += Animations[device][i]-Time;
      GapBefore = true;
    }
    if (i+3 >= Animations[device].size() || Time+Animations[device][i+1] < Animations[device][i+3])
    {
      if (GapBefore)
      {
        // This is a special case: There are gaps before and after the LED value.
        const int Duration = Animations[device][i+1] / 3;

        Container->AddGenerator(*new MAPeriodicGenerator(TargetValue,
                                                         0, Duration, Duration, Duration, 0, false));
        Time += Animations[device][i+1];
        continue;
      } else {
        // The LED value must be interpolated back to zero for the last period or
        // there is an empty gap until the next period
        TargetValue = 0;
      }
    }

    Container->AddGenerator(*new MASimpleGenerator(MCFloatInfinity(), TargetValue, Animations[device][i+1]));
    Time += Animations[device][i+1];
  }
  if (SkitDuration-Time > 0)
    Container->AddGenerator(*new MASimpleGenerator(0, 0, SkitDuration-Time));
  else
    Container->AddGenerator(*new MASimpleGenerator(0, 0, 0));

  return Container;
}