<commit_before>/*
   Copyright 2017 Thomas Krause <thomaskrause@posteo.de>

   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.
*/

#include "console.h"

#include <humblelogging/api.h>
#include <iomanip>

#include <thread>
#include <boost/thread/lock_guard.hpp>
#include <boost/thread/shared_lock_guard.hpp>

#include <annis/util/helper.h>
#include <annis/util/relannisloader.h>
#include <annis/query/query.h>
#include <annis/util/threadpool.h>
#include <annis/util/plan.h>

HUMBLE_LOGGER(logger, "default");

#ifdef ENABLE_VALGRIND
  #include <valgrind/callgrind.h>
#else
  #define CALLGRIND_STOP_INSTRUMENTATION

  #define CALLGRIND_START_INSTRUMENTATION
#endif // ENABLE_VALGRIND

using namespace annis;

Console::Console()
 : dbCache(1073741824l*8l), db(dbCache.get(currentDBPath.string(), true))
{
  currentDBPath = boost::filesystem::unique_path(
          boost::filesystem::temp_directory_path().string() + "/annis-temporary-workspace-%%%%-%%%%-%%%%-%%%%");
  HL_INFO(logger, "Using " + currentDBPath.string() + " as temporary path");

  unsigned int numOfCPUs = std::thread::hardware_concurrency();
  if(numOfCPUs >= 4)
  {
    config.threadPool = std::make_shared<ThreadPool>(numOfCPUs);
    config.numOfBackgroundTasks = numOfCPUs;
  }
}

bool Console::execute(const std::string &cmd, const std::vector<std::string> &args)
{
  try
  {
    if (cmd == "import")
    {
      import(args);
    }
    else if(cmd == "save")
    {
      save(args);
    }
    else if(cmd == "load")
    {
      load(args);
    }
    else if(cmd == "info")
    {
      info();
    }
    else if(cmd == "optimize")
    {
      optimize();
    }
    else if(cmd == "count")
    {
      count(args);
    }
    else if(cmd == "find")
    {
      find(args);
    }
    else if(cmd == "update_statistics")
    {
      updateStatistics();
    }
    else if(cmd == "guess")
    {
      guess(args);
    }
    else if(cmd == "guess_regex")
    {
      guessRegex(args);
    }
    else if(cmd == "plan")
    {
      plan(args);
    }
    else if(cmd == "memory")
    {
      memory(args);
    }
    else if (cmd == "quit" || cmd == "exit")
    {
      return true;
    }
    else
    {
      std::cout << "Unknown command \"" << cmd << "\"" << std::endl;
    }

  }
  catch(std::string ex)
  {
    std::cerr << "Exception: " << ex << std::endl;
  }

  return false;
}

void Console::import(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::cout << "Import relANNIS from " << args[0] << std::endl;
      RelANNISLoader::loadRelANNIS(*db, args[0]);
      if(args.size() > 1)
      {
        // directly save the imported corpus to directory
        HL_INFO(logger, "saving to " +  args[1]);
        db->save(args[1]);
      }
    }
    else
    {
      std::cout << "You have to give a path as argument" << std::endl;
    }
  }
}

void Console::save(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::cout << "Save to " << args[0] << std::endl;
      auto startTime = annis::Helper::getSystemTimeInMilliSeconds();
      db->save(args[0]);
      auto endTime = annis::Helper::getSystemTimeInMilliSeconds();
      std::cout << "Saved in " << (endTime - startTime) << " ms" << std::endl;
    }
    else
    {
      std::cout << "You have to give a path as argument" << std::endl;
    }
  }
}

void Console::load(const std::vector<std::string> &args)
{
  CALLGRIND_STOP_INSTRUMENTATION;
  if(args.size() > 0)
  {
    std::cout << "Loading from " << args[0] << std::endl;
    auto startTime = annis::Helper::getSystemTimeInMilliSeconds();
    db = dbCache.get(args[0], args.size() > 1 && args[1] == "preload");
    auto endTime = annis::Helper::getSystemTimeInMilliSeconds();
    std::cout << "Loaded in " << (endTime - startTime) << " ms" << std::endl;
  }
  else
  {
    std::cout << "You have to give a path as argument" << std::endl;
  }
  CALLGRIND_START_INSTRUMENTATION;

}

void Console::info()
{
  if(db)
  {
    std::cout << db->info() << std::endl;
  }
}

void Console::optimize()
{
  if(db)
  {
    std::cout << "Optimizing..." << std::endl;
    db->optimizeAll();
    std::cout << "Finished." << std::endl;
  }
}

void Console::count(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::string json = boost::join(args, " ");
      std::cout << "Counting..." << std::endl;
      std::stringstream ss;
      ss << json;
      try
      {
        std::shared_ptr<annis::Query> q = annis::JSONQueryParser::parse(*db, ss, config);
        int counter =0;
        auto startTime = annis::Helper::getSystemTimeInMilliSeconds();
        while(q->next())
        {
          counter++;
        }
        auto endTime = annis::Helper::getSystemTimeInMilliSeconds();
        std::cout << counter << " matches in " << (endTime - startTime) << " ms" << std::endl;
      }
      catch(Json::RuntimeError err)
      {
        std::cout << "JSON error: " << err.what() << std::endl;
      }

    }
    else
    {
      std::cout << "you need to give the query JSON as argument" << std::endl;
    }
  }
}

void Console::find(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::string json = boost::join(args, " ");
      std::cout << "Finding..." << std::endl;
      std::stringstream ss;
      ss << json;
      try
      {
        std::shared_ptr<annis::Query> q = annis::JSONQueryParser::parse(*db, ss, config);
        int counter =0;
        while(q->next())
        {
          std::vector<annis::Match> m = q->getCurrent();
          for(size_t i = 0; i < m.size(); i++)
          {
            const auto& n = m[i];
            if(db->getNodeType(n.node) == "node")
            {
              std::cout << db->getNodeDebugName(n.node);
              if(n.anno.ns != 0 && n.anno.name != 0)
              {
                std::cout << " " << db->strings.str(n.anno.ns)
                  << "::" << db->strings.str(n.anno.name);
              }
              if(i < m.size()-1)
              {
               std::cout << ", ";
              }
            }
          }
          std::cout << std::endl;
          counter++;
        }
        std::cout << counter << " matches" << std::endl;

      }
      catch(Json::RuntimeError err)
      {
        std::cout << "JSON error: " << err.what() << std::endl;
      }

    }
    else
    {
      std::cout << "you need to give the query JSON as argument" << std::endl;
    }
  }
}

void Console::updateStatistics()
{
  if(db)
  {
    std::cout << "Updating statistics...";
    db->nodeAnnos.calculateStatistics(db->strings);
    std::cout << " Done" << std::endl;
  }
}

void Console::guess(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() == 3)
    {
      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCount(db->strings, args[0], args[1], args[2]) << std::endl;
    }
    else if(args.size() == 2)
    {
      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCount(db->strings, args[0], args[1]) << std::endl;
    }
    else
    {
      std::cout << "Must provide at two (name and value) or three (namespace name value) arguments" << std::endl;
    }
  }
}

void Console::guessRegex(const std::vector<std::string> &args)
{
  if(db)
  {

    if(args.size() == 3)
    {

      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCountRegex(db->strings, args[0], args[1], re2::RE2(args[2])) << std::endl;
    }
    else if(args.size() == 2)
    {
      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCountRegex(db->strings, args[0], re2::RE2(args[1])) << std::endl;
    }
    else
    {
      std::cout << "Must provide at two (name and regex) or three (namespace name regex) arguments" << std::endl;
    }
  }
}

void Console::plan(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::string json = boost::join(args, " ");
      std::cout << "Planning..." << std::endl;
      std::stringstream ss;
      ss << json;
      try
      {
        std::shared_ptr<annis::Query> q = annis::JSONQueryParser::parse(*db, ss, config);
        std::cout << q->debugString() << std::endl;
      }
      catch(Json::RuntimeError err)
      {
        std::cout << "JSON error: " << err.what() << std::endl;
      }

    }
    else
    {
      std::cout << "you need to give the query JSON as argument" << std::endl;
    }
  }
}

void Console::memory(const std::vector<std::string> args)
{
  if(args.empty())
  {

    auto corpusSizes = dbCache.estimateCorpusSizes();
    for(auto it = corpusSizes.begin();
        it != corpusSizes.end(); it++)

    {
      if(!it->first.corpusPath.empty())
      {
        const DBCache::CorpusSize& size = it->second;
        std::cout << it->first.corpusPath << ": " << Helper::inMB(size.estimated) << " MB (estimated) " << Helper::inMB(size.measured) << " MB (measured)" << std::endl;
      }
    }
    DBCache::CorpusSize total = dbCache.calculateTotalSize();
    std::cout << "Used total memory (estimated): "  << Helper::inMB(total.estimated) << " MB" << std::endl;
    std::cout << "Used total memory (measured): "  << Helper::inMB(total.measured) << " MB" << std::endl;

  }
  else if(args[0] == "clear")
  {
    dbCache.releaseAll();
    std::cout << "Cleared cache" << std::endl;
  }
}

<commit_msg>Output the time it takes to do the planning<commit_after>/*
   Copyright 2017 Thomas Krause <thomaskrause@posteo.de>

   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.
*/

#include "console.h"

#include <humblelogging/api.h>
#include <iomanip>

#include <thread>
#include <boost/thread/lock_guard.hpp>
#include <boost/thread/shared_lock_guard.hpp>

#include <annis/util/helper.h>
#include <annis/util/relannisloader.h>
#include <annis/query/query.h>
#include <annis/util/threadpool.h>
#include <annis/util/plan.h>

HUMBLE_LOGGER(logger, "default");

#ifdef ENABLE_VALGRIND
  #include <valgrind/callgrind.h>
#else
  #define CALLGRIND_STOP_INSTRUMENTATION

  #define CALLGRIND_START_INSTRUMENTATION
#endif // ENABLE_VALGRIND

using namespace annis;

Console::Console()
 : dbCache(1073741824l*8l), db(dbCache.get(currentDBPath.string(), true))
{
  currentDBPath = boost::filesystem::unique_path(
          boost::filesystem::temp_directory_path().string() + "/annis-temporary-workspace-%%%%-%%%%-%%%%-%%%%");
  HL_INFO(logger, "Using " + currentDBPath.string() + " as temporary path");

  unsigned int numOfCPUs = std::thread::hardware_concurrency();
  if(numOfCPUs >= 4)
  {
    config.threadPool = std::make_shared<ThreadPool>(numOfCPUs);
    config.numOfBackgroundTasks = numOfCPUs;
  }
}

bool Console::execute(const std::string &cmd, const std::vector<std::string> &args)
{
  try
  {
    if (cmd == "import")
    {
      import(args);
    }
    else if(cmd == "save")
    {
      save(args);
    }
    else if(cmd == "load")
    {
      load(args);
    }
    else if(cmd == "info")
    {
      info();
    }
    else if(cmd == "optimize")
    {
      optimize();
    }
    else if(cmd == "count")
    {
      count(args);
    }
    else if(cmd == "find")
    {
      find(args);
    }
    else if(cmd == "update_statistics")
    {
      updateStatistics();
    }
    else if(cmd == "guess")
    {
      guess(args);
    }
    else if(cmd == "guess_regex")
    {
      guessRegex(args);
    }
    else if(cmd == "plan")
    {
      plan(args);
    }
    else if(cmd == "memory")
    {
      memory(args);
    }
    else if (cmd == "quit" || cmd == "exit")
    {
      return true;
    }
    else
    {
      std::cout << "Unknown command \"" << cmd << "\"" << std::endl;
    }

  }
  catch(std::string ex)
  {
    std::cerr << "Exception: " << ex << std::endl;
  }

  return false;
}

void Console::import(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::cout << "Import relANNIS from " << args[0] << std::endl;
      RelANNISLoader::loadRelANNIS(*db, args[0]);
      if(args.size() > 1)
      {
        // directly save the imported corpus to directory
        HL_INFO(logger, "saving to " +  args[1]);
        db->save(args[1]);
      }
    }
    else
    {
      std::cout << "You have to give a path as argument" << std::endl;
    }
  }
}

void Console::save(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::cout << "Save to " << args[0] << std::endl;
      auto startTime = annis::Helper::getSystemTimeInMilliSeconds();
      db->save(args[0]);
      auto endTime = annis::Helper::getSystemTimeInMilliSeconds();
      std::cout << "Saved in " << (endTime - startTime) << " ms" << std::endl;
    }
    else
    {
      std::cout << "You have to give a path as argument" << std::endl;
    }
  }
}

void Console::load(const std::vector<std::string> &args)
{
  CALLGRIND_STOP_INSTRUMENTATION;
  if(args.size() > 0)
  {
    std::cout << "Loading from " << args[0] << std::endl;
    auto startTime = annis::Helper::getSystemTimeInMilliSeconds();
    db = dbCache.get(args[0], args.size() > 1 && args[1] == "preload");
    auto endTime = annis::Helper::getSystemTimeInMilliSeconds();
    std::cout << "Loaded in " << (endTime - startTime) << " ms" << std::endl;
  }
  else
  {
    std::cout << "You have to give a path as argument" << std::endl;
  }
  CALLGRIND_START_INSTRUMENTATION;

}

void Console::info()
{
  if(db)
  {
    std::cout << db->info() << std::endl;
  }
}

void Console::optimize()
{
  if(db)
  {
    std::cout << "Optimizing..." << std::endl;
    db->optimizeAll();
    std::cout << "Finished." << std::endl;
  }
}

void Console::count(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::string json = boost::join(args, " ");
      std::cout << "Counting..." << std::endl;
      std::stringstream ss;
      ss << json;
      try
      {
        std::shared_ptr<annis::Query> q = annis::JSONQueryParser::parse(*db, ss, config);
        int counter =0;
        auto startTime = annis::Helper::getSystemTimeInMilliSeconds();
        while(q->next())
        {
          counter++;
        }
        auto endTime = annis::Helper::getSystemTimeInMilliSeconds();
        std::cout << counter << " matches in " << (endTime - startTime) << " ms" << std::endl;
      }
      catch(Json::RuntimeError err)
      {
        std::cout << "JSON error: " << err.what() << std::endl;
      }

    }
    else
    {
      std::cout << "you need to give the query JSON as argument" << std::endl;
    }
  }
}

void Console::find(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::string json = boost::join(args, " ");
      std::cout << "Finding..." << std::endl;
      std::stringstream ss;
      ss << json;
      try
      {
        std::shared_ptr<annis::Query> q = annis::JSONQueryParser::parse(*db, ss, config);
        int counter =0;
        while(q->next())
        {
          std::vector<annis::Match> m = q->getCurrent();
          for(size_t i = 0; i < m.size(); i++)
          {
            const auto& n = m[i];
            if(db->getNodeType(n.node) == "node")
            {
              std::cout << db->getNodeDebugName(n.node);
              if(n.anno.ns != 0 && n.anno.name != 0)
              {
                std::cout << " " << db->strings.str(n.anno.ns)
                  << "::" << db->strings.str(n.anno.name);
              }
              if(i < m.size()-1)
              {
               std::cout << ", ";
              }
            }
          }
          std::cout << std::endl;
          counter++;
        }
        std::cout << counter << " matches" << std::endl;

      }
      catch(Json::RuntimeError err)
      {
        std::cout << "JSON error: " << err.what() << std::endl;
      }

    }
    else
    {
      std::cout << "you need to give the query JSON as argument" << std::endl;
    }
  }
}

void Console::updateStatistics()
{
  if(db)
  {
    std::cout << "Updating statistics...";
    db->nodeAnnos.calculateStatistics(db->strings);
    std::cout << " Done" << std::endl;
  }
}

void Console::guess(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() == 3)
    {
      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCount(db->strings, args[0], args[1], args[2]) << std::endl;
    }
    else if(args.size() == 2)
    {
      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCount(db->strings, args[0], args[1]) << std::endl;
    }
    else
    {
      std::cout << "Must provide at two (name and value) or three (namespace name value) arguments" << std::endl;
    }
  }
}

void Console::guessRegex(const std::vector<std::string> &args)
{
  if(db)
  {

    if(args.size() == 3)
    {

      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCountRegex(db->strings, args[0], args[1], re2::RE2(args[2])) << std::endl;
    }
    else if(args.size() == 2)
    {
      std::cout << "Guessed maximum count: " << db->nodeAnnos.guessMaxCountRegex(db->strings, args[0], re2::RE2(args[1])) << std::endl;
    }
    else
    {
      std::cout << "Must provide at two (name and regex) or three (namespace name regex) arguments" << std::endl;
    }
  }
}

void Console::plan(const std::vector<std::string> &args)
{
  if(db)
  {
    if(args.size() > 0)
    {
      std::string json = boost::join(args, " ");
      std::cout << "Planning..." << std::endl;
      std::stringstream ss;
      ss << json;
      try
      {
        std::shared_ptr<annis::Query> q = annis::JSONQueryParser::parse(*db, ss, config);
        auto startTime = annis::Helper::getSystemTimeInMilliSeconds();
        std::string debugStr = q->debugString();
        auto endTime = annis::Helper::getSystemTimeInMilliSeconds();
        std::cout << debugStr << std::endl;

        std::cout << "planned in " << (endTime - startTime) << " ms" << std::endl;
      }
      catch(Json::RuntimeError err)
      {
        std::cout << "JSON error: " << err.what() << std::endl;
      }

    }
    else
    {
      std::cout << "you need to give the query JSON as argument" << std::endl;
    }
  }
}

void Console::memory(const std::vector<std::string> args)
{
  if(args.empty())
  {

    auto corpusSizes = dbCache.estimateCorpusSizes();
    for(auto it = corpusSizes.begin();
        it != corpusSizes.end(); it++)

    {
      if(!it->first.corpusPath.empty())
      {
        const DBCache::CorpusSize& size = it->second;
        std::cout << it->first.corpusPath << ": " << Helper::inMB(size.estimated) << " MB (estimated) " << Helper::inMB(size.measured) << " MB (measured)" << std::endl;
      }
    }
    DBCache::CorpusSize total = dbCache.calculateTotalSize();
    std::cout << "Used total memory (estimated): "  << Helper::inMB(total.estimated) << " MB" << std::endl;
    std::cout << "Used total memory (measured): "  << Helper::inMB(total.measured) << " MB" << std::endl;

  }
  else if(args[0] == "clear")
  {
    dbCache.releaseAll();
    std::cout << "Cleared cache" << std::endl;
  }
}

<|endoftext|>


<commit_before>#include <unittest.hpp>
#include <mmm.hpp>

namespace {
  using namespace mmm;

  auto constructors_1 = UnitTest("vector constructors (1)", +[] {
    vec4 v;
    vec4 u(0);
    vec4 t{0};
    vec4 s = vec4();
    vec4 r = vec4(0);

    if (v != u) return false;
    if (u != t) return false;
    if (t != s) return false;
    if (s != r) return false;
    if (r != 0) return false;

    return true;
  });
  auto constructors_2 = UnitTest("vector constructors (2)", +[] {
    vec2 v = vec2(3, 4);
    vec3 u = vec3(2, v);
    vec4 t = vec4(1, u);

    if (t != vec4(1, 2, 3, 4)) return false;

    return true;
  });
  auto constructors_3 = UnitTest("vector constructors (3)", +[] {
    vec2 v = vec2(1, 2);
    vec2 u = vec2(3, 4);
    vec4 t = vec4(v, u);

    vec<10> s = vec<10>(0, t, u + 2, v + 6, 9);

    if (s != vec<10>(0, 1, 2, 3, 4, 5, 6, 7, 8, 9)) return false;

    return true;
  });
  auto constructors_4 = UnitTest("vector constructors (4)", +[] {
    vec4 v = vec4(8, 7, 6, 1);
    vec4 u = vec4(2, 3, 5, 4);
    vec4 t = vec4(12, 10, 11, 13);
    vec4 s = vec4(0);

    vec<15> r = vec<15>(s.x, v.w, u.xywz, v.zyx, 9, t.yzxw, 14);

    if (r != vec<15>(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14))
      return false;

    return true;
  });
  auto constructors_5 = UnitTest("vector constructors (5)", +[] {
    vec4 v = vec4(1, 2, 3, 4);
    vec2 u = vec2(1, 2);

    u = vec2(v);
    if (u != vec2(1, 2)) return false;

    u = vec2(v.yzw);
    if (u != vec2(2, 3)) return false;

    u = vec2(v.zw);
    if (u != vec2(3, 4)) return false;

    u = vec2(v.x);
    if (u != vec2(1, 1)) return false;

    u = vec2(2);
    if (u != vec2(2, 2)) return false;

    u = vec2(0, v);
    if (u != vec2(0, 1)) return false;

    u = vec2(v.x, v.y, v.z);
    if (u != vec2(1, 2)) return false;

    u = vec2(2, v.z);
    if (u != vec2(2, 3)) return false;

    u = vec2(3, v.wzyx);
    if (u != vec2(3, 4)) return false;

    u = vec2(v.x, 2);
    if (u != vec2(1, 2)) return false;

    return true;
  });
  auto constructors_6 = UnitTest("vector constructors (6)", +[] {
    vec4 v = vec4(1, 2, 3, 4);
    vec3 u = vec3(1, 2, 3);

    u = vec3(v);
    if (u != vec3(1, 2, 3)) return false;

    u = vec3(v.wzyx);
    if (u != vec3(4, 3, 2)) return false;

    u = vec3(v.zyx);
    if (u != vec3(3, 2, 1)) return false;

    u = vec3(v.x);
    if (u != vec3(1, 1, 1)) return false;

    u = vec3(2);
    if (u != vec3(2, 2, 2)) return false;

    u = vec3(0, v);
    if (u != vec3(0, 1, 2)) return false;

    u = vec3(v.x, v.y, v.z, v.w);
    if (u != vec3(1, 2, 3)) return false;

    u = vec3(2, v.z, 4);
    if (u != vec3(2, 3, 4)) return false;

    u = vec3(5, v.wzyx);
    if (u != vec3(5, 4, 3)) return false;

    return true;
  });
  auto constructors_7 = UnitTest("vector constructors (7)", +[] {
    mat3x4 m = mat3x4(12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1);
    mat2 n = mat2(1, 2, 3, 4);

    vec2 v;
    vec3 u;
    vec4 t;

    v = vec2(n);
    if (v != vec2(1, 2)) return false;
    v = vec2(1, n);
    if (v != vec2(1, 1)) return false;
    v = vec2(m);
    if (v != vec2(12, 11)) return false;
    v = vec2(1, m);
    if (v != vec2(1, 12)) return false;

    u = vec3(n);
    if (u != vec3(1, 2, 3)) return false;
    u = vec3(1, n);
    if (u != vec3(1, 1, 2)) return false;
    u = vec3(m);
    if (u != vec3(12, 11, 10)) return false;
    u = vec3(1, m);
    if (u != vec3(1, 12, 11)) return false;

    t = vec4(n);
    if (t != vec4(1, 2, 3, 4)) return false;
    t = vec4(v, n);
    if (t != vec4(1, 12, 1, 2)) return false;
    t = vec4(m);
    if (t != vec4(12, 11, 10, 9)) return false;
    t = vec4(u, m);
    if (t != vec4(1, 12, 11, 12)) return false;

    return true;
  });
  auto constructors_8 = UnitTest("vector constructors (8)", +[] {
    ivec4 v = ivec4(4, 3, 2, 1);
    dvec4 u = dvec4(1, 2, 3, 4);
    vec4 r;

    r = vec4(v);
    if (r != vec4(4, 3, 2, 1)) return false;
    r = vec4(u);
    if (r != vec4(1, 2, 3, 4)) return false;

    r = vec4(u.xxyy);
    if (r != vec4(1, 1, 2, 2)) return false;

    return true;
  });
  auto constructors_9 = UnitTest("vector constructors (9)", +[] {
    ivec2 v = ivec2(2, 1);
    dvec2 u = dvec2(1, 2);
    vec2 r;

    r = vec2(v);
    if (r != vec2(2, 1)) return false;
    r = vec2(u);
    if (r != vec2(1, 2)) return false;

    r = vec2(u.yy);
    if (r != vec2(2, 2)) return false;

    return true;
  });

  auto index = UnitTest("vector index", +[] {
    vec4 v = vec4(1, 2, 3, 4);

    float& x = v[0];
    x = 0;
    if (v[0] != 0) return false;

    float y = v[1];
    y = 73;
    if (v[1] != 2) return false;

    v[2] = 12;
    if (v[2] != 12) return false;

    v.data[3] = -1;
    if (v[3] != -1) return false;

    return true;
  });

  auto array_cast = UnitTest("vector array_cast", +[] {
    ivec4 v = ivec4(8, 7, 6, 5);
    int* i = v;

    if (i[0] != 8 and i[1] != 7 and i[2] != 6 and i[3] != 5) return false;

    return true;
  });

  auto swiz_elems = UnitTest("vector swizzleElems", +[] {
    ivec<9> v = ivec<9>(0, 1, 2, 10, 11, 12, 20, 21, 22);

    ivec3 u = v.swizzleElems<3, 4, 5>() += 100;
    if (u != ivec3(110, 111, 112)) return false;
    if (v != ivec<9>(0, 1, 2, 110, 111, 112, 20, 21, 22)) return false;

    return true;
  });
  auto swiz_range1 = UnitTest("vector swizzleRange positive", +[] {
    ivec<9> v = ivec<9>(0, 1, 2, 10, 11, 12, 20, 21, 22);

    auto& u = v.swizzleRange<3, 5>();
    if (u != ivec3(10, 11, 12)) return false;

    u += 100;
    if (u != ivec3(110, 111, 112)) return false;

    if (v != ivec<9>(0, 1, 2, 110, 111, 112, 20, 21, 22)) return false;

    return true;
  });
  auto swiz_range2 = UnitTest("vector swizzleRange negative", +[] {
    ivec<9> v = ivec<9>(0, 1, 2, 10, 11, 12, 20, 21, 22);

    auto& u = v.swizzleRange<5, 3>();
    if (u != ivec3(12, 11, 10)) return false;

    u += 100;
    if (u != ivec3(112, 111, 110)) return false;

    if (v != ivec<9>(0, 1, 2, 110, 111, 112, 20, 21, 22)) return false;

    return true;
  });
}
<commit_msg>add test for `=` initializer_list<commit_after>#include <unittest.hpp>
#include <mmm.hpp>

namespace {
  using namespace mmm;

  auto constructors_1 = UnitTest("vector constructors (1)", +[] {
    vec4 v;
    vec4 u(0);
    vec4 t{0};
    vec4 s = vec4();
    vec4 r = vec4(0);

    if (v != u) return false;
    if (u != t) return false;
    if (t != s) return false;
    if (s != r) return false;
    if (r != 0) return false;

    return true;
  });
  auto constructors_2 = UnitTest("vector constructors (2)", +[] {
    vec2 v = vec2(3, 4);
    vec3 u = vec3(2, v);
    vec4 t = vec4(1, u);

    if (t != vec4(1, 2, 3, 4)) return false;

    return true;
  });
  auto constructors_3 = UnitTest("vector constructors (3)", +[] {
    vec2 v = vec2(1, 2);
    vec2 u = vec2(3, 4);
    vec4 t = vec4(v, u);

    vec<10> s = vec<10>(0, t, u + 2, v + 6, 9);

    if (s != vec<10>(0, 1, 2, 3, 4, 5, 6, 7, 8, 9)) return false;

    return true;
  });
  auto constructors_4 = UnitTest("vector constructors (4)", +[] {
    vec4 v = vec4(8, 7, 6, 1);
    vec4 u = vec4(2, 3, 5, 4);
    vec4 t = vec4(12, 10, 11, 13);
    vec4 s = vec4(0);

    vec<15> r = vec<15>(s.x, v.w, u.xywz, v.zyx, 9, t.yzxw, 14);

    if (r != vec<15>(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14))
      return false;

    return true;
  });
  auto constructors_5 = UnitTest("vector constructors (5)", +[] {
    vec4 v = vec4(1, 2, 3, 4);
    vec2 u = vec2(1, 2);

    u = vec2(v);
    if (u != vec2(1, 2)) return false;

    u = vec2(v.yzw);
    if (u != vec2(2, 3)) return false;

    u = vec2(v.zw);
    if (u != vec2(3, 4)) return false;

    u = vec2(v.x);
    if (u != vec2(1, 1)) return false;

    u = vec2(2);
    if (u != vec2(2, 2)) return false;

    u = vec2(0, v);
    if (u != vec2(0, 1)) return false;

    u = vec2(v.x, v.y, v.z);
    if (u != vec2(1, 2)) return false;

    u = vec2(2, v.z);
    if (u != vec2(2, 3)) return false;

    u = vec2(3, v.wzyx);
    if (u != vec2(3, 4)) return false;

    u = vec2(v.x, 2);
    if (u != vec2(1, 2)) return false;

    return true;
  });
  auto constructors_6 = UnitTest("vector constructors (6)", +[] {
    vec4 v = vec4(1, 2, 3, 4);
    vec3 u = vec3(1, 2, 3);

    u = vec3(v);
    if (u != vec3(1, 2, 3)) return false;

    u = vec3(v.wzyx);
    if (u != vec3(4, 3, 2)) return false;

    u = vec3(v.zyx);
    if (u != vec3(3, 2, 1)) return false;

    u = vec3(v.x);
    if (u != vec3(1, 1, 1)) return false;

    u = vec3(2);
    if (u != vec3(2, 2, 2)) return false;

    u = vec3(0, v);
    if (u != vec3(0, 1, 2)) return false;

    u = vec3(v.x, v.y, v.z, v.w);
    if (u != vec3(1, 2, 3)) return false;

    u = vec3(2, v.z, 4);
    if (u != vec3(2, 3, 4)) return false;

    u = vec3(5, v.wzyx);
    if (u != vec3(5, 4, 3)) return false;

    return true;
  });
  auto constructors_7 = UnitTest("vector constructors (7)", +[] {
    mat3x4 m = mat3x4(12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1);
    mat2 n = mat2(1, 2, 3, 4);

    vec2 v;
    vec3 u;
    vec4 t;

    v = vec2(n);
    if (v != vec2(1, 2)) return false;
    v = vec2(1, n);
    if (v != vec2(1, 1)) return false;
    v = vec2(m);
    if (v != vec2(12, 11)) return false;
    v = vec2(1, m);
    if (v != vec2(1, 12)) return false;

    u = vec3(n);
    if (u != vec3(1, 2, 3)) return false;
    u = vec3(1, n);
    if (u != vec3(1, 1, 2)) return false;
    u = vec3(m);
    if (u != vec3(12, 11, 10)) return false;
    u = vec3(1, m);
    if (u != vec3(1, 12, 11)) return false;

    t = vec4(n);
    if (t != vec4(1, 2, 3, 4)) return false;
    t = vec4(v, n);
    if (t != vec4(1, 12, 1, 2)) return false;
    t = vec4(m);
    if (t != vec4(12, 11, 10, 9)) return false;
    t = vec4(u, m);
    if (t != vec4(1, 12, 11, 12)) return false;

    return true;
  });
  auto constructors_8 = UnitTest("vector constructors (8)", +[] {
    ivec4 v = ivec4(4, 3, 2, 1);
    dvec4 u = dvec4(1, 2, 3, 4);
    vec4 r;

    r = vec4(v);
    if (r != vec4(4, 3, 2, 1)) return false;
    r = vec4(u);
    if (r != vec4(1, 2, 3, 4)) return false;

    r = vec4(u.xxyy);
    if (r != vec4(1, 1, 2, 2)) return false;

    return true;
  });
  auto constructors_9 = UnitTest("vector constructors (9)", +[] {
    ivec2 v = ivec2(2, 1);
    dvec2 u = dvec2(1, 2);
    vec2 r;

    r = vec2(v);
    if (r != vec2(2, 1)) return false;
    r = vec2(u);
    if (r != vec2(1, 2)) return false;

    r = vec2(u.yy);
    if (r != vec2(2, 2)) return false;

    return true;
  });
  auto constructors_10 = UnitTest("vector constructors (10)", +[] {
    vec2 v = {1, 2};
    vec3 u = {1, 2, 3};
    vec4 t = {1, 2, 3, 4};

    if (v != vec2(1, 2)) return false;
    if (u != vec3(1, 2, 3)) return false;
    if (t != vec4(1, 2, 3, 4)) return false;

    return true;
  });

  auto index = UnitTest("vector index", +[] {
    vec4 v = vec4(1, 2, 3, 4);

    float& x = v[0];
    x = 0;
    if (v[0] != 0) return false;

    float y = v[1];
    y = 73;
    if (v[1] != 2) return false;

    v[2] = 12;
    if (v[2] != 12) return false;

    v.data[3] = -1;
    if (v[3] != -1) return false;

    return true;
  });

  auto array_cast = UnitTest("vector array_cast", +[] {
    ivec4 v = ivec4(8, 7, 6, 5);
    int* i = v;

    if (i[0] != 8 and i[1] != 7 and i[2] != 6 and i[3] != 5) return false;

    return true;
  });

  auto swiz_elems = UnitTest("vector swizzleElems", +[] {
    ivec<9> v = ivec<9>(0, 1, 2, 10, 11, 12, 20, 21, 22);

    ivec3 u = v.swizzleElems<3, 4, 5>() += 100;
    if (u != ivec3(110, 111, 112)) return false;
    if (v != ivec<9>(0, 1, 2, 110, 111, 112, 20, 21, 22)) return false;

    return true;
  });
  auto swiz_range1 = UnitTest("vector swizzleRange positive", +[] {
    ivec<9> v = ivec<9>(0, 1, 2, 10, 11, 12, 20, 21, 22);

    auto& u = v.swizzleRange<3, 5>();
    if (u != ivec3(10, 11, 12)) return false;

    u += 100;
    if (u != ivec3(110, 111, 112)) return false;

    if (v != ivec<9>(0, 1, 2, 110, 111, 112, 20, 21, 22)) return false;

    return true;
  });
  auto swiz_range2 = UnitTest("vector swizzleRange negative", +[] {
    ivec<9> v = ivec<9>(0, 1, 2, 10, 11, 12, 20, 21, 22);

    auto& u = v.swizzleRange<5, 3>();
    if (u != ivec3(12, 11, 10)) return false;

    u += 100;
    if (u != ivec3(112, 111, 110)) return false;

    if (v != ivec<9>(0, 1, 2, 110, 111, 112, 20, 21, 22)) return false;

    return true;
  });
}
<|endoftext|>


<commit_before>/*
PStreams - POSIX Process I/O for C++
Copyright (C) 2002 Jonathan Wakely

This file is part of PStreams.

PStreams is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of
the License, or (at your option) any later version.

PStreams 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with PStreams; if not, write to the Free Software Foundation, Inc.,
59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#define REDI_PSTREAMS_POPEN_USES_BIDIRECTIONAL_PIPE 1

//#include "pstream_compat.h"

// TODO test rpstream - this does nothing for the moment
#define TEST_RPSTREAM 1

// test eviscerated pstreams
#define REDI_EVISCERATE_PSTREAMS 1

#include "pstream.h"

#if TEST_RPSTREAM
#include "rpstream.h"
#endif

// include these after pstream.h to ensure it #includes everything it needs
#include <iostream>
#include <iomanip>
#include <string>
#include <sstream>
#include <fstream>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
//#include <fcntl.h>
#include <errno.h>

using namespace std;
using namespace redi;

namespace  // anon
{
    // helper functions for printing test results

    string
    test_type(const istream& s)
    { return "r"; }

    string
    test_type(const ostream& s)
    { return "w"; }

    string
    test_type(const iostream& s)
    { return "b"; }

    template <typename T>
    string
    test_id(const T& s)
    {
        static int count = 0;
        ostringstream buf;
        buf << test_type(s) << ++count;
        return buf.str();
    }

    template <typename T>
    void
    print_result(const T& s, bool result)
    {
        clog << "Test " << setw(4) << test_id(s) << ": "
            << (result ? "Pass" : "Fail!")
            << endl;
    }

    template <typename T>
    bool
    check_pass(const T& s, bool expected = true)
    {
        bool res = s.good() == expected;
        print_result(s, res);
        return res;
    }

    template <typename T>
    bool
    check_fail(const T& s) { return check_pass(s, false); }
}

int main()
{
    ios_base::sync_with_stdio();

    string str;

    clog << "# Testing basic I/O\n";

    {
        // test formatted output
        //
        // This should read the strings on stdin and print them on stdout
        // prefixed by "STDOUT: "

        opstream os("/bin/sed 's/^./STDIN: &/' - /etc/issue");
        os << ".fnord.\n";
        str = "..fnord..\n";
        os << str;
        check_pass(os);
    }

    {
        // test execve() style construction
        //
        // This should read the strings on stdin and print them on stdout
        // prefixed by "STDIN: "

        vector<string> argv;
        argv.push_back("sed");
        argv.push_back("s/^./STDIN: &/");
        opstream os("sed", argv);

        check_pass(os << "Magic Monkey\n");
    }

    {
        // test unformatted output
        //
        // This should read the strings on stdin and print them on stdout
        // prefixed by "STDIN: "

        opstream sed("/bin/sed 's/^./STDIN: &/'");
        str = "Monkey Magic\n";
        for (string::const_iterator i = str.begin(); i!=str.end(); ++i)
            sed.put(*i);
        check_pass(sed);
    }

    {
        // test formatted input
        // should print hostname on stdout, prefixed by "STDOUT: "
        ipstream host("hostname");
        if (getline(host, str))  // extracts up to newline, eats newline
            cout << "STDOUT: " << str << endl;
        check_pass(host);
        // check we hit EOF at next read
        char c;
        print_result(host, !host.get(c));
        print_result(host, host.eof());
        check_fail(host);
    }

    {
        // test unformatted input
        // should print hostname on stdout, prefixed by "STDOUT: "
        ipstream host("date");
        str.clear();
        char c;
        while (host.get(c))  // extracts up to EOF (including newline)
            str += c;
        cout << "STDOUT:  " << str << flush;
        print_result(host, host.eof());
    }

    {
        // open after construction, then write
        opstream os;
        os.open("/bin/sed 's/^./STDIN: &/'");
        os << "Hello, world!\n";
        check_pass(os);
    }

    {
        // open after construction, then read
        ipstream is;
        is.open("hostname");
        string s;
        is >> s;
        cout << "STDOUT: " << s << endl;
        check_pass(is);
    }

    {
        // open after construction, then write
        ipstream host;
        host.open("hostname");
        if (host >> str)
            cout << "STDOUT: " << str << endl;
        check_pass(host);
        // chomp newline and try to read past end
        char c;
        host.get(c);
        host.get(c);
        check_fail(host);
    }

    clog << "# Testing bidirectional PStreams\n";

    pstreambuf::pmode all3streams =
        pstreambuf::pstdin|pstreambuf::pstdout|pstreambuf::pstderr;

    {
        // test output on bidirectional pstream
        string cmd = "grep ^127 -- /etc/hosts /no/such/file";
        pstream ps(cmd, all3streams);

        print_result(ps, ps.is_open());
        print_result(ps, ps.out());
        print_result(ps, ps.err());

        string buf;
        while (getline(ps.out(), buf))
            cout << "STDOUT: " << buf << endl;
        check_fail(ps);
        while (getline(ps.err(), buf))
            cout << "STDERR: " << buf << endl;
        check_fail(ps);
    }

    {
        // test input on bidirectional pstream and test peof manip
        string cmd = "grep fnord -- -";
        pstream ps(cmd, all3streams);

        print_result(ps, ps.is_open());
        print_result(ps, ps);

        ps << "12345\nfnord\n0000" << peof;
        // manip calls static_cast<pstreambuf*>(ps.rdbuf())->peof();

        string buf;
        getline(ps.out(), buf);

        do
        {
            print_result(ps, buf == "fnord");
            cout << "STDOUT: " << buf << endl;
        } while (getline(ps.out(), buf));

        print_result(ps, !(ps << "pipe closed, no fnord now"));
        check_fail(ps);
    }
    // TODO - test child moves onto next file after peof on stdin

    {
        // test signals
        string cmd = "grep 127 -- -";
        pstream ps(cmd, all3streams);

        pstreambuf* pbuf = static_cast<pstreambuf*>(ps.rdbuf());

        int e1 = pbuf->error();
        print_result(ps, e1 == 0);
        pbuf->kill(SIGTERM);
        int e2 = pbuf->error();
        print_result(ps, e1 == e2);

        pbuf->close();
        int e3 = pbuf->error();
        print_result(ps, !(ps << "127 fail 127\n"));
        print_result(ps, e1 == e3);
    }

    clog << "# Testing behaviour with bad commands" << endl;

    //string badcmd = "hgfhdgf";
    string badcmd = "hgfhdgf 2>/dev/null";

    {
        // check eof() works
        ipstream is(badcmd);
        print_result(is, is.get()==EOF);
        print_result(is, is.eof() );
    }

    {
        // test writing to bad command
        opstream ofail(badcmd);
        sleep(1);  // give shell time to try command and exit
        // this would cause SIGPIPE: ofail<<"blahblah";
        print_result(ofail, !ofail.is_open());
        // FAIL !!!
        // no way to tell if command fails,
    }

    {
        // reading from bad cmd
        vector<string> argv;
        argv.push_back("hdhdhd");
        argv.push_back("arg1");
        argv.push_back("arg2");
        ipstream ifail("hdhdhd", argv);
        check_fail(ifail>>str);
    }

    clog << "# Testing behaviour with uninit'ed streams" << endl;

    {
        // check eof() works
        ipstream is;
        print_result(is, is.get()==EOF);
        print_result(is, is.eof() );
    }

    {
        // test writing to no command
        opstream ofail;
        check_fail(ofail<<"blahblah");
    }

    clog << "# Testing other member functions\n";

    {
        string cmd("grep re");
        opstream s(cmd);
        print_result(s, cmd == s.command());
    }

    {
        string cmd("grep re");
        opstream s;
        s.open(cmd);
        print_result(s, cmd == s.command());
    }

    {
        string cmd("/bin/ls");
        ipstream s(cmd);
        print_result(s, cmd == s.command());
    }

    {
        string cmd("/bin/ls");
        ipstream s;
        s.open(cmd);
        print_result(s, cmd == s.command());
    }

    // TODO more testing of other members

    clog << "# Testing writing to closed stream\n";

    {
        opstream os("sed 's/foo/FOO/'");
        os << "foo\n";
        os.close();
        if (os << "bar\n")
            cout << "Wrote to closed stream" << endl;
        print_result(os, !(os << "bar\n"));
    }

#if REDI_EVISCERATE_PSTREAMS
    clog << "# Testing eviscerated pstream\n";

    {
        opstream os("sed 's/.*/FNORD/'");
        FILE *in, *out, *err;
        size_t res = os.fopen(in, out, err);
        print_result(os, res & pstreambuf::pstdin);
        print_result(os, in!=NULL);
        int i = fputs("flax\n", in);
        fflush(in);
        print_result(os, i>=0 && i!=EOF);
    }

    {
        string cmd = "ls /etc/motd /no/such/file";
        ipstream is(cmd, pstreambuf::pstdout|pstreambuf::pstderr);
        FILE *in, *out, *err;
        size_t res = is.fopen(in, out, err);
        print_result(is, res & pstreambuf::pstdout);
        print_result(is, res & pstreambuf::pstderr);
        print_result(is, out!=NULL);
        print_result(is, err!=NULL);

        size_t len = 256;
        char buf[len];
        char* p = fgets(buf, len, out);
        cout << "STDOUT: " << buf;
        print_result(is, p!=NULL);

        p = fgets(buf, len, err);
        cout << "STDERR: " << buf;
        print_result(is, p!=NULL);
    }

    {
        string cmd = "grep 127 -- - /etc/hosts /no/such/file";
        pstream ps(cmd, all3streams);
        FILE *in, *out, *err;
        size_t res = ps.fopen(in, out, err);
        print_result(ps, res & pstreambuf::pstdin);
        print_result(ps, res & pstreambuf::pstdout);
        print_result(ps, res & pstreambuf::pstderr);
        print_result(ps, in!=NULL);
        print_result(ps, out!=NULL);
        print_result(ps, err!=NULL);

        // ps << "12345\n1112777\n0000" << EOF;
#if 0
        size_t len = 256;
        char buf[len];
        char* p = fgets(buf, len, out);
        cout << "STDOUT: " << buf;
        print_result(ps, p!=NULL);

        p = fgets(buf, len, err);
        cout << "STDERR: " << buf;
        print_result(ps, p!=NULL);
#endif
    }

#endif

#if TEST_RPSTREAM
#endif

#if 0

#ifdef _STREAM_COMPAT

    // check fstreambase doesn't try to close() an attached fd
    FILE* f = fopen("/tmp/poo", "a");
    fprintf(f, "text\n");
    fflush(f);
    {
        fstream fs;
        fs.attach(fileno(f));
        fs << "more text" << endl;
    }
    fprintf(f, "even more text\n");
    fflush(f);
    cout << "retval of fclose(): " << fclose(f) << endl;

    // same check again, using only filedescs (no FILE*s)
    int fd = open("/tmp/oops", O_CREAT|O_WRONLY, 0777);
    clog << errno << ' ' << strerror(errno) << endl;
    errno=0;
    clog << write(fd, "TEXT\n", 5) << endl;
    clog << errno << ' ' << strerror(errno) << endl;
    errno=0;
    fsync(fd);
    {
        ofstream fs(fd);
        fs << "MORE TEXT" << endl;
        clog << strerror(errno) << endl;
        errno=0;
    }
    clog << write(fd, "EVEN MORE TEXT\n", 15) << endl;
    clog << strerror(errno) << endl;
    errno=0;
    fsync(fd);
    close(fd);
#endif  // _STREAM_COMPAT

#endif

    return 0;
}

<commit_msg>Test pstreambuf::wait(), fixes last failure.<commit_after>/*
PStreams - POSIX Process I/O for C++
Copyright (C) 2002 Jonathan Wakely

This file is part of PStreams.

PStreams is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of
the License, or (at your option) any later version.

PStreams 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with PStreams; if not, write to the Free Software Foundation, Inc.,
59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#define REDI_PSTREAMS_POPEN_USES_BIDIRECTIONAL_PIPE 1

//#include "pstream_compat.h"

// TODO test rpstream - this does nothing for the moment
#define TEST_RPSTREAM 1

// test eviscerated pstreams
#define REDI_EVISCERATE_PSTREAMS 1

#include "pstream.h"

#if TEST_RPSTREAM
#include "rpstream.h"
#endif

// include these after pstream.h to ensure it #includes everything it needs
#include <iostream>
#include <iomanip>
#include <string>
#include <sstream>
#include <fstream>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
//#include <fcntl.h>
#include <errno.h>

using namespace std;
using namespace redi;

namespace  // anon
{
    // helper functions for printing test results

    string
    test_type(const istream& s)
    { return "r"; }

    string
    test_type(const ostream& s)
    { return "w"; }

    string
    test_type(const iostream& s)
    { return "b"; }

    template <typename T>
    string
    test_id(const T& s)
    {
        static int count = 0;
        ostringstream buf;
        buf << test_type(s) << ++count;
        return buf.str();
    }

    template <typename T>
    void
    print_result(const T& s, bool result)
    {
        clog << "Test " << setw(4) << test_id(s) << ": "
            << (result ? "Pass" : "Fail!")
            << endl;
    }

    template <typename T>
    bool
    check_pass(const T& s, bool expected = true)
    {
        bool res = s.good() == expected;
        print_result(s, res);
        return res;
    }

    template <typename T>
    bool
    check_fail(const T& s) { return check_pass(s, false); }
}

int main()
{
    ios_base::sync_with_stdio();

    string str;

    clog << "# Testing basic I/O\n";

    {
        // test formatted output
        //
        // This should read the strings on stdin and print them on stdout
        // prefixed by "STDOUT: "

        opstream os("/bin/sed 's/^./STDIN: &/' - /etc/issue");
        os << ".fnord.\n";
        str = "..fnord..\n";
        os << str;
        check_pass(os);
    }

    {
        // test execve() style construction
        //
        // This should read the strings on stdin and print them on stdout
        // prefixed by "STDIN: "

        vector<string> argv;
        argv.push_back("sed");
        argv.push_back("s/^./STDIN: &/");
        opstream os("sed", argv);

        check_pass(os << "Magic Monkey\n");
    }

    {
        // test unformatted output
        //
        // This should read the strings on stdin and print them on stdout
        // prefixed by "STDIN: "

        opstream sed("/bin/sed 's/^./STDIN: &/'");
        str = "Monkey Magic\n";
        for (string::const_iterator i = str.begin(); i!=str.end(); ++i)
            sed.put(*i);
        check_pass(sed);
    }

    {
        // test formatted input
        // should print hostname on stdout, prefixed by "STDOUT: "
        ipstream host("hostname");
        if (getline(host, str))  // extracts up to newline, eats newline
            cout << "STDOUT: " << str << endl;
        check_pass(host);
        // check we hit EOF at next read
        char c;
        print_result(host, !host.get(c));
        print_result(host, host.eof());
        check_fail(host);
    }

    {
        // test unformatted input
        // should print hostname on stdout, prefixed by "STDOUT: "
        ipstream host("date");
        str.clear();
        char c;
        while (host.get(c))  // extracts up to EOF (including newline)
            str += c;
        cout << "STDOUT:  " << str << flush;
        print_result(host, host.eof());
    }

    {
        // open after construction, then write
        opstream os;
        os.open("/bin/sed 's/^./STDIN: &/'");
        os << "Hello, world!\n";
        check_pass(os);
    }

    {
        // open after construction, then read
        ipstream is;
        is.open("hostname");
        string s;
        is >> s;
        cout << "STDOUT: " << s << endl;
        check_pass(is);
    }

    {
        // open after construction, then write
        ipstream host;
        host.open("hostname");
        if (host >> str)
            cout << "STDOUT: " << str << endl;
        check_pass(host);
        // chomp newline and try to read past end
        char c;
        host.get(c);
        host.get(c);
        check_fail(host);
    }

    clog << "# Testing bidirectional PStreams\n";

    pstreambuf::pmode all3streams =
        pstreambuf::pstdin|pstreambuf::pstdout|pstreambuf::pstderr;

    {
        // test output on bidirectional pstream
        string cmd = "grep ^127 -- /etc/hosts /no/such/file";
        pstream ps(cmd, all3streams);

        print_result(ps, ps.is_open());
        print_result(ps, ps.out());
        print_result(ps, ps.err());

        string buf;
        while (getline(ps.out(), buf))
            cout << "STDOUT: " << buf << endl;
        check_fail(ps);
        while (getline(ps.err(), buf))
            cout << "STDERR: " << buf << endl;
        check_fail(ps);
    }

    {
        // test input on bidirectional pstream and test peof manip
        string cmd = "grep fnord -- -";
        pstream ps(cmd, all3streams);

        print_result(ps, ps.is_open());
        print_result(ps, ps);

        ps << "12345\nfnord\n0000" << peof;
        // manip calls static_cast<pstreambuf*>(ps.rdbuf())->peof();
        // safer to use ps.rdbuf()->peof() instead

        string buf;
        getline(ps.out(), buf);

        do
        {
            print_result(ps, buf == "fnord");
            cout << "STDOUT: " << buf << endl;
        } while (getline(ps.out(), buf));

        print_result(ps, !(ps << "pipe closed, no fnord now"));
        check_fail(ps);
    }
    // TODO - test child moves onto next file after peof on stdin

    {
        // test signals
        string cmd = "grep 127 -- -";
        pstream ps(cmd, all3streams);

        pstreambuf* pbuf = static_cast<pstreambuf*>(ps.rdbuf());

        int e1 = pbuf->error();
        print_result(ps, e1 == 0);
        pbuf->kill(SIGTERM);
        int e2 = pbuf->error();
        print_result(ps, e1 == e2);

        pbuf->close();
        int e3 = pbuf->error();
        print_result(ps, !(ps << "127 fail 127\n"));
        print_result(ps, e1 == e3);
    }

    clog << "# Testing behaviour with bad commands" << endl;

    //string badcmd = "hgfhdgf";
    string badcmd = "hgfhdgf 2>/dev/null";

    {
        // check eof() works
        ipstream is(badcmd);
        print_result(is, is.get()==EOF);
        print_result(is, is.eof() );
    }

    {
        // test writing to bad command
        opstream ofail(badcmd);
        sleep(1);  // give shell time to try command and exit
        // this would cause SIGPIPE: ofail<<"blahblah";
        // does not show failure: print_result(ofail, !ofail.is_open());
#if PSTREAMS_WAIT
        pstreambuf* buf = static_cast<pstreambuf*>(ofail.rdbuf());
        print_result(ofail, buf->exited());
        int status = buf->status();
        print_result(ofail, WIFEXITED(status) && WEXITSTATUS(status)==127);
#else
        // FAIL !!!
        // no way to tell if command fails,
#endif
    }

    {
        // reading from bad cmd
        vector<string> argv;
        argv.push_back("hdhdhd");
        argv.push_back("arg1");
        argv.push_back("arg2");
        ipstream ifail("hdhdhd", argv);
        check_fail(ifail>>str);
    }

    clog << "# Testing behaviour with uninit'ed streams" << endl;

    {
        // check eof() works
        ipstream is;
        print_result(is, is.get()==EOF);
        print_result(is, is.eof() );
    }

    {
        // test writing to no command
        opstream ofail;
        check_fail(ofail<<"blahblah");
    }

    clog << "# Testing other member functions\n";

    {
        string cmd("grep re");
        opstream s(cmd);
        print_result(s, cmd == s.command());
    }

    {
        string cmd("grep re");
        opstream s;
        s.open(cmd);
        print_result(s, cmd == s.command());
    }

    {
        string cmd("/bin/ls");
        ipstream s(cmd);
        print_result(s, cmd == s.command());
    }

    {
        string cmd("/bin/ls");
        ipstream s;
        s.open(cmd);
        print_result(s, cmd == s.command());
    }

    // TODO more testing of other members

    clog << "# Testing writing to closed stream\n";

    {
        opstream os("sed 's/foo/FOO/'");
        os << "foo\n";
        os.close();
        if (os << "bar\n")
            cout << "Wrote to closed stream" << endl;
        print_result(os, !(os << "bar\n"));
    }

#if REDI_EVISCERATE_PSTREAMS
    clog << "# Testing eviscerated pstream\n";

    {
        opstream os("sed 's/.*/FNORD/'");
        FILE *in, *out, *err;
        size_t res = os.fopen(in, out, err);
        print_result(os, res & pstreambuf::pstdin);
        print_result(os, in!=NULL);
        int i = fputs("flax\n", in);
        fflush(in);
        print_result(os, i>=0 && i!=EOF);
    }

    {
        string cmd = "ls /etc/motd /no/such/file";
        ipstream is(cmd, pstreambuf::pstdout|pstreambuf::pstderr);
        FILE *in, *out, *err;
        size_t res = is.fopen(in, out, err);
        print_result(is, res & pstreambuf::pstdout);
        print_result(is, res & pstreambuf::pstderr);
        print_result(is, out!=NULL);
        print_result(is, err!=NULL);

        size_t len = 256;
        char buf[len];
        char* p = fgets(buf, len, out);
        cout << "STDOUT: " << buf;
        print_result(is, p!=NULL);

        p = fgets(buf, len, err);
        cout << "STDERR: " << buf;
        print_result(is, p!=NULL);
    }

    {
        string cmd = "grep 127 -- - /etc/hosts /no/such/file";
        pstream ps(cmd, all3streams);
        FILE *in, *out, *err;
        size_t res = ps.fopen(in, out, err);
        print_result(ps, res & pstreambuf::pstdin);
        print_result(ps, res & pstreambuf::pstdout);
        print_result(ps, res & pstreambuf::pstderr);
        print_result(ps, in!=NULL);
        print_result(ps, out!=NULL);
        print_result(ps, err!=NULL);

        // ps << "12345\n1112777\n0000" << EOF;
#if 0
        size_t len = 256;
        char buf[len];
        char* p = fgets(buf, len, out);
        cout << "STDOUT: " << buf;
        print_result(ps, p!=NULL);

        p = fgets(buf, len, err);
        cout << "STDERR: " << buf;
        print_result(ps, p!=NULL);
#endif
    }

#endif

#if TEST_RPSTREAM
#endif

#if 0

#ifdef _STREAM_COMPAT

    // check fstreambase doesn't try to close() an attached fd
    FILE* f = fopen("/tmp/poo", "a");
    fprintf(f, "text\n");
    fflush(f);
    {
        fstream fs;
        fs.attach(fileno(f));
        fs << "more text" << endl;
    }
    fprintf(f, "even more text\n");
    fflush(f);
    cout << "retval of fclose(): " << fclose(f) << endl;

    // same check again, using only filedescs (no FILE*s)
    int fd = open("/tmp/oops", O_CREAT|O_WRONLY, 0777);
    clog << errno << ' ' << strerror(errno) << endl;
    errno=0;
    clog << write(fd, "TEXT\n", 5) << endl;
    clog << errno << ' ' << strerror(errno) << endl;
    errno=0;
    fsync(fd);
    {
        ofstream fs(fd);
        fs << "MORE TEXT" << endl;
        clog << strerror(errno) << endl;
        errno=0;
    }
    clog << write(fd, "EVEN MORE TEXT\n", 15) << endl;
    clog << strerror(errno) << endl;
    errno=0;
    fsync(fd);
    close(fd);
#endif  // _STREAM_COMPAT

#endif

    return 0;
}

<|endoftext|>


<commit_before>/** @copyright
 * Copyright (c) 2017 Stuart W Baker
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @file MCP2515Can.cxx
 * This file implements the CAN driver for the MCP2515 CAN Controller.
 *
 * @author Stuart W. Baker
 * @date 3 January 2017
 */

#include "MCP2515Can.hxx"

#include <fcntl.h>

const MCP2515Can::MCP2515Baud MCP2515Can::baudTable[] =
{
    /* 20 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 5) / 20 MHz = 500 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 16 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 7 TQ
     * PS1 = 4 TQ
     * PS2 = 4 TQ
     * sample time = (1 TQ + 7 TQ + 4 TQ) / 16 TQ = 75%
     * SJW = PS2 - 1 = 4 - 1 = 3
     * SJW = 3 * 500 nsec = 1.5 usec
     *
     * Oscillator Tolerance:
     *     4 / (2 * ((13 * 16) - 4)) = 0.980%
     *     3 / (20 * 16) = 0.938%
     *     = 0.938%
     */
    {20000000, 125000, {(5 - 1), (3 - 1)},
                       {(7 - 1), (4 - 1), 0, 1},
                       {(4 - 1), 0, 0}},
    /* 8 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 2) / 8 MHz = 500 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 16 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 7 TQ
     * PS1 = 4 TQ
     * PS2 = 4 TQ
     * sample time = (1 TQ + 7 TQ + 4 TQ) / 16 TQ = 75%
     * SJW = PS2 - 1 = 4 - 1 = 3
     * SJW = 3 * 500 nsec = 1.5 usec
     *
     * Oscillator Tolerance:
     *     4 / (2 * ((13 * 16) - 4)) = 0.980%
     *     3 / (20 * 16) = 0.938%
     *     = 0.938%
     */
    {8000000, 125000, {(2 - 1), (3 - 1)},
                      {(7 - 1), (4 - 1), 0, 1},
                      {(4 - 1), 0, 0}}
};

/*
 * init()
 */
void MCP2515Can::init(const char *spi_name, uint32_t freq, uint32_t baud)
{
    spi = ::open(spi_name, O_RDWR);
    HASSERT(spi >= 0);

    /* configure SPI bus settings */
    uint8_t spi_mode = SPI_MODE_0;
    uint8_t spi_bpw = 8;
    uint32_t spi_max_speed_hz = freq / 4;
    ::ioctl(spi, SPI_IOC_WR_MODE, &spi_mode);
    ::ioctl(spi, SPI_IOC_WR_BITS_PER_WORD, &spi_bpw);
    ::ioctl(spi, SPI_IOC_WR_MAX_SPEED_HZ, &spi_max_speed_hz);

    /* reset device */
    reset();

    /* wait until device is in configuration mode */
    while ((register_read(CANSTAT) & 0xE0) != 0x80);

    /* find valid timing settings for the requested frequency and buad rates */
    for (size_t i = 0; i < (sizeof(baudTable) / sizeof(baudTable[0])); ++i)
    {
        if (baudTable[i].freq == freq && baudTable[i].baud == baud)
        {
            register_write(CNF1, baudTable[i].cnf1.data);
            register_write(CNF2, baudTable[i].cnf2.data);
            register_write(CNF3, baudTable[i].cnf3.data);

            /* setup RX Buf 0 and 1 to receive any message */
            register_write(RXB0CTRL, 0x60);
            register_write(RXB1CTRL, 0x60);

            /* put the device into normal operation mode */
            register_write(CANCTRL, 0x00);

            /* wait until device is in normal mode */
            while ((register_read(CANSTAT) & 0xE0) != 0x00);

            return;
        }
    }

    /* unsupported frequency */
    HASSERT(0);
}

/*
 * enable()
 */
void MCP2515Can::enable()
{
    /* there is a mutex lock above us, so the following sequence is atomic */
    if (!is_created())
    {
        /* start the thread at the highest priority in the system */
        /** @todo make this the highest possible thread priority */
        start(name, configMAX_PRIORITIES - 1, 1024);
    }

    /* clear interrupt flags */
    register_write(CANINTF, 0);

    /* enable error and receive interrupts */
    register_write(CANINTE, MERR | ERRI | RX1I | RX0I);

    interrupt_enable();
}

/*
 * disable()
 */
void MCP2515Can::disable()
{
    interrupt_disable();

    /* disable all interrupt sources */
    register_write(CANINTE, 0);

    register_write(TXB0CTRL, 0x00);
    register_write(TXB1CTRL, 0x00);
    portENTER_CRITICAL();
    /* flush out any transmit data in the pipleline */
    txBuf->flush();
    txPending = 0;
    portEXIT_CRITICAL();
}

/*
 * tx_msg_locked()
 */
void MCP2515Can::tx_msg_locked()
{
    /* the node lock_ will be locked by the caller */
    if (txPending < 3)
    {
        struct can_frame *can_frame;

        /* find an empty buffer */
        int index = 0;
        if (txPending & 0x1)
        {
            /* buffer 0 already in use */
            index = 1;
        }

        portENTER_CRITICAL();
        if (txBuf->data_read_pointer(&can_frame))
        {
            Buffer tx_buf(can_frame);
            txBuf->consume(1);
            txBuf->signal_condition();
            portEXIT_CRITICAL();

            txPending |= (0x1 << index);

            /* bump up priority of the other buffer so it will transmit first
             * if it is pending
             */
            bit_modify(index == 0 ? TXB1CTRL : TXB0CTRL, 0x01, 0x03);
            /* load the tranmsit buffer */
            buffer_write(index, tx_buf.get_payload());
            /* request to send at lowest priority */
            bit_modify(index == 0 ? TXB0CTRL : TXB1CTRL, 0x08, 0x0B);
            /* enable transmit interrupt */
            bit_modify(CANINTE, TX0I << index, TX0I << index);
        }
        else
        {
            portEXIT_CRITICAL();
        }
    }
}

/*
 * rx_msg()
 */
void MCP2515Can::rx_msg(int index)
{
    /* Read the rx buffer.  The RXnIF flag is automatically cleared at the
     * end of the SPI transaction
     */
    Buffer rx_buf;
    buffer_read(index, rx_buf.get_payload());
    struct can_frame *can_frame;

    portENTER_CRITICAL();
    if (rxBuf->data_write_pointer(&can_frame))
    {
        rx_buf.build_struct_can_frame(can_frame);
        rxBuf->advance(1);
        ++numReceivedPackets_;
        rxBuf->signal_condition();
    }
    else
    {
        /* receive overrun occured */
        ++overrunCount;
    }
    portEXIT_CRITICAL();
}

/*
 * entry()
 */
void *MCP2515Can::entry()
{
    for ( ; /* forever */ ; )
    {
#if MCP2515_DEBUG
        int result = sem.timedwait(SEC_TO_NSEC(1));
        lock_.lock();

        if (result != 0)
        {
            spi_ioc_transfer xfer[2];
            memset(xfer, 0, sizeof(xfer));
            uint8_t wr_data[2] = {READ, 0};
            xfer[0].tx_buf = (unsigned long)wr_data;
            xfer[0].len = sizeof(wr_data);
            xfer[1].rx_buf = (unsigned long)regs;
            xfer[1].len = sizeof(regs);
            xfer[1].cs_change = 1;
            ::ioctl(spi, SPI_IOC_MESSAGE(2), xfer);
            lock_.unlock();
            continue;
        }
#else
        sem.wait();
        lock_.lock();
#endif
        /* read status flags */
        uint8_t canintf = register_read(CANINTF);

        if (canintf & MERR)
        {
            /* message error interrupt active */

            /* clear message error status flag */
            bit_modify(CANINTF, 0, MERR);

        }
        if (canintf & ERRI)
        {
            /* error handling, read error flag register */
            uint8_t eflg = register_read(EFLG);

            /* clear error status flag */
            bit_modify(CANINTF, 0, ERRI);

            if (eflg & (RX0OVR | RX1OVR))
            {
                /* receive overrun */
                ++overrunCount;

                /* clear error flag */
                bit_modify(EFLG, 0, (RX0OVR | RX1OVR));
            }
            if (eflg & TXBO)
            {
                /* bus off */
                ++busOffCount;
            }
            if (eflg & (TXEP || RXEP))
            {
                /* error active state */
                ++softErrorCount;

                /* flush out any transmit data in the pipleline */
                register_write(TXB0CTRL, 0x00);
                register_write(TXB1CTRL, 0x00);
                bit_modify(CANINTE, 0, TX0I | TX1I);
                bit_modify(CANINTF, 0, TX0I | TX1I);

                portENTER_CRITICAL();
                txBuf->flush();
                txBuf->signal_condition();
                portEXIT_CRITICAL();

                txPending = 0;
            }
        }
        if (canintf & RX0I)
        {
            /* receive interrupt active */
            rx_msg(0);
        }

        if (txPending)
        {
            /* transmit interrupt active and transmission complete */
            if (canintf & TX0I)
            {
                txPending &= ~0x1;
                bit_modify(CANINTE, 0, TX0I);
                bit_modify(CANINTF, 0, TX0I);
                ++numTransmittedPackets_;
            }
            if (canintf & TX1I)
            {
                txPending &= ~0x2;
                bit_modify(CANINTE, 0, TX1I);
                bit_modify(CANINTF, 0, TX1I);
                ++numTransmittedPackets_;
            }

            tx_msg_locked();
        }

        /* Refresh status flags just in case RX1 buffer became active
         * before we could finish reading out RX0 buffer.  This ussually
         * won't happen because we should be able to respond to incoming
         * messages fast enough to only use RX0 buffer.
         */
        canintf = register_read(CANINTF);

        if (canintf & RX1I)
        {
            /* receive interrupt active */
            rx_msg(1);
        }
        lock_.unlock();

        interrupt_enable();
    }

    return NULL;
}

/*
 * interrupt_handler()
 */
void MCP2515Can::interrupt_handler()
{
    int woken = false;
    interrupt_disable();
    sem.post_from_isr(&woken);
    os_isr_exit_yield_test(woken);
}
<commit_msg>Code simplification<commit_after>/** @copyright
 * Copyright (c) 2017 Stuart W Baker
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @file MCP2515Can.cxx
 * This file implements the CAN driver for the MCP2515 CAN Controller.
 *
 * @author Stuart W. Baker
 * @date 3 January 2017
 */

#include "MCP2515Can.hxx"

#include <fcntl.h>

const MCP2515Can::MCP2515Baud MCP2515Can::baudTable[] =
{
    /* 20 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 5) / 20 MHz = 500 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 16 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 7 TQ
     * PS1 = 4 TQ
     * PS2 = 4 TQ
     * sample time = (1 TQ + 7 TQ + 4 TQ) / 16 TQ = 75%
     * SJW = PS2 - 1 = 4 - 1 = 3
     * SJW = 3 * 500 nsec = 1.5 usec
     *
     * Oscillator Tolerance:
     *     4 / (2 * ((13 * 16) - 4)) = 0.980%
     *     3 / (20 * 16) = 0.938%
     *     = 0.938%
     */
    {20000000, 125000, {(5 - 1), (3 - 1)},
                       {(7 - 1), (4 - 1), 0, 1},
                       {(4 - 1), 0, 0}},
    /* 8 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 2) / 8 MHz = 500 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 16 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 7 TQ
     * PS1 = 4 TQ
     * PS2 = 4 TQ
     * sample time = (1 TQ + 7 TQ + 4 TQ) / 16 TQ = 75%
     * SJW = PS2 - 1 = 4 - 1 = 3
     * SJW = 3 * 500 nsec = 1.5 usec
     *
     * Oscillator Tolerance:
     *     4 / (2 * ((13 * 16) - 4)) = 0.980%
     *     3 / (20 * 16) = 0.938%
     *     = 0.938%
     */
    {8000000, 125000, {(2 - 1), (3 - 1)},
                      {(7 - 1), (4 - 1), 0, 1},
                      {(4 - 1), 0, 0}}
};

/*
 * init()
 */
void MCP2515Can::init(const char *spi_name, uint32_t freq, uint32_t baud)
{
    spi = ::open(spi_name, O_RDWR);
    HASSERT(spi >= 0);

    /* configure SPI bus settings */
    uint8_t spi_mode = SPI_MODE_0;
    uint8_t spi_bpw = 8;
    uint32_t spi_max_speed_hz = freq / 4;
    ::ioctl(spi, SPI_IOC_WR_MODE, &spi_mode);
    ::ioctl(spi, SPI_IOC_WR_BITS_PER_WORD, &spi_bpw);
    ::ioctl(spi, SPI_IOC_WR_MAX_SPEED_HZ, &spi_max_speed_hz);

    /* reset device */
    reset();

    /* wait until device is in configuration mode */
    while ((register_read(CANSTAT) & 0xE0) != 0x80);

    /* find valid timing settings for the requested frequency and buad rates */
    for (size_t i = 0; i < (sizeof(baudTable) / sizeof(baudTable[0])); ++i)
    {
        if (baudTable[i].freq == freq && baudTable[i].baud == baud)
        {
            register_write(CNF1, baudTable[i].cnf1.data);
            register_write(CNF2, baudTable[i].cnf2.data);
            register_write(CNF3, baudTable[i].cnf3.data);

            /* setup RX Buf 0 and 1 to receive any message */
            register_write(RXB0CTRL, 0x60);
            register_write(RXB1CTRL, 0x60);

            /* put the device into normal operation mode */
            register_write(CANCTRL, 0x00);

            /* wait until device is in normal mode */
            while ((register_read(CANSTAT) & 0xE0) != 0x00);

            return;
        }
    }

    /* unsupported frequency */
    HASSERT(0);
}

/*
 * enable()
 */
void MCP2515Can::enable()
{
    /* there is a mutex lock above us, so the following sequence is atomic */
    if (!is_created())
    {
        /* start the thread at the highest priority in the system */
        /** @todo make this the highest possible thread priority */
        start(name, configMAX_PRIORITIES - 1, 1024);
    }

    /* clear interrupt flags */
    register_write(CANINTF, 0);

    /* enable error and receive interrupts */
    register_write(CANINTE, MERR | ERRI | RX1I | RX0I);

    interrupt_enable();
}

/*
 * disable()
 */
void MCP2515Can::disable()
{
    interrupt_disable();

    /* disable all interrupt sources */
    register_write(CANINTE, 0);

    register_write(TXB0CTRL, 0x00);
    register_write(TXB1CTRL, 0x00);
    portENTER_CRITICAL();
    /* flush out any transmit data in the pipleline */
    txBuf->flush();
    txPending = 0;
    portEXIT_CRITICAL();
}

/*
 * tx_msg_locked()
 */
void MCP2515Can::tx_msg_locked()
{
    /* the node lock_ will be locked by the caller */
    if (txPending < 3)
    {
        struct can_frame *can_frame;

        /* find an empty buffer */
        int index = (txPending & 0x1) ? 1 : 0;

        portENTER_CRITICAL();
        if (txBuf->data_read_pointer(&can_frame))
        {
            Buffer tx_buf(can_frame);
            txBuf->consume(1);
            txBuf->signal_condition();
            portEXIT_CRITICAL();

            txPending |= (0x1 << index);

            /* bump up priority of the other buffer so it will transmit first
             * if it is pending
             */
            bit_modify(index == 0 ? TXB1CTRL : TXB0CTRL, 0x01, 0x03);
            /* load the tranmsit buffer */
            buffer_write(index, tx_buf.get_payload());
            /* request to send at lowest priority */
            bit_modify(index == 0 ? TXB0CTRL : TXB1CTRL, 0x08, 0x0B);
            /* enable transmit interrupt */
            bit_modify(CANINTE, TX0I << index, TX0I << index);
        }
        else
        {
            portEXIT_CRITICAL();
        }
    }
}

/*
 * rx_msg()
 */
void MCP2515Can::rx_msg(int index)
{
    /* Read the rx buffer.  The RXnIF flag is automatically cleared at the
     * end of the SPI transaction
     */
    Buffer rx_buf;
    buffer_read(index, rx_buf.get_payload());
    struct can_frame *can_frame;

    portENTER_CRITICAL();
    if (rxBuf->data_write_pointer(&can_frame))
    {
        rx_buf.build_struct_can_frame(can_frame);
        rxBuf->advance(1);
        ++numReceivedPackets_;
        rxBuf->signal_condition();
    }
    else
    {
        /* receive overrun occured */
        ++overrunCount;
    }
    portEXIT_CRITICAL();
}

/*
 * entry()
 */
void *MCP2515Can::entry()
{
    for ( ; /* forever */ ; )
    {
#if MCP2515_DEBUG
        int result = sem.timedwait(SEC_TO_NSEC(1));
        lock_.lock();

        if (result != 0)
        {
            spi_ioc_transfer xfer[2];
            memset(xfer, 0, sizeof(xfer));
            uint8_t wr_data[2] = {READ, 0};
            xfer[0].tx_buf = (unsigned long)wr_data;
            xfer[0].len = sizeof(wr_data);
            xfer[1].rx_buf = (unsigned long)regs;
            xfer[1].len = sizeof(regs);
            xfer[1].cs_change = 1;
            ::ioctl(spi, SPI_IOC_MESSAGE(2), xfer);
            lock_.unlock();
            continue;
        }
#else
        sem.wait();
        lock_.lock();
#endif
        /* read status flags */
        uint8_t canintf = register_read(CANINTF);

        if (canintf & MERR)
        {
            /* message error interrupt active */

            /* clear message error status flag */
            bit_modify(CANINTF, 0, MERR);

        }
        if (canintf & ERRI)
        {
            /* error handling, read error flag register */
            uint8_t eflg = register_read(EFLG);

            /* clear error status flag */
            bit_modify(CANINTF, 0, ERRI);

            if (eflg & (RX0OVR | RX1OVR))
            {
                /* receive overrun */
                ++overrunCount;

                /* clear error flag */
                bit_modify(EFLG, 0, (RX0OVR | RX1OVR));
            }
            if (eflg & TXBO)
            {
                /* bus off */
                ++busOffCount;
            }
            if (eflg & (TXEP || RXEP))
            {
                /* error active state */
                ++softErrorCount;

                /* flush out any transmit data in the pipleline */
                register_write(TXB0CTRL, 0x00);
                register_write(TXB1CTRL, 0x00);
                bit_modify(CANINTE, 0, TX0I | TX1I);
                bit_modify(CANINTF, 0, TX0I | TX1I);

                portENTER_CRITICAL();
                txBuf->flush();
                txBuf->signal_condition();
                portEXIT_CRITICAL();

                txPending = 0;
            }
        }
        if (canintf & RX0I)
        {
            /* receive interrupt active */
            rx_msg(0);
        }

        if (txPending)
        {
            /* transmit interrupt active and transmission complete */
            if (canintf & TX0I)
            {
                txPending &= ~0x1;
                bit_modify(CANINTE, 0, TX0I);
                bit_modify(CANINTF, 0, TX0I);
                ++numTransmittedPackets_;
            }
            if (canintf & TX1I)
            {
                txPending &= ~0x2;
                bit_modify(CANINTE, 0, TX1I);
                bit_modify(CANINTF, 0, TX1I);
                ++numTransmittedPackets_;
            }

            tx_msg_locked();
        }

        /* Refresh status flags just in case RX1 buffer became active
         * before we could finish reading out RX0 buffer.  This ussually
         * won't happen because we should be able to respond to incoming
         * messages fast enough to only use RX0 buffer.
         */
        canintf = register_read(CANINTF);

        if (canintf & RX1I)
        {
            /* receive interrupt active */
            rx_msg(1);
        }
        lock_.unlock();

        interrupt_enable();
    }

    return NULL;
}

/*
 * interrupt_handler()
 */
void MCP2515Can::interrupt_handler()
{
    int woken = false;
    interrupt_disable();
    sem.post_from_isr(&woken);
    os_isr_exit_yield_test(woken);
}
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

#include "modules/prediction/scenario/scenario_manager.h"

#include <algorithm>
#include <cmath>

#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/math/vec2d.h"
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/container/container_manager.h"

namespace apollo {
namespace prediction {

using common::adapter::AdapterConfig;
using common::math::Vec2d;
using common::math::Box2d;
using apollo::perception::PerceptionObstacle;

ScenarioManager::ScenarioManager() {}

void ScenarioManager::Run() {
  auto environment_features = FeatureExtractor::ExtractEnvironmentFeatures();

  auto ptr_scenario_features = ScenarioAnalyzer::Analyze(environment_features);

  current_scenario_ = ptr_scenario_features->scenario();

  if (FLAGS_enable_prioritize_obstacles) {
    PrioritizeObstacles(environment_features, ptr_scenario_features);
  }
  // TODO(all) other functionalities including lane, junction filters
}

const Scenario& ScenarioManager::scenario() const { return current_scenario_; }

void ScenarioManager::PrioritizeObstacles(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<ScenarioFeatures> ptr_scenario_features) {
  auto obstacles_container = ContainerManager::Instance()->GetContainer<
      ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);

  if (obstacles_container == nullptr) {
    AERROR << "Obstacles container pointer is a null pointer.";
    return;
  }

  auto pose_container = ContainerManager::Instance()->GetContainer<
      PoseContainer>(AdapterConfig::LOCALIZATION);
  if (pose_container == nullptr) {
    AERROR << "Pose container pointer is a null pointer.";
    return;
  }

  const PerceptionObstacle* pose_obstacle_ptr =
      pose_container->ToPerceptionObstacle();
  if (pose_container == nullptr) {
    AERROR << "Pose obstacle pointer is a null pointer.";
    return;
  }

  double pose_theta = pose_obstacle_ptr->theta();
  double pose_x = pose_obstacle_ptr->position().x();
  double pose_y = pose_obstacle_ptr->position().y();

  ADEBUG << "Get pose (" << pose_x << ", " << pose_y
         << ", " << pose_theta << ")";

  // Build rectangular scan_area
  Box2d scan_box({pose_x + FLAGS_scan_length / 2.0 * std::cos(pose_theta),
                  pose_y + FLAGS_scan_length / 2.0 * std::sin(pose_theta)},
                  pose_theta, FLAGS_scan_length, FLAGS_scan_width);

  const auto& scenario_type = ptr_scenario_features->scenario().type();
  const auto& obstacle_ids =
      obstacles_container->GetCurrentFramePredictableObstacleIds();

  for (const int& obstacle_id : obstacle_ids) {
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id);
    if (obstacle_ptr->history_size() == 0) {
      AERROR << "Obstacle [" << obstacle_ptr->id() << "] has no feature.";
      continue;
    }
    Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
    double obstacle_x = latest_feature_ptr->position().x();
    double obstacle_y = latest_feature_ptr->position().y();
    Vec2d ego_to_obstacle_vec(obstacle_x - pose_x, obstacle_y - pose_y);
    Vec2d ego_vec = Vec2d::CreateUnitVec2d(pose_theta);
    double s = ego_to_obstacle_vec.InnerProd(ego_vec);

    // Decide if we need consider this obstacle
    bool is_in_scan_area = scan_box.IsPointIn({obstacle_x, obstacle_y});
    bool is_on_lane = obstacle_ptr->IsOnLane();
    bool is_pedestrian_in_front = s > -2.0 &&
        latest_feature_ptr->type() == PerceptionObstacle::PEDESTRIAN;

    bool need_consider = is_in_scan_area || is_on_lane ||
                         is_pedestrian_in_front;

    if (scenario_type == Scenario::JUNCTION ||
        scenario_type == Scenario::JUNCTION_TRAFFIC_LIGHT ||
        scenario_type == Scenario::JUNCTION_STOP_SIGN) {
      bool is_in_junction = false;  // TODO(all) update
      need_consider = need_consider || is_in_junction;
    }

    if (!need_consider) {
      latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::IGNORE);
    } else {
      latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::NORMAL);
    }
  }
}

void ScenarioManager::PrioritizeObstaclesForCruiseScenario(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<CruiseScenarioFeatures> cruise_scenario_features,
    ObstaclesContainer* ptr_obstacle_contrainer) {
  const auto& obstacle_ids =
      ptr_obstacle_contrainer->GetCurrentFramePredictableObstacleIds();

  for (const int& obstacle_id : obstacle_ids) {
    Obstacle* obstacle_ptr = ptr_obstacle_contrainer->GetObstacle(obstacle_id);
    if (obstacle_ptr->history_size() == 0) {
      AERROR << "Obstacle [" << obstacle_ptr->id() << "] has no feature.";
      continue;
    }
    Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
    if (obstacle_ptr->IsOnLane()) {
      bool has_lane_of_interest = false;
      for (const auto& curr_lane :
           latest_feature_ptr->lane().current_lane_feature()) {
        const auto& curr_lane_id = curr_lane.lane_id();
        if (cruise_scenario_features->IsLaneOfInterest(curr_lane_id)) {
          has_lane_of_interest = true;
          break;
        }
      }
      if (!has_lane_of_interest) {
        for (const auto& nearby_lane :
             latest_feature_ptr->lane().nearby_lane_feature()) {
          const auto& nearby_lane_id = nearby_lane.lane_id();
          if (cruise_scenario_features->IsLaneOfInterest(nearby_lane_id)) {
            has_lane_of_interest = true;
            break;
          }
        }
      }
      if (!has_lane_of_interest) {
        latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::IGNORE);
      }
    } else {
      // Obstacle has neither lane nor neighbor lanes
      // Filter obstacle by its relative position to ego vehicle
      double ego_x = environment_features.get_ego_position().x();
      double ego_y = environment_features.get_ego_position().y();
      double ego_heading = environment_features.get_ego_heading();
      double ego_speed = environment_features.get_ego_speed();
      double obstacle_x = latest_feature_ptr->position().x();
      double obstacle_y = latest_feature_ptr->position().y();
      Vec2d ego_to_obstacle_vec(obstacle_x - ego_x, obstacle_y - ego_y);
      Vec2d ego_vec = Vec2d::CreateUnitVec2d(ego_heading);
      double l = ego_vec.CrossProd(ego_to_obstacle_vec);
      double s = ego_to_obstacle_vec.InnerProd(ego_vec);
      if (std::abs(l) > 10.0 || s > std::max(20.0, ego_speed * 5.0) ||
          s < 0.0) {
        latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::IGNORE);
      }
    }
  }
}

}  // namespace prediction
}  // namespace apollo
<commit_msg>Prediction: Prioritize obstacles in junction<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

#include "modules/prediction/scenario/scenario_manager.h"

#include <algorithm>
#include <cmath>

#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/math/vec2d.h"
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/container/container_manager.h"

namespace apollo {
namespace prediction {

using common::adapter::AdapterConfig;
using common::math::Vec2d;
using common::math::Box2d;
using apollo::perception::PerceptionObstacle;

ScenarioManager::ScenarioManager() {}

void ScenarioManager::Run() {
  auto environment_features = FeatureExtractor::ExtractEnvironmentFeatures();

  auto ptr_scenario_features = ScenarioAnalyzer::Analyze(environment_features);

  current_scenario_ = ptr_scenario_features->scenario();

  if (FLAGS_enable_prioritize_obstacles) {
    PrioritizeObstacles(environment_features, ptr_scenario_features);
  }
  // TODO(all) other functionalities including lane, junction filters
}

const Scenario& ScenarioManager::scenario() const { return current_scenario_; }

void ScenarioManager::PrioritizeObstacles(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<ScenarioFeatures> ptr_scenario_features) {
  auto obstacles_container = ContainerManager::Instance()->GetContainer<
      ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);

  if (obstacles_container == nullptr) {
    AERROR << "Obstacles container pointer is a null pointer.";
    return;
  }

  auto pose_container = ContainerManager::Instance()->GetContainer<
      PoseContainer>(AdapterConfig::LOCALIZATION);
  if (pose_container == nullptr) {
    AERROR << "Pose container pointer is a null pointer.";
    return;
  }

  const PerceptionObstacle* pose_obstacle_ptr =
      pose_container->ToPerceptionObstacle();
  if (pose_container == nullptr) {
    AERROR << "Pose obstacle pointer is a null pointer.";
    return;
  }

  double pose_theta = pose_obstacle_ptr->theta();
  double pose_x = pose_obstacle_ptr->position().x();
  double pose_y = pose_obstacle_ptr->position().y();

  ADEBUG << "Get pose (" << pose_x << ", " << pose_y
         << ", " << pose_theta << ")";

  // Build rectangular scan_area
  Box2d scan_box({pose_x + FLAGS_scan_length / 2.0 * std::cos(pose_theta),
                  pose_y + FLAGS_scan_length / 2.0 * std::sin(pose_theta)},
                  pose_theta, FLAGS_scan_length, FLAGS_scan_width);

  const auto& scenario_type = ptr_scenario_features->scenario().type();
  const auto& obstacle_ids =
      obstacles_container->GetCurrentFramePredictableObstacleIds();

  for (const int& obstacle_id : obstacle_ids) {
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id);
    if (obstacle_ptr->history_size() == 0) {
      AERROR << "Obstacle [" << obstacle_ptr->id() << "] has no feature.";
      continue;
    }
    Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
    double obstacle_x = latest_feature_ptr->position().x();
    double obstacle_y = latest_feature_ptr->position().y();
    Vec2d ego_to_obstacle_vec(obstacle_x - pose_x, obstacle_y - pose_y);
    Vec2d ego_vec = Vec2d::CreateUnitVec2d(pose_theta);
    double s = ego_to_obstacle_vec.InnerProd(ego_vec);

    // Decide if we need consider this obstacle
    bool is_in_scan_area = scan_box.IsPointIn({obstacle_x, obstacle_y});
    bool is_on_lane = obstacle_ptr->IsOnLane();
    bool is_pedestrian_in_front = s > -2.0 &&
        latest_feature_ptr->type() == PerceptionObstacle::PEDESTRIAN;

    bool need_consider = is_in_scan_area || is_on_lane ||
                         is_pedestrian_in_front;

    // For junction scenario, need_consider if obstacle is in junction_polygon
    if (scenario_type == Scenario::JUNCTION ||
        scenario_type == Scenario::JUNCTION_TRAFFIC_LIGHT ||
        scenario_type == Scenario::JUNCTION_STOP_SIGN) {
      std::string junction_id =
          ptr_scenario_features->scenario().junction_id();
      std::shared_ptr<const apollo::hdmap::JunctionInfo> junction_info_ptr =
          PredictionMap::JunctionById(junction_id);
      CHECK_NOTNULL(junction_info_ptr);
      bool is_in_junction =
          junction_info_ptr->polygon().IsPointIn({obstacle_x, obstacle_y});
      need_consider = need_consider || is_in_junction;
    }

    if (!need_consider) {
      latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::IGNORE);
    } else {
      latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::NORMAL);
    }
  }
}

void ScenarioManager::PrioritizeObstaclesForCruiseScenario(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<CruiseScenarioFeatures> cruise_scenario_features,
    ObstaclesContainer* ptr_obstacle_contrainer) {
  const auto& obstacle_ids =
      ptr_obstacle_contrainer->GetCurrentFramePredictableObstacleIds();

  for (const int& obstacle_id : obstacle_ids) {
    Obstacle* obstacle_ptr = ptr_obstacle_contrainer->GetObstacle(obstacle_id);
    if (obstacle_ptr->history_size() == 0) {
      AERROR << "Obstacle [" << obstacle_ptr->id() << "] has no feature.";
      continue;
    }
    Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
    if (obstacle_ptr->IsOnLane()) {
      bool has_lane_of_interest = false;
      for (const auto& curr_lane :
           latest_feature_ptr->lane().current_lane_feature()) {
        const auto& curr_lane_id = curr_lane.lane_id();
        if (cruise_scenario_features->IsLaneOfInterest(curr_lane_id)) {
          has_lane_of_interest = true;
          break;
        }
      }
      if (!has_lane_of_interest) {
        for (const auto& nearby_lane :
             latest_feature_ptr->lane().nearby_lane_feature()) {
          const auto& nearby_lane_id = nearby_lane.lane_id();
          if (cruise_scenario_features->IsLaneOfInterest(nearby_lane_id)) {
            has_lane_of_interest = true;
            break;
          }
        }
      }
      if (!has_lane_of_interest) {
        latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::IGNORE);
      }
    } else {
      // Obstacle has neither lane nor neighbor lanes
      // Filter obstacle by its relative position to ego vehicle
      double ego_x = environment_features.get_ego_position().x();
      double ego_y = environment_features.get_ego_position().y();
      double ego_heading = environment_features.get_ego_heading();
      double ego_speed = environment_features.get_ego_speed();
      double obstacle_x = latest_feature_ptr->position().x();
      double obstacle_y = latest_feature_ptr->position().y();
      Vec2d ego_to_obstacle_vec(obstacle_x - ego_x, obstacle_y - ego_y);
      Vec2d ego_vec = Vec2d::CreateUnitVec2d(ego_heading);
      double l = ego_vec.CrossProd(ego_to_obstacle_vec);
      double s = ego_to_obstacle_vec.InnerProd(ego_vec);
      if (std::abs(l) > 10.0 || s > std::max(20.0, ego_speed * 5.0) ||
          s < 0.0) {
        latest_feature_ptr->mutable_priority()->set_priority(
            ObstaclePriority::IGNORE);
      }
    }
  }
}

}  // namespace prediction
}  // namespace apollo
<|endoftext|>