<commit_before>/*
   Copyright (c) 2021 Advanced Micro Devices, Inc. All rights reserved.
   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:
   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.
   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
 */

/* Test Case Description:
   Scenario 1: The  test validates if fine grain
   behavior is observed or not with memory allocated using hipHostMalloc()
   Scenario 2: The test validates if fine grain
   behavior is observed or not with memory allocated using hipMallocManaged()
   Scenario 3: The test validates if memory access is fine
   with memory allocated using hipMallocManaged() and CoarseGrain Advise
   Scenario 4: The test validates if memory access is fine
   with memory allocated using hipMalloc() and CoarseGrain Advise
   Scenario 5: The test validates if fine grain
   behavior is observed or not with memory allocated using
   hipExtMallocWithFlags()*/

#include <hip_test_common.hh>
#include <chrono>

__global__  void CoherentTst(int *ptr, int PeakClk) {
  // Incrementing the value by 1
  int64_t GpuFrq = (PeakClk * 1000);
  int64_t StrtTck = clock64();
  atomicAdd(ptr, 1);
  // The following while loop checks the value in ptr for around 3-4 seconds
  while ((clock64() - StrtTck) <= (3 * GpuFrq)) {
    if (*ptr == 3) {
      atomicAdd(ptr, 1);
      return;
    }
  }
}

__global__  void SquareKrnl(int *ptr) {
  // ptr value squared here
  *ptr = (*ptr) * (*ptr);
}

// The variable below will work as signal to decide pass/fail
static bool YES_COHERENT = false;

// The function tests the coherency of allocated memory
static void TstCoherency(int *Ptr, bool HmmMem) {
  int *Dptr = nullptr, peak_clk;
  hipStream_t strm;
  HIP_CHECK(hipStreamCreate(&strm));
  // storing value 1 in the memory created above
  *Ptr = 1;
  // Getting gpu frequency
  HIP_CHECK(hipDeviceGetAttribute(&peak_clk, hipDeviceAttributeClockRate, 0));
  if (!HmmMem) {
    HIP_CHECK(hipHostGetDevicePointer(reinterpret_cast<void **>(&Dptr), Ptr,
                                      0));
    CoherentTst<<<1, 1, 0, strm>>>(Dptr, peak_clk);
  } else {
    CoherentTst<<<1, 1, 0, strm>>>(Ptr, peak_clk);
  }
  // looping until the value is 2 for 3 seconds
  std::chrono::steady_clock::time_point start =
               std::chrono::steady_clock::now();
  while (std::chrono::duration_cast<std::chrono::seconds>(
         std::chrono::steady_clock::now() - start).count() < 3) {
    if (*Ptr == 2) {
      *Ptr += 1;
      break;
    }
  }
  HIP_CHECK(hipStreamSynchronize(strm));
  HIP_CHECK(hipStreamDestroy(strm));
  if (*Ptr == 4) {
    YES_COHERENT = true;
  }
}

/* Test case description: The following test validates if fine grain
   behavior is observed or not with memory allocated using hipHostMalloc()*/
// The following tests are disabled for Nvidia as they are not consistently
// passing
#if HT_AMD
TEST_CASE("Unit_hipHostMalloc_CoherentTst") {
  int *Ptr = nullptr,  SIZE = sizeof(int);
  bool HmmMem = false;
  YES_COHERENT = false;
  // Allocating hipHostMalloc() memory with hipHostMallocCoherent flag
  SECTION("hipHostMalloc with hipHostMallocCoherent flag") {
    HIP_CHECK(hipHostMalloc(&Ptr, SIZE, hipHostMallocCoherent));
  }
  SECTION("hipHostMalloc with Default flag") {
    HIP_CHECK(hipHostMalloc(&Ptr, SIZE));
  }
  SECTION("hipHostMalloc with hipHostMallocMapped flag") {
    HIP_CHECK(hipHostMalloc(&Ptr, SIZE, hipHostMallocMapped));
  }

  TstCoherency(Ptr, HmmMem);
  HIP_CHECK(hipHostFree(Ptr));
  REQUIRE(YES_COHERENT);
}
#endif

/* Test case description: The following test validates if fine grain
   behavior is observed or not with memory allocated using hipMallocManaged()*/
// The following tests are disabled for Nvidia as they are not consistently
// passing
#if HT_AMD
TEST_CASE("Unit_hipMallocManaged_CoherentTst") {
  int *Ptr = nullptr, SIZE = sizeof(int);
  bool HmmMem = true;
  YES_COHERENT = false;
  // Allocating hipMallocManaged() memory
  SECTION("hipMallocManaged with hipMemAttachGlobal flag") {
    HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachGlobal));
  }
  SECTION("hipMallocManaged with hipMemAttachHost flag") {
    HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachHost));
  }
  TstCoherency(Ptr, HmmMem);
  HIP_CHECK(hipFree(Ptr));
  REQUIRE(YES_COHERENT);
}
#endif

/* Test case description: The following test validates if memory access is fine
   with memory allocated using hipMallocManaged() and CoarseGrain Advise*/
TEST_CASE("Unit_hipMallocManaged_CoherentTstWthAdvise") {
  int *Ptr = nullptr, SIZE = sizeof(int);
  YES_COHERENT = false;
  // Allocating hipMallocManaged() memory
  SECTION("hipMallocManaged with hipMemAttachGlobal flag") {
    HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachGlobal));
  }
  SECTION("hipMallocManaged with hipMemAttachHost flag") {
    HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachHost));
  }
#if HT_AMD
  HIP_CHECK(hipMemAdvise(Ptr, SIZE, hipMemAdviseSetCoarseGrain, 0));
#endif
  // Initializing Ptr memory with 9
  *Ptr = 9;
  hipStream_t strm;
  HIP_CHECK(hipStreamCreate(&strm));
  SquareKrnl<<<1, 1, 0, strm>>>(Ptr);
  HIP_CHECK(hipStreamSynchronize(strm));
  if (*Ptr == 81) {
    YES_COHERENT = true;
  }
  HIP_CHECK(hipFree(Ptr));
  HIP_CHECK(hipStreamDestroy(strm));
  REQUIRE(YES_COHERENT);
}

/* Test case description: The following test validates if memory allocated
   using hipMalloc() are of type Coarse Grain*/
// The following tests are disabled for Nvidia as they are not applicable
#if HT_AMD
TEST_CASE("Unit_hipMalloc_CoherentTst") {
  int *Ptr = nullptr, SIZE = sizeof(int);
  uint32_t svm_attrib = 0;
  bool IfTstPassed = false;
  // Allocating hipMalloc() memory
  HIP_CHECK(hipMalloc(&Ptr, SIZE));
  HIP_CHECK(hipMemRangeGetAttribute(&svm_attrib, sizeof(svm_attrib),
        hipMemRangeAttributeCoherencyMode, Ptr, SIZE));
  if (svm_attrib == hipMemRangeCoherencyModeCoarseGrain) {
    IfTstPassed = true;
  }
  HIP_CHECK(hipFree(Ptr));
  REQUIRE(IfTstPassed);
}
#endif
/* Test case description: The following test validates if fine grain
   behavior is observed or not with memory allocated using
   hipExtMallocWithFlags()*/
#if HT_AMD
TEST_CASE("Unit_hipExtMallocWithFlags_CoherentTst") {
  int *Ptr = nullptr, SIZE = sizeof(int), InitVal = 9;
  bool FineGrain = true;
  YES_COHERENT = false;
  // Allocating hipExtMallocWithFlags() memory with flags
  SECTION("hipExtMallocWithFlags with hipDeviceMallocFinegrained flag") {
    HIP_CHECK(hipExtMallocWithFlags(reinterpret_cast<void**>(&Ptr), SIZE*2,
                                    hipDeviceMallocFinegrained));
  }
  SECTION("hipExtMallocWithFlags with hipDeviceMallocSignalMemory flag") {
    // for hipMallocSignalMemory flag the size of memory must be 8
    HIP_CHECK(hipExtMallocWithFlags(reinterpret_cast<void**>(&Ptr), SIZE*2,
                                    hipMallocSignalMemory));
  }
  SECTION("hipExtMallocWithFlags with hipDeviceMallocDefault flag") {
    /* hipExtMallocWithFlags() with flag
    hipDeviceMallocDefault allocates CoarseGrain memory */
    FineGrain = false;
    HIP_CHECK(hipExtMallocWithFlags(reinterpret_cast<void**>(&Ptr), SIZE*2,
                                    hipDeviceMallocDefault));
  }
  if (FineGrain) {
    TstCoherency(Ptr, FineGrain);
  } else {
    *Ptr = InitVal;
    hipStream_t strm;
    HIP_CHECK(hipStreamCreate(&strm));
    SquareKrnl<<<1, 1, 0, strm>>>(Ptr);
    HIP_CHECK(hipStreamSynchronize(strm));
    if (*Ptr == (InitVal * InitVal)) {
      YES_COHERENT = true;
    }
  }
  HIP_CHECK(hipFree(Ptr));
  REQUIRE(YES_COHERENT);
}
#endif

<commit_msg>Add missing checks in hipMemCoherencyTst.cc (#2442)<commit_after>/*
   Copyright (c) 2021 Advanced Micro Devices, Inc. All rights reserved.
   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:
   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.
   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
 */

/* Test Case Description:
   Scenario 1: The  test validates if fine grain
   behavior is observed or not with memory allocated using hipHostMalloc()
   Scenario 2: The test validates if fine grain
   behavior is observed or not with memory allocated using hipMallocManaged()
   Scenario 3: The test validates if memory access is fine
   with memory allocated using hipMallocManaged() and CoarseGrain Advise
   Scenario 4: The test validates if memory access is fine
   with memory allocated using hipMalloc() and CoarseGrain Advise
   Scenario 5: The test validates if fine grain
   behavior is observed or not with memory allocated using
   hipExtMallocWithFlags()*/

#include <hip_test_common.hh>
#include <chrono>

__global__  void CoherentTst(int *ptr, int PeakClk) {
  // Incrementing the value by 1
  int64_t GpuFrq = (PeakClk * 1000);
  int64_t StrtTck = clock64();
  atomicAdd(ptr, 1);
  // The following while loop checks the value in ptr for around 3-4 seconds
  while ((clock64() - StrtTck) <= (3 * GpuFrq)) {
    if (*ptr == 3) {
      atomicAdd(ptr, 1);
      return;
    }
  }
}

__global__  void SquareKrnl(int *ptr) {
  // ptr value squared here
  *ptr = (*ptr) * (*ptr);
}

// The variable below will work as signal to decide pass/fail
static bool YES_COHERENT = false;

// The function tests the coherency of allocated memory
static void TstCoherency(int *Ptr, bool HmmMem) {
  int *Dptr = nullptr, peak_clk;
  hipStream_t strm;
  HIP_CHECK(hipStreamCreate(&strm));
  // storing value 1 in the memory created above
  *Ptr = 1;
  // Getting gpu frequency
  HIP_CHECK(hipDeviceGetAttribute(&peak_clk, hipDeviceAttributeClockRate, 0));
  if (!HmmMem) {
    HIP_CHECK(hipHostGetDevicePointer(reinterpret_cast<void **>(&Dptr), Ptr,
                                      0));
    CoherentTst<<<1, 1, 0, strm>>>(Dptr, peak_clk);
  } else {
    CoherentTst<<<1, 1, 0, strm>>>(Ptr, peak_clk);
  }
  // looping until the value is 2 for 3 seconds
  std::chrono::steady_clock::time_point start =
               std::chrono::steady_clock::now();
  while (std::chrono::duration_cast<std::chrono::seconds>(
         std::chrono::steady_clock::now() - start).count() < 3) {
    if (*Ptr == 2) {
      *Ptr += 1;
      break;
    }
  }
  HIP_CHECK(hipStreamSynchronize(strm));
  HIP_CHECK(hipStreamDestroy(strm));
  if (*Ptr == 4) {
    YES_COHERENT = true;
  }
}

static int HmmAttrPrint() {
  int managed = 0;
  INFO("The following are the attribute values related to HMM for"
         " device 0:\n");
  HIP_CHECK(hipDeviceGetAttribute(&managed,
              hipDeviceAttributeDirectManagedMemAccessFromHost, 0));
  INFO("hipDeviceAttributeDirectManagedMemAccessFromHost: " << managed);
  HIP_CHECK(hipDeviceGetAttribute(&managed,
                                 hipDeviceAttributeConcurrentManagedAccess, 0));
  INFO("hipDeviceAttributeConcurrentManagedAccess: " << managed);
  HIP_CHECK(hipDeviceGetAttribute(&managed,
                                 hipDeviceAttributePageableMemoryAccess, 0));
  INFO("hipDeviceAttributePageableMemoryAccess: " << managed);
  HIP_CHECK(hipDeviceGetAttribute(&managed,
              hipDeviceAttributePageableMemoryAccessUsesHostPageTables, 0));
  INFO("hipDeviceAttributePageableMemoryAccessUsesHostPageTables:"
         << managed);

  HIP_CHECK(hipDeviceGetAttribute(&managed, hipDeviceAttributeManagedMemory,
                                  0));
  INFO("hipDeviceAttributeManagedMemory: " << managed);
  return managed;
}

/* Test case description: The following test validates if fine grain
   behavior is observed or not with memory allocated using hipHostMalloc()*/
// The following tests are disabled for Nvidia as they are not consistently
// passing
#if HT_AMD
TEST_CASE("Unit_hipHostMalloc_CoherentTst") {
  int *Ptr = nullptr,  SIZE = sizeof(int);
  bool HmmMem = false;
  YES_COHERENT = false;
  // Allocating hipHostMalloc() memory with hipHostMallocCoherent flag
  SECTION("hipHostMalloc with hipHostMallocCoherent flag") {
    HIP_CHECK(hipHostMalloc(&Ptr, SIZE, hipHostMallocCoherent));
  }
  SECTION("hipHostMalloc with Default flag") {
    HIP_CHECK(hipHostMalloc(&Ptr, SIZE));
  }
  SECTION("hipHostMalloc with hipHostMallocMapped flag") {
    HIP_CHECK(hipHostMalloc(&Ptr, SIZE, hipHostMallocMapped));
  }

  TstCoherency(Ptr, HmmMem);
  HIP_CHECK(hipHostFree(Ptr));
  REQUIRE(YES_COHERENT);
}
#endif

/* Test case description: The following test validates if fine grain
   behavior is observed or not with memory allocated using hipMallocManaged()*/
// The following tests are disabled for Nvidia as they are not consistently
// passing
#if HT_AMD
TEST_CASE("Unit_hipMallocManaged_CoherentTst") {
  int *Ptr = nullptr, SIZE = sizeof(int);
  bool HmmMem = true;
  YES_COHERENT = false;

  int managed =  HmmAttrPrint();
  if (managed == 1) {
    // Allocating hipMallocManaged() memory
    SECTION("hipMallocManaged with hipMemAttachGlobal flag") {
      HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachGlobal));
    }
    SECTION("hipMallocManaged with hipMemAttachHost flag") {
      HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachHost));
    }
    TstCoherency(Ptr, HmmMem);
    HIP_CHECK(hipFree(Ptr));
    REQUIRE(YES_COHERENT);
  } else {
    SUCCEED("GPU 0 doesn't support hipDeviceAttributeManagedMemory "
           "attribute. Hence skipping the testing with Pass result.\n");
  }
}
#endif

/* Test case description: The following test validates if memory access is fine
   with memory allocated using hipMallocManaged() and CoarseGrain Advise*/
TEST_CASE("Unit_hipMallocManaged_CoherentTstWthAdvise") {
  int *Ptr = nullptr, SIZE = sizeof(int);
  YES_COHERENT = false;
  // Allocating hipMallocManaged() memory
  SECTION("hipMallocManaged with hipMemAttachGlobal flag") {
    HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachGlobal));
  }
  SECTION("hipMallocManaged with hipMemAttachHost flag") {
    HIP_CHECK(hipMallocManaged(&Ptr, SIZE, hipMemAttachHost));
  }
#if HT_AMD
  HIP_CHECK(hipMemAdvise(Ptr, SIZE, hipMemAdviseSetCoarseGrain, 0));
#endif
  // Initializing Ptr memory with 9
  *Ptr = 9;
  hipStream_t strm;
  HIP_CHECK(hipStreamCreate(&strm));
  SquareKrnl<<<1, 1, 0, strm>>>(Ptr);
  HIP_CHECK(hipStreamSynchronize(strm));
  if (*Ptr == 81) {
    YES_COHERENT = true;
  }
  HIP_CHECK(hipFree(Ptr));
  HIP_CHECK(hipStreamDestroy(strm));
  REQUIRE(YES_COHERENT);
}

/* Test case description: The following test validates if memory allocated
   using hipMalloc() are of type Coarse Grain*/
// The following tests are disabled for Nvidia as they are not applicable
#if HT_AMD
TEST_CASE("Unit_hipMalloc_CoherentTst") {
  int *Ptr = nullptr, SIZE = sizeof(int);
  uint32_t svm_attrib = 0;
  bool IfTstPassed = false;
  // Allocating hipMalloc() memory
  HIP_CHECK(hipMalloc(&Ptr, SIZE));
  HIP_CHECK(hipMemRangeGetAttribute(&svm_attrib, sizeof(svm_attrib),
        hipMemRangeAttributeCoherencyMode, Ptr, SIZE));
  if (svm_attrib == hipMemRangeCoherencyModeCoarseGrain) {
    IfTstPassed = true;
  }
  HIP_CHECK(hipFree(Ptr));
  REQUIRE(IfTstPassed);
}
#endif
/* Test case description: The following test validates if fine grain
   behavior is observed or not with memory allocated using
   hipExtMallocWithFlags()*/
#if HT_AMD
TEST_CASE("Unit_hipExtMallocWithFlags_CoherentTst") {
  int *Ptr = nullptr, SIZE = sizeof(int), InitVal = 9;
  bool FineGrain = true;
  YES_COHERENT = false;

  int managed =  HmmAttrPrint();
  if (managed == 1) {
    // Allocating hipExtMallocWithFlags() memory with flags
    SECTION("hipExtMallocWithFlags with hipDeviceMallocFinegrained flag") {
      HIP_CHECK(hipExtMallocWithFlags(reinterpret_cast<void**>(&Ptr), SIZE*2,
                                      hipDeviceMallocFinegrained));
    }
    SECTION("hipExtMallocWithFlags with hipDeviceMallocSignalMemory flag") {
      // for hipMallocSignalMemory flag the size of memory must be 8
      HIP_CHECK(hipExtMallocWithFlags(reinterpret_cast<void**>(&Ptr), SIZE*2,
                                      hipMallocSignalMemory));
    }
    SECTION("hipExtMallocWithFlags with hipDeviceMallocDefault flag") {
      /* hipExtMallocWithFlags() with flag
      hipDeviceMallocDefault allocates CoarseGrain memory */
      FineGrain = false;
      HIP_CHECK(hipExtMallocWithFlags(reinterpret_cast<void**>(&Ptr), SIZE*2,
                                      hipDeviceMallocDefault));
    }
    if (FineGrain) {
      TstCoherency(Ptr, FineGrain);
    } else {
      *Ptr = InitVal;
      hipStream_t strm;
      HIP_CHECK(hipStreamCreate(&strm));
      SquareKrnl<<<1, 1, 0, strm>>>(Ptr);
      HIP_CHECK(hipStreamSynchronize(strm));
      if (*Ptr == (InitVal * InitVal)) {
        YES_COHERENT = true;
      }
    }
    HIP_CHECK(hipFree(Ptr));
    REQUIRE(YES_COHERENT);
  } else {
    SUCCEED("GPU 0 doesn't support hipDeviceAttributeManagedMemory "
           "attribute. Hence skipping the testing with Pass result.\n");
  }
}
#endif

<|endoftext|>


<commit_before>//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <limits>
#include <deque>
#include <cstdlib>
#include <hpp/util/assertion.hh>
#include <hpp/util/debug.hh>
#include <hpp/core/distance.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/random-shortcut.hh>

namespace hpp {
  namespace core {
    // Compute the length of a vector of paths assuming that each element
    // is optimal for the given distance.
    static value_type pathLength (const PathVectorPtr_t& path,
				  const DistancePtr_t& distance)
    {
      value_type result = 0;
      for (std::size_t i=0; i<path->numberPaths (); ++i) {
	const PathPtr_t& element (path->pathAtRank (i));
	Configuration_t q1 = element->initial ();
	Configuration_t q2 = element->end ();
	result += (*distance) (q1, q2);
      }
      return result;
    }

    RandomShortcutPtr_t
    RandomShortcut::create (const Problem& problem)
    {
      RandomShortcut* ptr = new RandomShortcut (problem);
      return RandomShortcutPtr_t (ptr);
    }

    RandomShortcut::RandomShortcut (const Problem& problem) :
      PathOptimizer (problem)
    {
    }

    PathVectorPtr_t RandomShortcut::optimize (const PathVectorPtr_t& path)
    {
      using std::numeric_limits;
      using std::make_pair;
      bool finished = false;
      value_type t3 = path->timeRange ().second;
      Configuration_t q0 = path->initial ();
      Configuration_t q3 = path->end ();
      PathVectorPtr_t tmpPath = path;

      // Maximal number of iterations without improvements
      const std::size_t n = 5;
      std::deque <value_type> length (n-1,
				      numeric_limits <value_type>::infinity ());
      length.push_back (pathLength (tmpPath, problem ().distance ()));
      PathVectorPtr_t result;
      Configuration_t q1 (path->outputSize ()),
                      q2 (path->outputSize ());

      while (!finished) {
	t3 = tmpPath->timeRange ().second;
	value_type u2 = t3 * rand ()/RAND_MAX;
	value_type u1 = t3 * rand ()/RAND_MAX;
	value_type t1, t2;
	if (u1 < u2) {t1 = u1; t2 = u2;} else {t1 = u2; t2 = u1;}
	if (!(*tmpPath) (q1, t1)) {
          hppDout (error, "Configuration at param " << t1 << " could not be "
              "projected");
        }
	if (!(*tmpPath) (q2, t2)) {
          hppDout (error, "Configuration at param " << t1 << " could not be "
              "projected");
        }
	// Validate sub parts
	bool valid [3];
	PathPtr_t straight [3];
	straight [0] = steer (q0, q1);
	straight [1] = steer (q1, q2);
	straight [2] = steer (q2, q3);
	for (unsigned i=0; i<3; ++i) {
	  PathPtr_t validPart;
	  PathValidationReportPtr_t report;
          if (!straight [i]) valid[i] = false;
          else {
            valid [i] = problem ().pathValidation ()->validate
              (straight [i], false, validPart, report);
          }
	}
	// Replace valid parts
	result = PathVector::create (path->outputSize (),
				     path->outputDerivativeSize ());
	if (valid [0])
	  result->appendPath (straight [0]);
	else
	  result->concatenate (*(tmpPath->extract
				 (make_pair <value_type,value_type> (0, t1))->
				 as <PathVector> ()));
	if (valid [1])
	  result->appendPath (straight [1]);
	else
	  result->concatenate (*(tmpPath->extract
				 (make_pair <value_type,value_type> (t1, t2))->
				 as <PathVector> ()));
	if (valid [2])
	  result->appendPath (straight [2]);
	else
	  result->concatenate (*(tmpPath->extract
				(make_pair <value_type, value_type> (t2, t3))->
				 as <PathVector> ()));
	length.push_back (pathLength (result, problem ().distance ()));
	length.pop_front ();
	finished = (length [0] <= length [n-1]);
	hppDout (info, "length = " << length [n-1]);
	tmpPath = result;
      }
      hppDout (info, "RandomShortcut:" << *result);
      for (std::size_t i = 0; i < result->numberPaths (); ++i) {
        if (result->pathAtRank(i)->constraints())
          hppDout (info, "At rank " << i << ", constraints are " <<
              *result->pathAtRank(i)->constraints());
        else
          hppDout (info, "At rank " << i << ", no constraints");
      }
      return result;
    }
  } // namespace core
} // namespace hpp
<commit_msg>RandomShortcut handles projection errors<commit_after>//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <limits>
#include <deque>
#include <cstdlib>
#include <hpp/util/assertion.hh>
#include <hpp/util/debug.hh>
#include <hpp/core/distance.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/random-shortcut.hh>

namespace hpp {
  namespace core {
    // Compute the length of a vector of paths assuming that each element
    // is optimal for the given distance.
    static value_type pathLength (const PathVectorPtr_t& path,
				  const DistancePtr_t& distance)
    {
      value_type result = 0;
      for (std::size_t i=0; i<path->numberPaths (); ++i) {
	const PathPtr_t& element (path->pathAtRank (i));
	Configuration_t q1 = element->initial ();
	Configuration_t q2 = element->end ();
	result += (*distance) (q1, q2);
      }
      return result;
    }

    RandomShortcutPtr_t
    RandomShortcut::create (const Problem& problem)
    {
      RandomShortcut* ptr = new RandomShortcut (problem);
      return RandomShortcutPtr_t (ptr);
    }

    RandomShortcut::RandomShortcut (const Problem& problem) :
      PathOptimizer (problem)
    {
    }

    PathVectorPtr_t RandomShortcut::optimize (const PathVectorPtr_t& path)
    {
      using std::numeric_limits;
      using std::make_pair;
      bool finished = false;
      value_type t3 = path->timeRange ().second;
      Configuration_t q0 = path->initial ();
      Configuration_t q3 = path->end ();
      PathVectorPtr_t tmpPath = path;

      // Maximal number of iterations without improvements
      const std::size_t n = 5;
      std::size_t projectionError = n;
      std::deque <value_type> length (n-1,
				      numeric_limits <value_type>::infinity ());
      length.push_back (pathLength (tmpPath, problem ().distance ()));
      PathVectorPtr_t result;
      Configuration_t q1 (path->outputSize ()),
                      q2 (path->outputSize ());

      while (!finished && projectionError != 0) {
	t3 = tmpPath->timeRange ().second;
	value_type u2 = t3 * rand ()/RAND_MAX;
	value_type u1 = t3 * rand ()/RAND_MAX;
	value_type t1, t2;
	if (u1 < u2) {t1 = u1; t2 = u2;} else {t1 = u2; t2 = u1;}
	if (!(*tmpPath) (q1, t1)) {
          hppDout (error, "Configuration at param " << t1 << " could not be "
              "projected");
          projectionError--;
          continue;
        }
	if (!(*tmpPath) (q2, t2)) {
          hppDout (error, "Configuration at param " << t1 << " could not be "
              "projected");
          projectionError--;
          continue;
        }
	// Validate sub parts
	bool valid [3];
	PathPtr_t straight [3];
	straight [0] = steer (q0, q1);
	straight [1] = steer (q1, q2);
	straight [2] = steer (q2, q3);
	for (unsigned i=0; i<3; ++i) {
	  PathPtr_t validPart;
	  PathValidationReportPtr_t report;
          if (!straight [i]) valid[i] = false;
          else {
            valid [i] = problem ().pathValidation ()->validate
              (straight [i], false, validPart, report);
          }
	}
	// Replace valid parts
	result = PathVector::create (path->outputSize (),
				     path->outputDerivativeSize ());
        try {
          if (valid [0])
            result->appendPath (straight [0]);
          else
            result->concatenate (*(tmpPath->extract
                  (make_pair <value_type,value_type> (0, t1))->
                  as <PathVector> ()));
          if (valid [1])
            result->appendPath (straight [1]);
          else
            result->concatenate (*(tmpPath->extract
                  (make_pair <value_type,value_type> (t1, t2))->
                  as <PathVector> ()));
          if (valid [2])
            result->appendPath (straight [2]);
          else
            result->concatenate (*(tmpPath->extract
                  (make_pair <value_type, value_type> (t2, t3))->
                  as <PathVector> ()));
        } catch (const projection_error& e) {
          hppDout (error, "Caught exception at with time " << t1 << " and " <<
              t2 << ": " << e.what ());
          projectionError--;
          result = tmpPath;
          continue;
        }
	length.push_back (pathLength (result, problem ().distance ()));
	length.pop_front ();
	finished = (length [0] <= length [n-1]);
	hppDout (info, "length = " << length [n-1]);
	tmpPath = result;
      }
      hppDout (info, "RandomShortcut:" << *result);
      for (std::size_t i = 0; i < result->numberPaths (); ++i) {
        if (result->pathAtRank(i)->constraints())
          hppDout (info, "At rank " << i << ", constraints are " <<
              *result->pathAtRank(i)->constraints());
        else
          hppDout (info, "At rank " << i << ", no constraints");
      }
      return result;
    }
  } // namespace core
} // namespace hpp
<|endoftext|>


<commit_before>/*
 * raspberrysensor
 * https://github.com/markw/raspberrysensor
 *
 * Access a temperature and humidity sensor using the GPIO ports on the Raspberry Pi
 *
 * Copyright (c) 2012 Mark Wolfe
 * Licensed under the MIT license.
 */

#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <sched.h>
#include <sys/io.h>
#include <bcm2835.h>

#define NSEC_PER_SEC    1000000000

#include <node.h>
#include <string>

using namespace v8;

static inline int timespec_cmp (struct timespec a, struct timespec b)
{
  return (a.tv_sec < b.tv_sec ? -1
    : a.tv_sec > b.tv_sec ? 1
    : a.tv_nsec - b.tv_nsec);
}

// Forward declaration. Usually, you do this in a header file.
Handle<Value> Async(const Arguments& args);
void AsyncWork(uv_work_t* req);
void AsyncAfter(uv_work_t* req);

// We use a struct to store information about the asynchronous "work request".
struct Baton {
  // This handle holds the callback function we'll call after the work request
  // has been completed in a threadpool thread. It's persistent so that V8
  // doesn't garbage collect it away while our request waits to be processed.
  // This means that we'll have to dispose of it later ourselves.
  Persistent<Function> callback;

  // The GPIO pin which will be read
  int32_t pin;

  // return values
  int32_t temp;
  int32_t humidity;

  // time taken
  int32_t time_taken;

  // time stamps
  struct timespec t_before;
  struct timespec t_after;

  // Tracking errors that happened in the worker function. You can use any
  // variables you want. E.g. in some cases, it might be useful to report
  // an error number.
  bool error;

  // an optional error message
  std::string error_message;

  // Custom data you can pass through.
  int32_t result;
};

// This is the function called directly from JavaScript land. It creates a
// work request object and schedules it for execution.
Handle<Value> Async(const Arguments& args) {
  HandleScope scope;

  if (!args[0]->IsFunction()) {
    return ThrowException(Exception::TypeError(
          String::New("First argument must be a callback function.")));
  }

  if (!args[1]->IsUndefined() && !args[1]->IsNumber()) {
    return ThrowException(Exception::TypeError(
          String::New("Second argument must be the number of the pin to read data from.")));
  }

  // There's no ToFunction(), use a Cast instead.
  Local<Function> callback = Local<Function>::Cast(args[0]);

  // The baton holds our custom status information for this asynchronous call,
  // like the callback function we want to call when returning to the main
  // thread and the status information.
  Baton* baton = new Baton();
  baton->error = false;
  baton->callback = Persistent<Function>::New(callback);
  baton->pin = args[1]->IsUndefined() ? 4 : args[1]->NumberValue();

  // This creates the work request struct.
  uv_work_t *req = new uv_work_t();
  req->data = baton;

  // Schedule our work request with libuv. Here you can specify the functions
  // that should be executed in the threadpool and back in the main thread
  // after the threadpool function completed.
  int status = uv_queue_work(uv_default_loop(), req, AsyncWork, AsyncAfter);
  assert(status == 0);

  return Undefined();
}

// This function is executed in another thread at some point after it has been
// scheduled. IT MUST NOT USE ANY V8 FUNCTIONALITY. Otherwise your extension
// will crash randomly and you'll have a lot of fun debugging.
// If you want to use parameters passed into the original call, you have to
// convert them to PODs or some other fancy method.
void AsyncWork(uv_work_t* req) {
  Baton* baton = static_cast<Baton*>(req->data);

  // initialise the bcm2835, returning an error if this fails.
  if (!bcm2835_init()){
    baton->error_message = "Unable to initialise bcm2835";
    baton->error = true;
    return;
  }

  struct sched_param param;
  int retryCount = 0;

  clock_gettime(0, &baton->t_before);

  do {

    if(retryCount > 100){
      clock_gettime(0, &baton->t_after);
      baton->result = timespec_cmp(baton->t_after, baton->t_before);
      return;
    }

    bcm2835_delayMicroseconds(10);

    retryCount++;

  } while (1);

  // Do work in threadpool here.
  baton->result = baton->pin;

  // If the work we do fails, set baton->error_message to the error string
  // and baton->error to true.
}

// This function is executed in the main V8/JavaScript thread. That means it's
// safe to use V8 functions again. Don't forget the HandleScope!
void AsyncAfter(uv_work_t* req) {
  HandleScope scope;
  Baton* baton = static_cast<Baton*>(req->data);

  if (baton->error) {
    Local<Value> err = Exception::Error(String::New(baton->error_message.c_str()));

    // Prepare the parameters for the callback function.
    const unsigned argc = 1;
    Local<Value> argv[argc] = { err };

    // Wrap the callback function call in a TryCatch so that we can call
    // node's FatalException afterwards. This makes it possible to catch
    // the exception from JavaScript land using the
    // process.on('uncaughtException') event.
    TryCatch try_catch;
    baton->callback->Call(Context::GetCurrent()->Global(), argc, argv);
    if (try_catch.HasCaught()) {
      node::FatalException(try_catch);
    }
  } else {
    // In case the operation succeeded, convention is to pass null as the
    // first argument before the result arguments.
    // In case you produced more complex data, this is the place to convert
    // your plain C++ data structures into JavaScript/V8 data structures.
    const unsigned argc = 2;
    Local<Value> argv[argc] = {
      Local<Value>::New(Null()),
      Local<Value>::New(Integer::New(baton->result))
    };

    // Wrap the callback function call in a TryCatch so that we can call
    // node's FatalException afterwards. This makes it possible to catch
    // the exception from JavaScript land using the
    // process.on('uncaughtException') event.
    TryCatch try_catch;
    baton->callback->Call(Context::GetCurrent()->Global(), argc, argv);
    if (try_catch.HasCaught()) {
      node::FatalException(try_catch);
    }
  }

  // The callback is a permanent handle, so we have to dispose of it manually.
  baton->callback.Dispose();

  // We also created the baton and the work_req struct with new, so we have to
  // manually delete both.
  delete baton;
  delete req;
}

void RegisterModule(Handle<Object> target) {
  target->Set(String::NewSymbol("async"),
      FunctionTemplate::New(Async)->GetFunction());
}

NODE_MODULE(raspberrysensor, RegisterModule);
<commit_msg>Reading data is working need to work on decoding.<commit_after>/*
 * raspberrysensor
 * https://github.com/markw/raspberrysensor
 *
 * Access a temperature and humidity sensor using the GPIO ports on the Raspberry Pi
 *
 * Copyright (c) 2012 Mark Wolfe
 * Licensed under the MIT license.
 */

#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <sched.h>
#include <sys/io.h>
#include <bcm2835.h>

#define DHT22_DATA_BIT_COUNT 41

#include <node.h>
#include <string>

using namespace v8;

static inline int timespec_cmp (struct timespec a, struct timespec b)
{
  return (a.tv_sec < b.tv_sec ? -1
    : a.tv_sec > b.tv_sec ? 1
    : a.tv_nsec - b.tv_nsec);
}

// Forward declaration. Usually, you do this in a header file.
Handle<Value> Async(const Arguments& args);
void AsyncWork(uv_work_t* req);
void AsyncAfter(uv_work_t* req);

// We use a struct to store information about the asynchronous "work request".
struct Baton {
  // This handle holds the callback function we'll call after the work request
  // has been completed in a threadpool thread. It's persistent so that V8
  // doesn't garbage collect it away while our request waits to be processed.
  // This means that we'll have to dispose of it later ourselves.
  Persistent<Function> callback;

  // The GPIO pin which will be read
  int32_t pin;

  // return values
  int32_t temp;
  int32_t humidity;

  // time taken
  int32_t time_taken;

  // time stamps
  struct timespec t_before;
  struct timespec t_after;

  // Tracking errors that happened in the worker function. You can use any
  // variables you want. E.g. in some cases, it might be useful to report
  // an error number.
  bool error;

  // an optional error message
  std::string error_message;

  // Custom data you can pass through.
  int32_t result;
};

// This is the function called directly from JavaScript land. It creates a
// work request object and schedules it for execution.
Handle<Value> Async(const Arguments& args) {
  HandleScope scope;

  if (!args[0]->IsFunction()) {
    return ThrowException(Exception::TypeError(
          String::New("First argument must be a callback function.")));
  }

  if (!args[1]->IsUndefined() && !args[1]->IsNumber()) {
    return ThrowException(Exception::TypeError(
          String::New("Second argument must be the number of the pin to read data from.")));
  }

  // There's no ToFunction(), use a Cast instead.
  Local<Function> callback = Local<Function>::Cast(args[0]);

  // The baton holds our custom status information for this asynchronous call,
  // like the callback function we want to call when returning to the main
  // thread and the status information.
  Baton* baton = new Baton();
  baton->error = false;
  baton->callback = Persistent<Function>::New(callback);
  baton->pin = args[1]->IsUndefined() ? 4 : args[1]->NumberValue();

  // This creates the work request struct.
  uv_work_t *req = new uv_work_t();
  req->data = baton;

  // Schedule our work request with libuv. Here you can specify the functions
  // that should be executed in the threadpool and back in the main thread
  // after the threadpool function completed.
  int status = uv_queue_work(uv_default_loop(), req, AsyncWork, AsyncAfter);
  assert(status == 0);

  return Undefined();
}

// This function is executed in another thread at some point after it has been
// scheduled. IT MUST NOT USE ANY V8 FUNCTIONALITY. Otherwise your extension
// will crash randomly and you'll have a lot of fun debugging.
// If you want to use parameters passed into the original call, you have to
// convert them to PODs or some other fancy method.
void AsyncWork(uv_work_t* req) {
  Baton* baton = static_cast<Baton*>(req->data);

  // initialise the bcm2835, returning an error if this fails.
  if (!bcm2835_init()){
    baton->error_message = "Unable to initialise bcm2835";
    baton->error = true;
    return;
  }

//  struct sched_param param;
  uint8_t retryCount;
  uint8_t bitTimes[DHT22_DATA_BIT_COUNT];
  int i;

  int currentTemperature;
  int currentHumidity;

  // configure the pin for output
  bcm2835_gpio_fsel(baton->pin, BCM2835_GPIO_FSEL_OUTP);

  // Pin needs to start HIGH, wait until it is HIGH with a timeout
  retryCount = 0;

  do {
    if (retryCount > 125) {
      baton->error_message = "DHT bus timeout";
      baton->error = true;
      return;
    }
    bcm2835_delayMicroseconds(2);
    retryCount++;
  } while (bcm2835_gpio_lev(baton->pin) == HIGH); // should be high

  // set the pin low
  bcm2835_gpio_write(baton->pin, LOW);

  // hold it for 1ms
  bcm2835_delay(1);

  // configure the pin for output
  bcm2835_gpio_fsel(baton->pin, BCM2835_GPIO_FSEL_INPT);

  // Find the start of the ACK Pulse
  retryCount = 0;

  do {
    if(retryCount > 25) { //(Spec is 20 to 40 us, 25*2 == 50 us)
      baton->error_message = "DHT not present.";
      baton->error = true;
      return;
    }
    bcm2835_delayMicroseconds(2);
    retryCount++;
  } while (bcm2835_gpio_lev(baton->pin) == LOW);

  // Find the end of the ACK Pulse
  retryCount = 0;

  do {
    if (retryCount > 50) { //(Spec is 80 us, 50*2 == 100 us)
      baton->error_message = "DHT not present.";
      baton->error = true;
      return;
    }
    bcm2835_delayMicroseconds(2);
    retryCount++;
  } while (bcm2835_gpio_lev(baton->pin) == HIGH);

  // Read the 40 bit data stream
  for(i = 0; i < DHT22_DATA_BIT_COUNT; i++) {

    // Find the start of the sync pulse
    retryCount = 0;
    do {
      if (retryCount > 35) { //(Spec is 50 us, 35*2 == 70 us)
        baton->error_message = "DHT sync error.";
        baton->error = true;
        return;
      }
      bcm2835_delayMicroseconds(2);
      retryCount++;
    } while (bcm2835_gpio_lev(baton->pin) == LOW);

    // Measure the width of the data pulse
    retryCount = 0;
    do {
      if (retryCount > 50) { //(Spec is 80 us, 50*2 == 100 us)
        baton->error_message = "DHT data timeout error.";
        baton->error = true;
        return;
      }
      bcm2835_delayMicroseconds(2);
      retryCount++;
    } while (bcm2835_gpio_lev(baton->pin) == HIGH);

    // assign the bit value
    bitTimes[i] = retryCount;
  }

  // DEBUG
  printf("values: ");
  for(i = 0; i < DHT22_DATA_BIT_COUNT; i++) {
    printf("%d ", bitTimes[i]);
  }
  printf("\n");
  // DEBUG

  // Spec: 0 is 26 to 28 us
  // Spec: 1 is 70 us
  // bitTimes[x] <= 11 is a 0
  // bitTimes[x] >  11 is a 1

  // read humidity
  currentHumidity = 0;
  for(i = 0; i < 16; i++) {
    if(bitTimes[i + 1] > 11) {
      currentHumidity |= (1 << (15 - i));
    }
  }

  baton->humidity = currentHumidity & 0x7FFF;

  // DEBUG
  printf("currentHumidity = %d\n", baton->humidity);
  // DEBUG

  // read the temperature
  currentTemperature = 0;
  for(i = 0; i < 16; i++) {
    if(bitTimes[i + 17] > 11){
      currentTemperature |= (1 << (15 - i));
    }
  }

  baton->temp = currentTemperature & 0x7FFF;

  // DEBUG
  printf("currentTemperature = %d\n", baton->temp);
  // DEBUG

/*
  clock_gettime(0, &baton->t_before);

  retryCount = 0;

  do {

    if(retryCount > 100){
      clock_gettime(0, &baton->t_after);
      baton->result = timespec_cmp(baton->t_after, baton->t_before);
      return;
    }

    bcm2835_delayMicroseconds(10);
    retryCount++;

  } while (1);
*/
  // Do work in threadpool here.
  baton->result = baton->pin;

  // If the work we do fails, set baton->error_message to the error string
  // and baton->error to true.
}

// This function is executed in the main V8/JavaScript thread. That means it's
// safe to use V8 functions again. Don't forget the HandleScope!
void AsyncAfter(uv_work_t* req) {
  HandleScope scope;
  Baton* baton = static_cast<Baton*>(req->data);

  if (baton->error) {
    Local<Value> err = Exception::Error(String::New(baton->error_message.c_str()));

    // Prepare the parameters for the callback function.
    const unsigned argc = 1;
    Local<Value> argv[argc] = { err };

    // Wrap the callback function call in a TryCatch so that we can call
    // node's FatalException afterwards. This makes it possible to catch
    // the exception from JavaScript land using the
    // process.on('uncaughtException') event.
    TryCatch try_catch;
    baton->callback->Call(Context::GetCurrent()->Global(), argc, argv);
    if (try_catch.HasCaught()) {
      node::FatalException(try_catch);
    }
  } else {
    // In case the operation succeeded, convention is to pass null as the
    // first argument before the result arguments.
    // In case you produced more complex data, this is the place to convert
    // your plain C++ data structures into JavaScript/V8 data structures.
    const unsigned argc = 2;
    Local<Value> argv[argc] = {
      Local<Value>::New(Null()),
      Local<Value>::New(Integer::New(baton->result))
    };

    // Wrap the callback function call in a TryCatch so that we can call
    // node's FatalException afterwards. This makes it possible to catch
    // the exception from JavaScript land using the
    // process.on('uncaughtException') event.
    TryCatch try_catch;
    baton->callback->Call(Context::GetCurrent()->Global(), argc, argv);
    if (try_catch.HasCaught()) {
      node::FatalException(try_catch);
    }
  }

  // The callback is a permanent handle, so we have to dispose of it manually.
  baton->callback.Dispose();

  // We also created the baton and the work_req struct with new, so we have to
  // manually delete both.
  delete baton;
  delete req;
}

void RegisterModule(Handle<Object> target) {
  target->Set(String::NewSymbol("async"),
      FunctionTemplate::New(Async)->GetFunction());
}

NODE_MODULE(raspberrysensor, RegisterModule);
<|endoftext|>


<commit_before>
// .\Release\x64\benchmarks.exe --benchmark_repetitions=3 --benchmark_filter=Planetarium  // NOLINT(whitespace/line_length)

#include "ksp_plugin/planetarium.hpp"

#include <random>
#include <vector>

#include "astronomy/time_scales.hpp"
#include "benchmark/benchmark.h"
#include "physics/body_centred_non_rotating_dynamic_frame.hpp"
#include "physics/solar_system.hpp"
#include "testing_utilities/solar_system_factory.hpp"

namespace principia {
namespace geometry {

using astronomy::operator""_UT1;
using astronomy::operator""_TT;
using base::make_not_null_unique;
using base::not_null;
using geometry::Bivector;
using geometry::Perspective;
using geometry::RigidTransformation;
using geometry::RP2Lines;
using geometry::Vector;
using geometry::Velocity;
using integrators::SymmetricLinearMultistepIntegrator;
using integrators::methods::Quinlan1999Order8A;
using integrators::methods::QuinlanTremaine1990Order12;
using ksp_plugin::Camera;
using ksp_plugin::Barycentric;
using ksp_plugin::Navigation;
using ksp_plugin::NavigationFrame;
using ksp_plugin::Planetarium;
using physics::BodyCentredNonRotatingDynamicFrame;
using physics::DegreesOfFreedom;
using physics::DiscreteTrajectory;
using physics::Ephemeris;
using physics::KeplerianElements;
using physics::KeplerOrbit;
using physics::MassiveBody;
using physics::MasslessBody;
using physics::SolarSystem;
using quantities::Angle;
using quantities::Cos;
using quantities::Infinity;
using quantities::Length;
using quantities::Sin;
using quantities::Time;
using quantities::si::ArcMinute;
using quantities::si::Day;
using quantities::si::Degree;
using quantities::si::Kilo;
using quantities::si::Metre;
using quantities::si::Milli;
using quantities::si::Minute;
using quantities::si::Radian;
using quantities::si::Second;
using testing_utilities::SolarSystemFactory;

namespace {

constexpr Length near = 40'000 * Kilo(Metre);
constexpr Length far = 400'000 * Kilo(Metre);
constexpr Length focal = 1 * Metre;

Perspective<Navigation, Camera> PolarPerspective(
    Length const distance_from_earth) {
  return {
      RigidTransformation<Navigation, Camera>(
          Navigation::origin + Displacement<Navigation>(
                                   {0 * Metre, 0 * Metre, distance_from_earth}),
          Camera::origin,
          Rotation<Navigation, Camera>(Vector<double, Navigation>({1, 0, 0}),
                                       Vector<double, Navigation>({0, 0, 1}),
                                       Bivector<double, Navigation>({0, -1, 0}))
              .Forget()),
      focal};
}

Perspective<Navigation, Camera> EquatorialPerspective(
    Length const distance_from_earth) {
  return {
      RigidTransformation<Navigation, Camera>(
          Navigation::origin + Displacement<Navigation>(
                                   {0 * Metre, distance_from_earth, 0 * Metre}),
          Camera::origin,
          Rotation<Navigation, Camera>(Vector<double, Navigation>({1, 0, 0}),
                                       Vector<double, Navigation>({0, 0, 1}),
                                       Bivector<double, Navigation>({0, -1, 0}))
              .Forget()),
      focal};
}

class Satellites {
 public:
  Satellites()
      : solar_system_(make_not_null_unique<SolarSystem<Barycentric>>(
            SOLUTION_DIR / "astronomy" / "sol_gravity_model.proto.txt",
            SOLUTION_DIR / "astronomy" /
                "sol_initial_state_jd_2451545_000000000.proto.txt",
            /*ignore_frame=*/true)),
        ephemeris_(
            solar_system_->MakeEphemeris(/*fitting_tolerance=*/1 * Milli(Metre),
                                         EphemerisParameters())),
        earth_(solar_system_->massive_body(
            *ephemeris_,
            SolarSystemFactory::name(SolarSystemFactory::Earth))),
        earth_centred_inertial_(
            make_not_null_unique<
                BodyCentredNonRotatingDynamicFrame<Barycentric, Navigation>>(
                ephemeris_.get(),
                earth_)) {
    // Two-line elements for GOES-8:
    // 1 23051U 94022A   00004.06628221 -.00000243  00000-0  00000-0 0  9630
    // 2 23051   0.4232  97.7420 0004776 192.8349 121.5613  1.00264613 28364
    constexpr Instant goes_8_epoch = "JD2451548.56628221"_UT1;
    KeplerianElements<Barycentric> goes_8_elements;
    goes_8_elements.inclination = 0.4232 * Degree;
    goes_8_elements.longitude_of_ascending_node = 97.7420 * Degree;
    goes_8_elements.eccentricity = 0.0004776;
    goes_8_elements.argument_of_periapsis = 192.8349 * Degree;
    goes_8_elements.mean_anomaly = 121.5613 * Degree;
    goes_8_elements.mean_motion = 1.00264613 * (2 * π * Radian / Day);
    KeplerOrbit<Barycentric> const goes_8_orbit(
        earth_, MasslessBody{}, goes_8_elements, goes_8_epoch);
    goes_8_trajectory_.Append(
        goes_8_epoch,
        ephemeris_->trajectory(earth_)->EvaluateDegreesOfFreedom(goes_8_epoch) +
            goes_8_orbit.StateVectors(goes_8_epoch));

    ephemeris_->Prolong(goes_8_epoch);
    auto goes_8_instance = ephemeris_->NewInstance(
        {&goes_8_trajectory_},
        Ephemeris<Barycentric>::NoIntrinsicAccelerations,
        HistoryParameters());
    ephemeris_->FlowWithFixedStep(goes_8_epoch + 100 * Day, goes_8_instance)
  }

  DiscreteTrajectory<Barycentric> const& GOES8Trajectory() const {
    return goes_8_trajectory_;
  }

  Planetarium MakePlanetarium(
      Perspective<Navigation, Camera> const& perspective) const {
    // No dark area, human visual acuity, wide field of view.
    Planetarium::Parameters parameters(
        /*sphere_radius_multiplier=*/1,
        /*angular_resolution=*/0.4 * ArcMinute,
        /*field_of_view=*/90 * Degree);
    Planetarium planetarium(
        parameters, perspective, &ephemeris_, &earth_centred_inertial_);
  }

 private:
  Ephemeris<Barycentric>::FixedStepParameters EphemerisParameters() {
    return Ephemeris<Barycentric>::FixedStepParameters(
        SymmetricLinearMultistepIntegrator<QuinlanTremaine1990Order12,
                                           Position<Barycentric>>(),
        /*step=*/10 * Minute);
  }

  Ephemeris<Barycentric>::FixedStepParameters HistoryParameters() {
    return Ephemeris<Barycentric>::FixedStepParameters(
        SymmetricLinearMultistepIntegrator<Quinlan1999Order8A,
                                           Position<Barycentric>>(),
        /*step=*/10 * Second);
  }

  not_null<std::unique_ptr<SolarSystem<Barycentric>>> const solar_system_;
  not_null<std::unique_ptr<Ephemeris<Barycentric>>> const ephemeris_;
  not_null<MassiveBody const*> const earth_;
  not_null<std::unique_ptr<NavigationFrame>> const earth_centred_inertial_;
  DiscreteTrajectory<Barycentric> goes_8_trajectory_;

};

}  // namespace

void RunBenchmark(benchmark::State& state,
                  Perspective<Navigation, Camera> const& perspective) {
  Satellites satellites;
  Planetarium planetarium = satellites.MakePlanetarium(perspective);
  RP2Lines<Length, Camera> lines;
  int total_lines = 0;
  int total_points = 0;
  int iterations = 0;
  constexpr Instant now = "2000-01-21T04:41:30,5"_TT;
  while (state.KeepRunning()) {
    lines = planetarium.PlotMethod2(satellites.GOES8Trajectory().Begin(),
                                    satellites.GOES8Trajectory().End(),
                                    now,
                                    /*reverse=*/false);
    state.PauseTiming();
    total_lines += lines.size();
    for (auto const& line : lines) {
      total_points += line.size();
    }
    ++iterations;
    state.ResumeTiming();
  }
  Length min_x = Infinity<Length>();
  Length min_y = Infinity<Length>();
  Length max_x = Infinity<Length>();
  Length max_y = Infinity<Length>();
  for (auto const& line : lines) {
    for (auto const& point : line) {
      min_x = std::min(min_x, point.x());
      min_y = std::min(min_y, point.y());
      max_x = std::max(max_x, point.x());
      max_y = std::max(max_y, point.y());
    }
  }
  state.SetLabel(std::to_string(total_points / iterations) + " points in " +
                 std::to_string(total_lines / iterations) + " within [" +
                 DebugString(min_x) + ", " + DebugString(max_x) + "] × [" +
                 DebugString(min_y) + ", " + DebugString(max_y) + "]")
}

void BM_PlanetariumPlotMethod2NearPolarPerspective(
    benchmark::State& state,
    Perspective<Navigation, Camera> const& perspective) {
  RunBenchmark(state, PolarPerspective(near));
}

BENCHMARK(BM_PlanetariumPlotMethod2NearPolarPerspective);

void BM_PlanetariumPlotMethod2FarPolarPerspective(
    benchmark::State& state,
    Perspective<Navigation, Camera> const& perspective) {
  RunBenchmark(state, PolarPerspective(far));
}

BENCHMARK(BM_PlanetariumPlotMethod2FarPolarPerspective);

void BM_PlanetariumPlotMethod2NearEquatorialPerspective(
    benchmark::State& state,
    Perspective<Navigation, Camera> const& perspective) {
  RunBenchmark(state, EquatorialPerspective(near));
}

BENCHMARK(BM_PlanetariumPlotMethod2NearEquatorialPerspective);

void BM_PlanetariumPlotMethod2FarEquatorialPerspective(
    benchmark::State& state,
    Perspective<Navigation, Camera> const& perspective) {
  RunBenchmark(state, EquatorialPerspective(far));
}

BENCHMARK(BM_PlanetariumPlotMethod2FarEquatorialPerspective);

}  // namespace geometry
}  // namespace principia
<commit_msg>make it compile<commit_after>
// .\Release\x64\benchmarks.exe --benchmark_repetitions=3 --benchmark_filter=Planetarium  // NOLINT(whitespace/line_length)

#include "ksp_plugin/planetarium.hpp"

#include <random>
#include <vector>

#include "astronomy/time_scales.hpp"
#include "benchmark/benchmark.h"
#include "physics/body_centred_non_rotating_dynamic_frame.hpp"
#include "physics/solar_system.hpp"
#include "testing_utilities/solar_system_factory.hpp"

namespace principia {
namespace geometry {

using astronomy::operator""_UT1;
using astronomy::operator""_TT;
using base::make_not_null_unique;
using base::not_null;
using geometry::Bivector;
using geometry::Perspective;
using geometry::RigidTransformation;
using geometry::RP2Lines;
using geometry::Vector;
using geometry::Velocity;
using integrators::SymmetricLinearMultistepIntegrator;
using integrators::methods::Quinlan1999Order8A;
using integrators::methods::QuinlanTremaine1990Order12;
using ksp_plugin::Camera;
using ksp_plugin::Barycentric;
using ksp_plugin::Navigation;
using ksp_plugin::NavigationFrame;
using ksp_plugin::Planetarium;
using physics::BodyCentredNonRotatingDynamicFrame;
using physics::DegreesOfFreedom;
using physics::DiscreteTrajectory;
using physics::Ephemeris;
using physics::KeplerianElements;
using physics::KeplerOrbit;
using physics::MassiveBody;
using physics::MasslessBody;
using physics::SolarSystem;
using quantities::Angle;
using quantities::Cos;
using quantities::Infinity;
using quantities::Length;
using quantities::Sin;
using quantities::Time;
using quantities::si::ArcMinute;
using quantities::si::Day;
using quantities::si::Degree;
using quantities::si::Kilo;
using quantities::si::Metre;
using quantities::si::Milli;
using quantities::si::Minute;
using quantities::si::Radian;
using quantities::si::Second;
using testing_utilities::SolarSystemFactory;

namespace {

constexpr Length near = 40'000 * Kilo(Metre);
constexpr Length far = 400'000 * Kilo(Metre);
constexpr Length focal = 1 * Metre;

Perspective<Navigation, Camera> PolarPerspective(
    Length const distance_from_earth) {
  return {
      RigidTransformation<Navigation, Camera>(
          Navigation::origin + Displacement<Navigation>(
                                   {0 * Metre, 0 * Metre, distance_from_earth}),
          Camera::origin,
          Rotation<Navigation, Camera>(Vector<double, Navigation>({1, 0, 0}),
                                       Vector<double, Navigation>({0, 0, 1}),
                                       Bivector<double, Navigation>({0, -1, 0}))
              .Forget()),
      focal};
}

Perspective<Navigation, Camera> EquatorialPerspective(
    Length const distance_from_earth) {
  return {
      RigidTransformation<Navigation, Camera>(
          Navigation::origin + Displacement<Navigation>(
                                   {0 * Metre, distance_from_earth, 0 * Metre}),
          Camera::origin,
          Rotation<Navigation, Camera>(Vector<double, Navigation>({1, 0, 0}),
                                       Vector<double, Navigation>({0, 0, 1}),
                                       Bivector<double, Navigation>({0, -1, 0}))
              .Forget()),
      focal};
}

class Satellites {
 public:
  Satellites()
      : solar_system_(make_not_null_unique<SolarSystem<Barycentric>>(
            SOLUTION_DIR / "astronomy" / "sol_gravity_model.proto.txt",
            SOLUTION_DIR / "astronomy" /
                "sol_initial_state_jd_2451545_000000000.proto.txt",
            /*ignore_frame=*/true)),
        ephemeris_(
            solar_system_->MakeEphemeris(/*fitting_tolerance=*/1 * Milli(Metre),
                                         EphemerisParameters())),
        earth_(solar_system_->massive_body(
            *ephemeris_,
            SolarSystemFactory::name(SolarSystemFactory::Earth))),
        earth_centred_inertial_(
            make_not_null_unique<
                BodyCentredNonRotatingDynamicFrame<Barycentric, Navigation>>(
                ephemeris_.get(),
                earth_)) {
    // Two-line elements for GOES-8:
    // 1 23051U 94022A   00004.06628221 -.00000243  00000-0  00000-0 0  9630
    // 2 23051   0.4232  97.7420 0004776 192.8349 121.5613  1.00264613 28364
    constexpr Instant goes_8_epoch = "JD2451548.56628221"_UT1;
    KeplerianElements<Barycentric> goes_8_elements;
    goes_8_elements.inclination = 0.4232 * Degree;
    goes_8_elements.longitude_of_ascending_node = 97.7420 * Degree;
    goes_8_elements.eccentricity = 0.0004776;
    goes_8_elements.argument_of_periapsis = 192.8349 * Degree;
    goes_8_elements.mean_anomaly = 121.5613 * Degree;
    goes_8_elements.mean_motion = 1.00264613 * (2 * π * Radian / Day);
    KeplerOrbit<Barycentric> const goes_8_orbit(
        *earth_, MasslessBody{}, goes_8_elements, goes_8_epoch);
    goes_8_trajectory_.Append(
        goes_8_epoch,
        ephemeris_->trajectory(earth_)->EvaluateDegreesOfFreedom(goes_8_epoch) +
            goes_8_orbit.StateVectors(goes_8_epoch));

    ephemeris_->Prolong(goes_8_epoch);
    auto goes_8_instance = ephemeris_->NewInstance(
        {&goes_8_trajectory_},
        Ephemeris<Barycentric>::NoIntrinsicAccelerations,
        HistoryParameters());
    CHECK_OK(ephemeris_->FlowWithFixedStep(goes_8_epoch + 100 * Day,
                                           *goes_8_instance));
  }

  DiscreteTrajectory<Barycentric> const& GOES8Trajectory() const {
    return goes_8_trajectory_;
  }

  Planetarium MakePlanetarium(
      Perspective<Navigation, Camera> const& perspective) const {
    // No dark area, human visual acuity, wide field of view.
    Planetarium::Parameters parameters(
        /*sphere_radius_multiplier=*/1,
        /*angular_resolution=*/0.4 * ArcMinute,
        /*field_of_view=*/90 * Degree);
    return Planetarium(parameters,
                       perspective,
                       ephemeris_.get(),
                       earth_centred_inertial_.get());
  }

 private:
  Ephemeris<Barycentric>::FixedStepParameters EphemerisParameters() {
    return Ephemeris<Barycentric>::FixedStepParameters(
        SymmetricLinearMultistepIntegrator<QuinlanTremaine1990Order12,
                                           Position<Barycentric>>(),
        /*step=*/10 * Minute);
  }

  Ephemeris<Barycentric>::FixedStepParameters HistoryParameters() {
    return Ephemeris<Barycentric>::FixedStepParameters(
        SymmetricLinearMultistepIntegrator<Quinlan1999Order8A,
                                           Position<Barycentric>>(),
        /*step=*/10 * Second);
  }

  not_null<std::unique_ptr<SolarSystem<Barycentric>>> const solar_system_;
  not_null<std::unique_ptr<Ephemeris<Barycentric>>> const ephemeris_;
  not_null<MassiveBody const*> const earth_;
  not_null<std::unique_ptr<NavigationFrame>> const earth_centred_inertial_;
  DiscreteTrajectory<Barycentric> goes_8_trajectory_;

};

}  // namespace

void RunBenchmark(benchmark::State& state,
                  Perspective<Navigation, Camera> const& perspective) {
  Satellites satellites;
  Planetarium planetarium = satellites.MakePlanetarium(perspective);
  RP2Lines<Length, Camera> lines;
  int total_lines = 0;
  int total_points = 0;
  int iterations = 0;
  constexpr Instant now = "2000-01-21T04:41:30,5"_TT;
  while (state.KeepRunning()) {
    lines = planetarium.PlotMethod2(satellites.GOES8Trajectory().Begin(),
                                    satellites.GOES8Trajectory().End(),
                                    now,
                                    /*reverse=*/false);
    state.PauseTiming();
    total_lines += lines.size();
    for (auto const& line : lines) {
      total_points += line.size();
    }
    ++iterations;
    state.ResumeTiming();
  }
  Length min_x = Infinity<Length>();
  Length min_y = Infinity<Length>();
  Length max_x = Infinity<Length>();
  Length max_y = Infinity<Length>();
  for (auto const& line : lines) {
    for (auto const& point : line) {
      min_x = std::min(min_x, point.x());
      min_y = std::min(min_y, point.y());
      max_x = std::max(max_x, point.x());
      max_y = std::max(max_y, point.y());
    }
  }
  state.SetLabel(std::to_string(total_points / iterations) + " points in " +
                 std::to_string(total_lines / iterations) + " within [" +
                 DebugString(min_x) + ", " + DebugString(max_x) + "] × [" +
                 DebugString(min_y) + ", " + DebugString(max_y) + "]");
}

void BM_PlanetariumPlotMethod2NearPolarPerspective(benchmark::State& state) {
  RunBenchmark(state, PolarPerspective(near));
}

BENCHMARK(BM_PlanetariumPlotMethod2NearPolarPerspective);

void BM_PlanetariumPlotMethod2FarPolarPerspective(benchmark::State& state) {
  RunBenchmark(state, PolarPerspective(far));
}

BENCHMARK(BM_PlanetariumPlotMethod2FarPolarPerspective);

void BM_PlanetariumPlotMethod2NearEquatorialPerspective(
    benchmark::State& state) {
  RunBenchmark(state, EquatorialPerspective(near));
}

BENCHMARK(BM_PlanetariumPlotMethod2NearEquatorialPerspective);

void BM_PlanetariumPlotMethod2FarEquatorialPerspective(
    benchmark::State& state) {
  RunBenchmark(state, EquatorialPerspective(far));
}

BENCHMARK(BM_PlanetariumPlotMethod2FarEquatorialPerspective);

}  // namespace geometry
}  // namespace principia
<|endoftext|>


<commit_before>#include "psinc/Transport.h"
#include <emergent/logger/Logger.hpp>
#include <regex>
#include <set>

using namespace std;
using namespace emergent;

#define WRITE_PIPE	0x03
#define READ_PIPE	0x81

namespace psinc
{
	Transport::Transport()
	{
		libusb_init(&this->context);

		this->legacy = !libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG);
	}

	Transport::~Transport()
	{
		this->Release();

		if (!this->legacy)
		{
			libusb_hotplug_deregister_callback(this->context, this->hotplug);
		}

		libusb_exit(this->context);
	}

	bool Transport::Initialise(uint16_t product, string serial, std::function<void(bool)> onConnection, int timeout)
	{
		this->serial		= serial;
		this->product		= product;
		this->onConnection	= onConnection;
		this->timeout		= timeout;

		if (!this->legacy)
		{
			libusb_hotplug_deregister_callback(this->context, this->hotplug);
			this->registered = false;
		}
		else Log::Info("USB hotplug not supported on this platform, running in legacy mode");

		return true;
	}

	bool Transport::Connected()
	{
		return this->handle;
	}

	int LIBUSB_CALL OnHotplug(libusb_context *context, libusb_device *device, libusb_hotplug_event event, void *data)
	{
		reinterpret_cast<Transport *>(data)->pending.enqueue({ device, event });
		return 0;
	}

	void Transport::Poll(int time)
	{
		if (this->legacy)
		{
			if (!this->handle) this->LegacyConnect();
			return;
		}

		if (!this->registered)
		{
			auto events = (libusb_hotplug_event)(LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED | LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT);

			if (libusb_hotplug_register_callback(this->context, events, LIBUSB_HOTPLUG_ENUMERATE, LIBUSB_HOTPLUG_MATCH_ANY, this->product, LIBUSB_HOTPLUG_MATCH_ANY, &OnHotplug, this, &this->hotplug))
			{
				Log::Error("Unable to register USB hotplug callback");
				return;
			}

			this->registered = true;
		}

		struct timeval tv = { 0, time * 1000 };
		libusb_handle_events_timeout_completed(this->context, &tv, nullptr);

		Pending item;
		if (this->pending.try_dequeue(item))
		{
			if (Valid(item.device, this->product))
			{
				if (item.event == LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT && this->handle)
				{
					if (item.device == libusb_get_device(this->handle))
					{
						this->cs.lock();
							this->Release();
						this->cs.unlock();

						if (this->onConnection) this->onConnection(false);
					}
				}

				if (item.event == LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED && !this->handle)
				{
					if (this->Claim(item.device) && this->onConnection)
					{
						this->onConnection(true);
					}
				}
			}
		}
	}

	bool Transport::Match(libusb_device_handle *device, int index)
	{
		unsigned char data[128];

		libusb_get_string_descriptor_ascii(device, index, data, 128);

		this->id = reinterpret_cast<char *>(data);

		return this->serial.empty() ? true : regex_match(this->id, regex(this->serial));
	}

	bool Transport::Valid(libusb_device *device, uint16_t product)
	{
		static const set<uint16_t> VENDORS = { 0x0525 };
		libusb_device_descriptor descriptor;

		return libusb_get_device_descriptor(device, &descriptor) == 0
			&& VENDORS.count(descriptor.idVendor)
			&& descriptor.idProduct == product;
	}

	void Transport::LegacyConnect()
	{
		libusb_device **list;
		libusb_get_device_list(this->context, &list);

		// Loop through the list of connected USB devices
		for (libusb_device **device = list; *device; device++)
		{
			if (Valid(*device, this->product))
			{
				// If a particular device matches the known vendor, product ID then attempt to claim it.
				if (this->Claim(*device))
				{
					if (this->onConnection) this->onConnection(true);
					break;
				}
			}
		}

		libusb_free_device_list(list, 1);
	}

	bool Transport::Claim(libusb_device *device)
	{
		lock_guard<mutex> lock(this->cs);

		libusb_device_descriptor descriptor;

		if (libusb_get_device_descriptor(device, &descriptor) == 0)
		{
			// Attempt to open and claim the device
			if (libusb_open(device, &this->handle) == 0)
			{
				// If a serial number has been specified then check that this device matches it
				if (this->Match(this->handle, descriptor.iSerialNumber))
				{
					// TODO: Disabled this while PSI is solving an issue with
					// the camera that causes it to get stuck in a strange state.
					//if (libusb_set_configuration(this->handle, 1) == 0)
					{
						if (libusb_claim_interface(this->handle, 0) == 0)
						{
							Log::Info("%u: USB device claimed - %s", Timestamp::LogTime(), this->id);
							return true;
						}
					}
				}

				// Claiming was unsuccessful so clean up
				libusb_close(this->handle);
				this->handle 	= nullptr;
				this->id		= "";
			}
		}

		return false;
	}

	vector<string> Transport::List(uint16_t product)
	{
		vector<string> result;

		unsigned char data[128];
		libusb_device **list;
		libusb_device_descriptor descriptor;
		libusb_device_handle *handle;
		libusb_context *context = nullptr;

		libusb_init(&context);
		libusb_get_device_list(context, &list);

		for (libusb_device **device = list; *device; device++)
		{
			if (Valid(*device, product) && libusb_get_device_descriptor(*device, &descriptor) == 0)
			{
				if (libusb_open(*device, &handle) == 0)
				{
					libusb_get_string_descriptor_ascii(handle, descriptor.iSerialNumber, data, 128);

					result.push_back(reinterpret_cast<char *>(data));

					libusb_close(handle);
				}
			}
		}

		libusb_exit(context);

		return result;
	}

	void Transport::Disconnect()
	{
		lock_guard<mutex> lock(this->cs);

		this->Release();
	}

	void Transport::Release()
	{
		if (this->handle)
		{
			// Release the device and close the handle
			libusb_release_interface(this->handle, 0);
			libusb_close(this->handle);

			Log::Info("%u: USB deviced released - %s", Timestamp::LogTime(), this->id);

			this->handle	= nullptr;
			this->id		= "";
		}
	}

	bool Transport::Reset()
	{
		lock_guard<mutex> lock(this->cs);

		if (this->handle)
		{
			if (libusb_reset_device(this->handle) != 0)
			{
				// Something has gone wrong with the reset and so the device appears
				// as if it has been reconnected. Therefore this handle is invalid
				// and must be cleaned up.
				this->Release();

				Log::Error("Problem resetting USB device, handle has been released");
			}
		}

		return this->handle;
	}

	bool Transport::Transfer(Buffer<byte> *send, Buffer<byte> *receive, atomic<bool> &waiting, bool check, bool truncate)
	{
		lock_guard<mutex> lock(this->cs);

		return this->handle ? this->Transfer(send, true, check, false) && (waiting = true) && this->Transfer(receive, false, check, truncate) : false;
	}

	bool Transport::Transfer(Buffer<byte> *buffer, bool write, bool check, bool truncate)
	{
		if (buffer)
		{
			int transferred;
			bool result	= false;
			int err		= libusb_bulk_transfer(this->handle, write ? WRITE_PIPE : READ_PIPE, *buffer, buffer->Size(), &transferred, this->timeout);

			if (!err)
			{
				result = (write || check) ? transferred == buffer->Size() : true;

				// When requested, truncate the buffer to the size of data actually received.
				if (!write && result && truncate)
				{
					buffer->Truncate(transferred);
				}

				if (!result)
				{
					Log::Error(
						"%u: USB device %s - Incomplete transfer when %s (%d of %d bytes)",
						Timestamp::LogTime(),
						this->id,
						write ? "writing" : "reading",
						transferred,
						buffer->Size()
					);
				}
			}
			else if (this->legacy && (err == LIBUSB_ERROR_NO_DEVICE || err == LIBUSB_ERROR_IO))
			{
				Log::Error("%u: USB device %s - Device has been disconnected", Timestamp::LogTime(), this->id);
				this->Release();
			}
			else
			{
				Log::Error("%u: USB device %s - Transfer error %d when %s", Timestamp::LogTime(), this->id, err, write ? "writing" : "reading");
			}

			return result;
		}

		return true;
	}
}

<commit_msg>Added new PSI vendor ID. Original one remains for backwards compatibility with previous generation cameras.<commit_after>#include "psinc/Transport.h"
#include <emergent/logger/Logger.hpp>
#include <regex>
#include <set>

using namespace std;
using namespace emergent;

#define WRITE_PIPE	0x03
#define READ_PIPE	0x81

namespace psinc
{
	Transport::Transport()
	{
		libusb_init(&this->context);

		this->legacy = !libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG);
	}

	Transport::~Transport()
	{
		this->Release();

		if (!this->legacy)
		{
			libusb_hotplug_deregister_callback(this->context, this->hotplug);
		}

		libusb_exit(this->context);
	}

	bool Transport::Initialise(uint16_t product, string serial, std::function<void(bool)> onConnection, int timeout)
	{
		this->serial		= serial;
		this->product		= product;
		this->onConnection	= onConnection;
		this->timeout		= timeout;

		if (!this->legacy)
		{
			libusb_hotplug_deregister_callback(this->context, this->hotplug);
			this->registered = false;
		}
		else Log::Info("USB hotplug not supported on this platform, running in legacy mode");

		return true;
	}

	bool Transport::Connected()
	{
		return this->handle;
	}

	int LIBUSB_CALL OnHotplug(libusb_context *context, libusb_device *device, libusb_hotplug_event event, void *data)
	{
		reinterpret_cast<Transport *>(data)->pending.enqueue({ device, event });
		return 0;
	}

	void Transport::Poll(int time)
	{
		if (this->legacy)
		{
			if (!this->handle) this->LegacyConnect();
			return;
		}

		if (!this->registered)
		{
			auto events = (libusb_hotplug_event)(LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED | LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT);

			if (libusb_hotplug_register_callback(this->context, events, LIBUSB_HOTPLUG_ENUMERATE, LIBUSB_HOTPLUG_MATCH_ANY, this->product, LIBUSB_HOTPLUG_MATCH_ANY, &OnHotplug, this, &this->hotplug))
			{
				Log::Error("Unable to register USB hotplug callback");
				return;
			}

			this->registered = true;
		}

		struct timeval tv = { 0, time * 1000 };
		libusb_handle_events_timeout_completed(this->context, &tv, nullptr);

		Pending item;
		if (this->pending.try_dequeue(item))
		{
			if (Valid(item.device, this->product))
			{
				if (item.event == LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT && this->handle)
				{
					if (item.device == libusb_get_device(this->handle))
					{
						this->cs.lock();
							this->Release();
						this->cs.unlock();

						if (this->onConnection) this->onConnection(false);
					}
				}

				if (item.event == LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED && !this->handle)
				{
					if (this->Claim(item.device) && this->onConnection)
					{
						this->onConnection(true);
					}
				}
			}
		}
	}

	bool Transport::Match(libusb_device_handle *device, int index)
	{
		unsigned char data[128];

		libusb_get_string_descriptor_ascii(device, index, data, 128);

		this->id = reinterpret_cast<char *>(data);

		return this->serial.empty() ? true : regex_match(this->id, regex(this->serial));
	}

	bool Transport::Valid(libusb_device *device, uint16_t product)
	{
		static const set<uint16_t> VENDORS = { 0x2dd8, 0x0525 };
		libusb_device_descriptor descriptor;

		return libusb_get_device_descriptor(device, &descriptor) == 0
			&& VENDORS.count(descriptor.idVendor)
			&& descriptor.idProduct == product;
	}

	void Transport::LegacyConnect()
	{
		libusb_device **list;
		libusb_get_device_list(this->context, &list);

		// Loop through the list of connected USB devices
		for (libusb_device **device = list; *device; device++)
		{
			if (Valid(*device, this->product))
			{
				// If a particular device matches the known vendor, product ID then attempt to claim it.
				if (this->Claim(*device))
				{
					if (this->onConnection) this->onConnection(true);
					break;
				}
			}
		}

		libusb_free_device_list(list, 1);
	}

	bool Transport::Claim(libusb_device *device)
	{
		lock_guard<mutex> lock(this->cs);

		libusb_device_descriptor descriptor;

		if (libusb_get_device_descriptor(device, &descriptor) == 0)
		{
			// Attempt to open and claim the device
			if (libusb_open(device, &this->handle) == 0)
			{
				// If a serial number has been specified then check that this device matches it
				if (this->Match(this->handle, descriptor.iSerialNumber))
				{
					// TODO: Disabled this while PSI is solving an issue with
					// the camera that causes it to get stuck in a strange state.
					//if (libusb_set_configuration(this->handle, 1) == 0)
					{
						if (libusb_claim_interface(this->handle, 0) == 0)
						{
							Log::Info("%u: USB device claimed - %s", Timestamp::LogTime(), this->id);
							return true;
						}
					}
				}

				// Claiming was unsuccessful so clean up
				libusb_close(this->handle);
				this->handle 	= nullptr;
				this->id		= "";
			}
		}

		return false;
	}

	vector<string> Transport::List(uint16_t product)
	{
		vector<string> result;

		unsigned char data[128];
		libusb_device **list;
		libusb_device_descriptor descriptor;
		libusb_device_handle *handle;
		libusb_context *context = nullptr;

		libusb_init(&context);
		libusb_get_device_list(context, &list);

		for (libusb_device **device = list; *device; device++)
		{
			if (Valid(*device, product) && libusb_get_device_descriptor(*device, &descriptor) == 0)
			{
				if (libusb_open(*device, &handle) == 0)
				{
					libusb_get_string_descriptor_ascii(handle, descriptor.iSerialNumber, data, 128);

					result.push_back(reinterpret_cast<char *>(data));

					libusb_close(handle);
				}
			}
		}

		libusb_exit(context);

		return result;
	}

	void Transport::Disconnect()
	{
		lock_guard<mutex> lock(this->cs);

		this->Release();
	}

	void Transport::Release()
	{
		if (this->handle)
		{
			// Release the device and close the handle
			libusb_release_interface(this->handle, 0);
			libusb_close(this->handle);

			Log::Info("%u: USB deviced released - %s", Timestamp::LogTime(), this->id);

			this->handle	= nullptr;
			this->id		= "";
		}
	}

	bool Transport::Reset()
	{
		lock_guard<mutex> lock(this->cs);

		if (this->handle)
		{
			if (libusb_reset_device(this->handle) != 0)
			{
				// Something has gone wrong with the reset and so the device appears
				// as if it has been reconnected. Therefore this handle is invalid
				// and must be cleaned up.
				this->Release();

				Log::Error("Problem resetting USB device, handle has been released");
			}
		}

		return this->handle;
	}

	bool Transport::Transfer(Buffer<byte> *send, Buffer<byte> *receive, atomic<bool> &waiting, bool check, bool truncate)
	{
		lock_guard<mutex> lock(this->cs);

		return this->handle ? this->Transfer(send, true, check, false) && (waiting = true) && this->Transfer(receive, false, check, truncate) : false;
	}

	bool Transport::Transfer(Buffer<byte> *buffer, bool write, bool check, bool truncate)
	{
		if (buffer)
		{
			int transferred;
			bool result	= false;
			int err		= libusb_bulk_transfer(this->handle, write ? WRITE_PIPE : READ_PIPE, *buffer, buffer->Size(), &transferred, this->timeout);

			if (!err)
			{
				result = (write || check) ? transferred == buffer->Size() : true;

				// When requested, truncate the buffer to the size of data actually received.
				if (!write && result && truncate)
				{
					buffer->Truncate(transferred);
				}

				if (!result)
				{
					Log::Error(
						"%u: USB device %s - Incomplete transfer when %s (%d of %d bytes)",
						Timestamp::LogTime(),
						this->id,
						write ? "writing" : "reading",
						transferred,
						buffer->Size()
					);
				}
			}
			else if (this->legacy && (err == LIBUSB_ERROR_NO_DEVICE || err == LIBUSB_ERROR_IO))
			{
				Log::Error("%u: USB device %s - Device has been disconnected", Timestamp::LogTime(), this->id);
				this->Release();
			}
			else
			{
				Log::Error("%u: USB device %s - Transfer error %d when %s", Timestamp::LogTime(), this->id, err, write ? "writing" : "reading");
			}

			return result;
		}

		return true;
	}
}

<|endoftext|>