<commit_before>
/**
* @file BallDetector.cpp
*
* Implementation of class BallDetector
*
*/

#include "BallDetector.h"

#include "Tools/DataStructures/ArrayQueue.h"
#include "Tools/CameraGeometry.h"

#include <Representations/Infrastructure/CameraInfoConstants.h>

#include "Tools/Math/Geometry.h"
#include "Tools/ImageProcessing/BresenhamLineScan.h"
#include "Tools/ImageProcessing/MaximumScan.h"
#include "Tools/ImageProcessing/Filter.h"

BallDetector::BallDetector()
{
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:markPeakScanFull", "mark the scanned points in image", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:markPeakScan", "mark the scanned points in image", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:markPeak", "mark found maximum red peak in image", false);

  DEBUG_REQUEST_REGISTER("Vision:BallDetector:drawScanlines", "", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:drawScanEndPoints", "", false);

  DEBUG_REQUEST_REGISTER("Vision:BallDetector:draw_ball_estimated","..", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:draw_ball_radius_match", "..", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:draw_ball","..", false);

  getDebugParameterList().add(&params);
}

BallDetector::~BallDetector()
{
  getDebugParameterList().remove(&params);
}

void BallDetector::execute(CameraInfo::CameraID id)
{
  cameraID = id;

  getBallPercept().reset();

  // STEP 1: find the starting point for the search
  listOfRedPoints.clear();
  //??? single condition
  if(!findMaximumRedPoint(listOfRedPoints) || listOfRedPoints.empty()) {
    return;
  }

  bool ballFound = false;

  //NOTE: the points are sorted - the most red points are at the end
  double radius = -1;
  Vector2d center;
  //Liste von rotenPunkten durchgehen
  for(int i = (int)listOfRedPoints.size()-1; i >= 0 ; i--)
  {
    const Vector2i& point = listOfRedPoints[i];

    // the point is within the previously calculated ball
    if(radius > 0 && radius*radius*2 > (center - Vector2d(point)).abs2()) {
      continue;
    }

    //3d Projection -> Aus der Entfernung der Punktes die ungefhre Ballposition berechenen
    double estimatedRadius = estimatedBallRadius(point);

    DEBUG_REQUEST("Vision:BallDetector:draw_ball_estimated",
      //estimatedRadius <=0 possible? will be a problem  in "radius < 2*estimatedRadius"
      if(estimatedRadius > 0) {
        CIRCLE_PX(ColorClasses::white, point.x, point.y, (int)(estimatedRadius+0.5));
      }
    );

    ballEndPoints.clear();
    bool goodBallCandidateFound = spiderScan(point, ballEndPoints);

    if(goodBallCandidateFound && Geometry::calculateCircle(ballEndPoints, center, radius))
    {
      DEBUG_REQUEST("Vision:BallDetector:draw_ball_radius_match",
        CIRCLE_PX(ColorClasses::yellow, (int)(center.x+0.5), (int)(center.y+0.5), (int)(radius+0.5));
      );

      if(radius > 0 && radius < 2*estimatedRadius) {
        if(sanityCheck(center, radius))
        {
          ballFound = true;
          calculateBallPercept(center, radius);
          break;
        }
      }
    }
  }

  if(ballFound) {

    /*
    Vector2d betterCenter;
    double betterRadius;
    ckecknearBall(center, betterCenter, betterRadius);

    calculateBallPercept(betterCenter, betterRadius);
    */
    //calculateBallPercept(center, radius);
  }
}

bool BallDetector::findMaximumRedPoint(std::vector<Vector2i>& points) const
{
  //
  // STEP I: find the maximal height minY to be scanned in the image
  //
  if(!getFieldPercept().valid) {
    return false;
  }

  const FieldPercept::FieldPoly& fieldPolygon = getFieldPercept().getValidField();

  // find the top point of the polygon
  int minY = getImage().height();
  for(int i = 0; i < fieldPolygon.length ; i++)
  {
    if(fieldPolygon.points[i].y < minY && fieldPolygon.points[i].y >= 0) {
      minY = fieldPolygon.points[i].y;
    }
  }

  // double check: polygon is empty
  if(minY == (int)getImage().height() || minY < 0) {
    return false;
  }

  //
  // STEP II: calculate the step size for the scan
  //
  int stepSizeAdjusted = params.stepSize;
  if(getCameraMatrix().rotation.getYAngle() > Math::fromDegrees(40))
  {
    stepSizeAdjusted *= 3;
  }
  else if(getCameraMatrix().rotation.getYAngle() > Math::fromDegrees(10))
  {
    stepSizeAdjusted *= 2;
  }

  int maxRedPeak = 128;//getFieldColorPercept().range.getMax().v; // initialize with the maximal red croma of the field color
  Vector2i point;
  Pixel pixel;
  Vector2i redPeak;

  for(point.y = (int) (getImage().height() - 3); point.y > minY; point.y -= stepSizeAdjusted) {
    for(point.x = 0; point.x < (int) getImage().width(); point.x += stepSizeAdjusted)
    {
      getImage().get(point.x, point.y, pixel);

      DEBUG_REQUEST("Vision:BallDetector:markPeakScanFull",
        POINT_PX(ColorClasses::blue, point.x, point.y);
      );

      if
      (
        maxRedPeak < pixel.v && // "v" is the croma RED channel
        isOrange(pixel) &&
        fieldPolygon.isInside_inline(point) // only points inside the field polygon
        //&& !getGoalPostHistograms().isPostColor(pixel) // ball is not goal like colored
        //&& getGoalPostHistograms().histogramV.mean + params.minOffsetToGoalV < pixel.v
      )
      {
        //if(ckecknearBall(point) > 1)
        {
          maxRedPeak = (int)pixel.v;
          redPeak = point;
          points.push_back(point);
        }
      }

      DEBUG_REQUEST("Vision:BallDetector:markPeakScan",
        if
        (
          isOrange(pixel) &&
          fieldPolygon.isInside_inline(point) // only points inside the field polygon
        )
        {
          POINT_PX(ColorClasses::red, point.x, point.y);
        }
      );
    }
  }

  DEBUG_REQUEST("Vision:BallDetector:markPeak",
    LINE_PX(ColorClasses::skyblue, redPeak.x-10, redPeak.y, redPeak.x+10, redPeak.y);
    LINE_PX(ColorClasses::skyblue, redPeak.x, redPeak.y-10, redPeak.x, redPeak.y+10);
    CIRCLE_PX(ColorClasses::white, redPeak.x, redPeak.y, 5);
  );

  // maxRedPeak is larger than its initial value
  return maxRedPeak > 128;//getFieldColorPercept().range.getMax().v;
}

bool BallDetector::spiderScan(const Vector2i& start, std::vector<Vector2i>& endPoints) const
{
  int goodBorderPointCount = 0;
  goodBorderPointCount += scanForEdges(start, Vector2d( 1, 0), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d(-1, 0), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d( 0, 1), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d( 0,-1), endPoints);

  goodBorderPointCount += scanForEdges(start, Vector2d( 1, 1).normalize(), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d(-1, 1).normalize(), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d( 1,-1).normalize(), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d(-1,-1).normalize(), endPoints);

  DEBUG_REQUEST("Vision:BallDetector:drawScanEndPoints",
    for(size_t i = 0; i < endPoints.size(); i++)
    {
      ColorClasses::Color col = ColorClasses::green;
      if(goodBorderPointCount == 0)
      {
        col = ColorClasses::red;
      }
      POINT_PX(col, endPoints[i].x, endPoints[i].y);
    }
  );
  return goodBorderPointCount > 0;
}

bool BallDetector::scanForEdges(const Vector2i& start, const Vector2d& direction, std::vector<Vector2i>& points) const
{
  Vector2i point(start);
  BresenhamLineScan scanner(point, direction, getImage().cameraInfo);

  // initialize the scanner
  Vector2i peak_point_min(start);
  MaximumScan<Vector2i,double> negativeScan(peak_point_min, params.thresholdGradientUV);

  Filter<Prewitt3x1, Vector2i, double, 3> filter;

  Pixel pixel;
  double stepLength = 0;
  Vector2i lastPoint(point); // needed for step length

  //int max_length = 6;
  //int i = 0;
  while(scanner.getNextWithCheck(point)) // i < max_length*/)
  {
    getImage().get(point.x, point.y, pixel);
    int f_y = (int)pixel.v - (int)pixel.u;

    // hack
    /*
    if(pixel.v < getFieldColorPercept().range.getMax().v)
      i++;
    else
      i = 0;
      */

    filter.add(point, f_y);
    if(!filter.ready()) {
      // assume the step length is constant, so we only calculate it in the starting phase of the filter
      stepLength += Vector2d(point - lastPoint).abs();
      lastPoint = point;
      ASSERT(stepLength > 0);
      continue;
    }

    DEBUG_REQUEST("Vision:BallDetector:drawScanlines",
      POINT_PX(ColorClasses::blue, point.x, point.y);
    );

    // jump down found
    // NOTE: we scale the filter value with the stepLength to acount for diagonal scans
    if(negativeScan.add(filter.point(), -filter.value()/stepLength))
    {
      DEBUG_REQUEST("Vision:BallDetector:drawScanlines",
        POINT_PX(ColorClasses::pink, peak_point_min.x, peak_point_min.y);
      );
      points.push_back(peak_point_min);
      return pixel.y < params.maxBorderBrightness;
    }
  }//end while

  return false; //getFieldColorPercept().isFieldColor(pixel);
}//end scanForEdges

void BallDetector::calculateBallPercept(const Vector2i& center, double radius)
{
  // calculate the ball
    bool projectionOK = CameraGeometry::imagePixelToFieldCoord(
		  getCameraMatrix(),
		  getImage().cameraInfo,
		  center.x,
		  center.y,
		  getFieldInfo().ballRadius,
		  getBallPercept().bearingBasedOffsetOnField);

    // HACK: don't take to far balls
    projectionOK = projectionOK && getBallPercept().bearingBasedOffsetOnField.abs2() < 10000*10000; // closer than 10m

    // HACK: if the ball center is in image it has to be in the field polygon
    Vector2d ballPointToCheck(center.x, center.y - 5);
    projectionOK = projectionOK &&
      (!getImage().isInside((int)(ballPointToCheck.x+0.5), (int)(ballPointToCheck.y+0.5)) ||
        getFieldPercept().getValidField().isInside(ballPointToCheck));

    if(projectionOK)
    {
		  getBallPercept().radiusInImage = radius;
		  getBallPercept().centerInImage = center;
		  getBallPercept().ballWasSeen = projectionOK;
		  getBallPercept().frameInfoWhenBallWasSeen = getFrameInfo();

      DEBUG_REQUEST("Vision:BallDetector:draw_ball",
        CIRCLE_PX(ColorClasses::orange, center.x, center.y, (int)(radius+0.5));
		  );
    }
}

double BallDetector::estimatedBallRadius(const Vector2i& point) const
{
  Vector2d pointOnField;
  if(!CameraGeometry::imagePixelToFieldCoord(
		  getCameraMatrix(),
		  getImage().cameraInfo,
		  point.x,
		  point.y,
		  getFieldInfo().ballRadius,
		  pointOnField))
  {
    return -1;
  }

  Vector3d d = getCameraMatrix()*Vector3d(pointOnField.x, pointOnField.y, getFieldInfo().ballRadius);
  double cameraBallDistance = d.abs();
  if(cameraBallDistance > getFieldInfo().ballRadius) {
    double a = asin(getFieldInfo().ballRadius / d.abs());
    return a / getImage().cameraInfo.getOpeningAngleHeight() * getImage().cameraInfo.resolutionHeight;
  }

  return -1;
}

void BallDetector::estimateCircleSimple(const std::vector<Vector2i>& endPoints, Vector2d& center, double& radius) const
{
  Vector2d sum;
  for(size_t i = 0; i < endPoints.size(); i++) {
    sum += endPoints[i];
  }

  center = sum/((double)endPoints.size());

  RingBufferWithSum<double, 8> radiusBuffer;
  for(size_t i = 0; i < endPoints.size(); i++) {
    radiusBuffer.add((center - Vector2d(endPoints[i])).abs2());
  }

  radius = sqrt(radiusBuffer.getAverage());
}

bool BallDetector::sanityCheck(const Vector2i& center, double radius)
{
  size_t sampleSize = 21;
  double maxSquareSize = sqrt(radius*radius/2.0);
  int searchSize = int(maxSquareSize/2.0);
  int width = static_cast<int>(getImage().width());
  int height = static_cast<int>(getImage().height());

  double yMean = 0.0;
  double uMean = 0.0;
  double vMean = 0.0;
  Pixel pixel;
  for(size_t i = 0; i < sampleSize; i++)
  {
    int x = Math::clamp(Math::random(center.x - searchSize, center.y + searchSize), 0, width);
    int y = Math::clamp(Math::random(center.x - searchSize, center.y + searchSize), 0, height);
    getImage().get(x, y, pixel);
    yMean += pixel.y;
    uMean += pixel.u;
    vMean += pixel.v;
  }
  yMean /= static_cast<double>(sampleSize);
  uMean /= static_cast<double>(sampleSize);
  vMean /= static_cast<double>(sampleSize);

  pixel.y = static_cast<unsigned char>(yMean);
  pixel.u = static_cast<unsigned char>(uMean);
  pixel.v = static_cast<unsigned char>(vMean);

  return isOrange(pixel);
}
<commit_msg>worked out bugs in clamping<commit_after>
/**
* @file BallDetector.cpp
*
* Implementation of class BallDetector
*
*/

#include "BallDetector.h"

#include "Tools/DataStructures/ArrayQueue.h"
#include "Tools/CameraGeometry.h"

#include <Representations/Infrastructure/CameraInfoConstants.h>

#include "Tools/Math/Geometry.h"
#include "Tools/ImageProcessing/BresenhamLineScan.h"
#include "Tools/ImageProcessing/MaximumScan.h"
#include "Tools/ImageProcessing/Filter.h"

BallDetector::BallDetector()
{
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:markPeakScanFull", "mark the scanned points in image", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:markPeakScan", "mark the scanned points in image", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:markPeak", "mark found maximum red peak in image", false);

  DEBUG_REQUEST_REGISTER("Vision:BallDetector:drawScanlines", "", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:drawScanEndPoints", "", false);

  DEBUG_REQUEST_REGISTER("Vision:BallDetector:draw_ball_estimated","..", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:draw_ball_radius_match", "..", false);
  DEBUG_REQUEST_REGISTER("Vision:BallDetector:draw_ball","..", false);

  getDebugParameterList().add(&params);
}

BallDetector::~BallDetector()
{
  getDebugParameterList().remove(&params);
}

void BallDetector::execute(CameraInfo::CameraID id)
{
  cameraID = id;

  getBallPercept().reset();

  // STEP 1: find the starting point for the search
  listOfRedPoints.clear();
  //??? single condition
  if(!findMaximumRedPoint(listOfRedPoints) || listOfRedPoints.empty()) {
    return;
  }

  bool ballFound = false;

  //NOTE: the points are sorted - the most red points are at the end
  double radius = -1;
  Vector2d center;
  //Liste von rotenPunkten durchgehen
  for(int i = (int)listOfRedPoints.size()-1; i >= 0 ; i--)
  {
    const Vector2i& point = listOfRedPoints[i];

    // the point is within the previously calculated ball
    if(radius > 0 && radius*radius*2 > (center - Vector2d(point)).abs2()) {
      continue;
    }

    //3d Projection -> Aus der Entfernung der Punktes die ungefhre Ballposition berechenen
    double estimatedRadius = estimatedBallRadius(point);

    DEBUG_REQUEST("Vision:BallDetector:draw_ball_estimated",
      //estimatedRadius <=0 possible? will be a problem  in "radius < 2*estimatedRadius"
      if(estimatedRadius > 0) {
        CIRCLE_PX(ColorClasses::white, point.x, point.y, (int)(estimatedRadius+0.5));
      }
    );

    ballEndPoints.clear();
    bool goodBallCandidateFound = spiderScan(point, ballEndPoints);

    if(goodBallCandidateFound && Geometry::calculateCircle(ballEndPoints, center, radius))
    {
      DEBUG_REQUEST("Vision:BallDetector:draw_ball_radius_match",
        CIRCLE_PX(ColorClasses::yellow, (int)(center.x+0.5), (int)(center.y+0.5), (int)(radius+0.5));
      );

      if(radius > 0 && radius < 2*estimatedRadius) {
        if(sanityCheck(center, radius))
        {
          ballFound = true;
          calculateBallPercept(center, radius);
          break;
        }
      }
    }
  }

  if(ballFound) {

    /*
    Vector2d betterCenter;
    double betterRadius;
    ckecknearBall(center, betterCenter, betterRadius);

    calculateBallPercept(betterCenter, betterRadius);
    */
    //calculateBallPercept(center, radius);
  }
}

bool BallDetector::findMaximumRedPoint(std::vector<Vector2i>& points) const
{
  //
  // STEP I: find the maximal height minY to be scanned in the image
  //
  if(!getFieldPercept().valid) {
    return false;
  }

  const FieldPercept::FieldPoly& fieldPolygon = getFieldPercept().getValidField();

  // find the top point of the polygon
  int minY = getImage().height();
  for(int i = 0; i < fieldPolygon.length ; i++)
  {
    if(fieldPolygon.points[i].y < minY && fieldPolygon.points[i].y >= 0) {
      minY = fieldPolygon.points[i].y;
    }
  }

  // double check: polygon is empty
  if(minY == (int)getImage().height() || minY < 0) {
    return false;
  }

  //
  // STEP II: calculate the step size for the scan
  //
  int stepSizeAdjusted = params.stepSize;
  if(getCameraMatrix().rotation.getYAngle() > Math::fromDegrees(40))
  {
    stepSizeAdjusted *= 3;
  }
  else if(getCameraMatrix().rotation.getYAngle() > Math::fromDegrees(10))
  {
    stepSizeAdjusted *= 2;
  }

  int maxRedPeak = 128;//getFieldColorPercept().range.getMax().v; // initialize with the maximal red croma of the field color
  Vector2i point;
  Pixel pixel;
  Vector2i redPeak;

  for(point.y = (int) (getImage().height() - 3); point.y > minY; point.y -= stepSizeAdjusted) {
    for(point.x = 0; point.x < (int) getImage().width(); point.x += stepSizeAdjusted)
    {
      getImage().get(point.x, point.y, pixel);

      DEBUG_REQUEST("Vision:BallDetector:markPeakScanFull",
        POINT_PX(ColorClasses::blue, point.x, point.y);
      );

      if
      (
        maxRedPeak < pixel.v && // "v" is the croma RED channel
        isOrange(pixel) &&
        fieldPolygon.isInside_inline(point) // only points inside the field polygon
        //&& !getGoalPostHistograms().isPostColor(pixel) // ball is not goal like colored
        //&& getGoalPostHistograms().histogramV.mean + params.minOffsetToGoalV < pixel.v
      )
      {
        //if(ckecknearBall(point) > 1)
        {
          maxRedPeak = (int)pixel.v;
          redPeak = point;
          points.push_back(point);
        }
      }

      DEBUG_REQUEST("Vision:BallDetector:markPeakScan",
        if
        (
          isOrange(pixel) &&
          fieldPolygon.isInside_inline(point) // only points inside the field polygon
        )
        {
          POINT_PX(ColorClasses::red, point.x, point.y);
        }
      );
    }
  }

  DEBUG_REQUEST("Vision:BallDetector:markPeak",
    LINE_PX(ColorClasses::skyblue, redPeak.x-10, redPeak.y, redPeak.x+10, redPeak.y);
    LINE_PX(ColorClasses::skyblue, redPeak.x, redPeak.y-10, redPeak.x, redPeak.y+10);
    CIRCLE_PX(ColorClasses::white, redPeak.x, redPeak.y, 5);
  );

  // maxRedPeak is larger than its initial value
  return maxRedPeak > 128;//getFieldColorPercept().range.getMax().v;
}

bool BallDetector::spiderScan(const Vector2i& start, std::vector<Vector2i>& endPoints) const
{
  int goodBorderPointCount = 0;
  goodBorderPointCount += scanForEdges(start, Vector2d( 1, 0), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d(-1, 0), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d( 0, 1), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d( 0,-1), endPoints);

  goodBorderPointCount += scanForEdges(start, Vector2d( 1, 1).normalize(), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d(-1, 1).normalize(), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d( 1,-1).normalize(), endPoints);
  goodBorderPointCount += scanForEdges(start, Vector2d(-1,-1).normalize(), endPoints);

  DEBUG_REQUEST("Vision:BallDetector:drawScanEndPoints",
    for(size_t i = 0; i < endPoints.size(); i++)
    {
      ColorClasses::Color col = ColorClasses::green;
      if(goodBorderPointCount == 0)
      {
        col = ColorClasses::red;
      }
      POINT_PX(col, endPoints[i].x, endPoints[i].y);
    }
  );
  return goodBorderPointCount > 0;
}

bool BallDetector::scanForEdges(const Vector2i& start, const Vector2d& direction, std::vector<Vector2i>& points) const
{
  Vector2i point(start);
  BresenhamLineScan scanner(point, direction, getImage().cameraInfo);

  // initialize the scanner
  Vector2i peak_point_min(start);
  MaximumScan<Vector2i,double> negativeScan(peak_point_min, params.thresholdGradientUV);

  Filter<Prewitt3x1, Vector2i, double, 3> filter;

  Pixel pixel;
  double stepLength = 0;
  Vector2i lastPoint(point); // needed for step length

  //int max_length = 6;
  //int i = 0;
  while(scanner.getNextWithCheck(point)) // i < max_length*/)
  {
    getImage().get(point.x, point.y, pixel);
    int f_y = (int)pixel.v - (int)pixel.u;

    // hack
    /*
    if(pixel.v < getFieldColorPercept().range.getMax().v)
      i++;
    else
      i = 0;
      */

    filter.add(point, f_y);
    if(!filter.ready()) {
      // assume the step length is constant, so we only calculate it in the starting phase of the filter
      stepLength += Vector2d(point - lastPoint).abs();
      lastPoint = point;
      ASSERT(stepLength > 0);
      continue;
    }

    DEBUG_REQUEST("Vision:BallDetector:drawScanlines",
      POINT_PX(ColorClasses::blue, point.x, point.y);
    );

    // jump down found
    // NOTE: we scale the filter value with the stepLength to acount for diagonal scans
    if(negativeScan.add(filter.point(), -filter.value()/stepLength))
    {
      DEBUG_REQUEST("Vision:BallDetector:drawScanlines",
        POINT_PX(ColorClasses::pink, peak_point_min.x, peak_point_min.y);
      );
      points.push_back(peak_point_min);
      return pixel.y < params.maxBorderBrightness;
    }
  }//end while

  return false; //getFieldColorPercept().isFieldColor(pixel);
}//end scanForEdges

void BallDetector::calculateBallPercept(const Vector2i& center, double radius)
{
  // calculate the ball
    bool projectionOK = CameraGeometry::imagePixelToFieldCoord(
		  getCameraMatrix(),
		  getImage().cameraInfo,
		  center.x,
		  center.y,
		  getFieldInfo().ballRadius,
		  getBallPercept().bearingBasedOffsetOnField);

    // HACK: don't take to far balls
    projectionOK = projectionOK && getBallPercept().bearingBasedOffsetOnField.abs2() < 10000*10000; // closer than 10m

    // HACK: if the ball center is in image it has to be in the field polygon
    Vector2d ballPointToCheck(center.x, center.y - 5);
    projectionOK = projectionOK &&
      (!getImage().isInside((int)(ballPointToCheck.x+0.5), (int)(ballPointToCheck.y+0.5)) ||
        getFieldPercept().getValidField().isInside(ballPointToCheck));

    if(projectionOK)
    {
		  getBallPercept().radiusInImage = radius;
		  getBallPercept().centerInImage = center;
		  getBallPercept().ballWasSeen = projectionOK;
		  getBallPercept().frameInfoWhenBallWasSeen = getFrameInfo();

      DEBUG_REQUEST("Vision:BallDetector:draw_ball",
        CIRCLE_PX(ColorClasses::orange, center.x, center.y, (int)(radius+0.5));
		  );
    }
}

double BallDetector::estimatedBallRadius(const Vector2i& point) const
{
  Vector2d pointOnField;
  if(!CameraGeometry::imagePixelToFieldCoord(
		  getCameraMatrix(),
		  getImage().cameraInfo,
		  point.x,
		  point.y,
		  getFieldInfo().ballRadius,
		  pointOnField))
  {
    return -1;
  }

  Vector3d d = getCameraMatrix()*Vector3d(pointOnField.x, pointOnField.y, getFieldInfo().ballRadius);
  double cameraBallDistance = d.abs();
  if(cameraBallDistance > getFieldInfo().ballRadius) {
    double a = asin(getFieldInfo().ballRadius / d.abs());
    return a / getImage().cameraInfo.getOpeningAngleHeight() * getImage().cameraInfo.resolutionHeight;
  }

  return -1;
}

void BallDetector::estimateCircleSimple(const std::vector<Vector2i>& endPoints, Vector2d& center, double& radius) const
{
  Vector2d sum;
  for(size_t i = 0; i < endPoints.size(); i++) {
    sum += endPoints[i];
  }

  center = sum/((double)endPoints.size());

  RingBufferWithSum<double, 8> radiusBuffer;
  for(size_t i = 0; i < endPoints.size(); i++) {
    radiusBuffer.add((center - Vector2d(endPoints[i])).abs2());
  }

  radius = sqrt(radiusBuffer.getAverage());
}

bool BallDetector::sanityCheck(const Vector2i& center, double radius)
{
  size_t sampleSize = 21;
  double maxSquareSize = sqrt(radius*radius/2.0);
  int searchSize = int(maxSquareSize/2.0);
  int width = static_cast<int>(getImage().width());
  int height = static_cast<int>(getImage().height());

  int minX = Math::clamp(center.x - searchSize, 0, width);
  int maxX = Math::clamp(center.x + searchSize, 0, width);
  int minY = Math::clamp(center.y - searchSize, 0, height);
  int maxY = Math::clamp(center.y + searchSize, 0, height);

  size_t goodPoints = 0;
  Pixel pixel;
  for(size_t i = 0; i < sampleSize; i++)
  {
    int x = Math::random(minX, maxX);
    int y = Math::random(minY, maxY);
    getImage().get(x, y, pixel);
    if(isOrange(pixel))
    {
      goodPoints++;
      POINT_PX(ColorClasses::green, x, y);
    } else
    {
      POINT_PX(ColorClasses::white, x, y);
    }
  }

  return static_cast<double>(goodPoints) / static_cast<double>(sampleSize) > 0.5;
}
<|endoftext|>


<commit_before>#include "GameManager.h"
#include "TriggerManager.h"
#include "InputManager.h"
#include "MouseCommand.h"
#include "ObserverComponent.h"
#include "KeyboardTrigger.h"

bool Arthas::MouseCommand::init()
{
	if (!CommandComponent::init())
	{
		return false;
	}
	return true;
}

void Arthas::MouseCommand::update(float dTime)
{
	auto observer = getObserverComponent();
	if (observer != nullptr)
	{
		auto mouseState = GET_INPUT_MANAGER()->getMouseInfo();
		if (mouseState.mouseState != MS_NONE)
		{

		}
	}
}

void Arthas::MouseCommand::enter()
{

}

void Arthas::MouseCommand::exit()
{

}
<commit_msg>MouseCommand<commit_after>#include "GameManager.h"
#include "TriggerManager.h"
#include "InputManager.h"
#include "MouseCommand.h"
#include "ObserverComponent.h"
#include "MouseTrigger.h"

bool Arthas::MouseCommand::init()
{
	if (!CommandComponent::init())
	{
		return false;
	}
	return true;
}

void Arthas::MouseCommand::update(float dTime)
{
	auto observer = getObserverComponent();
	if (observer != nullptr)
	{
		auto mouseState = GET_INPUT_MANAGER()->getMouseInfo();
		if (mouseState.mouseState != MS_NONE)
		{
//			auto mouseTrigger = (MouseTrigger*)GET_TRIGGER_MANAGER()->createTrigger<MouseTrigger>();
//			keyTrigger->initKeyCode((KeyCode)keyCode, keyState);
//			observer->addTrigger((Trigger*)mouseTrigger);
		}
	}
}

void Arthas::MouseCommand::enter()
{

}

void Arthas::MouseCommand::exit()
{

}
<|endoftext|>


<commit_before><commit_msg>WaE: -Werror=unused-parameter on Android<commit_after><|endoftext|>


<commit_before>#pragma once
//=====================================================================//
/*!	@file
	@brief	RX マイコン、デバイス固有ヘッダー
    @author 平松邦仁 (hira@rvf-rc45.net)
	@copyright	Copyright (C) 2018 Kunihito Hiramatsu @n
				Released under the MIT license @n
				https://github.com/hirakuni45/RX/blob/master/LICENSE
*/
//=====================================================================//
#include "common/io_utils.hpp"

#if defined(SIG_RX24T)
#include "RX24T/peripheral.hpp"
#include "RX24T/system.hpp"
#include "RX24T/power_mgr.hpp"
#include "RX24T/icu.hpp"
#include "RX24T/icu_mgr.hpp"
#include "RX24T/port_map.hpp"

#elif defined(SIG_RX64M) || defined(SIG_RX71M)
#include "RX600/peripheral.hpp"
#include "RX600/system.hpp"
#include "RX600/power_mgr.hpp"
#include "RX600/icu.hpp"
#include "RX600/icu_mgr.hpp"
#include "RX600/port_map.hpp"

#elif defined(SIG_RX65N)
#include "RX65x/peripheral.hpp"
#include "RX600/system.hpp"
#include "RX65x/power_mgr.hpp"
#include "RX65x/icu.hpp"
#include "RX65x/icu_mgr.hpp"
#include "RX65x/port_map.hpp"

#elif defined(SIG_RX66T)
#include "RX66T/peripheral.hpp"
#include "RX600/system.hpp"
#include "RX66T/power_mgr.hpp"
#include "RX66T/icu.hpp"
#include "RX66T/icu_mgr.hpp"
#include "RX66T/port_map.hpp"

#else
#  error "device.hpp: Requires SIG_XXX to be defined"
#endif
<commit_msg>update: RX62N siguneture<commit_after>#pragma once
//=====================================================================//
/*!	@file
	@brief	RX マイコン、デバイス固有ヘッダー
    @author 平松邦仁 (hira@rvf-rc45.net)
	@copyright	Copyright (C) 2018 Kunihito Hiramatsu @n
				Released under the MIT license @n
				https://github.com/hirakuni45/RX/blob/master/LICENSE
*/
//=====================================================================//
#include "common/io_utils.hpp"

#if defined(SIG_RX24T)
#include "RX24T/peripheral.hpp"
#include "RX24T/system.hpp"
#include "RX24T/power_mgr.hpp"
#include "RX24T/icu.hpp"
#include "RX24T/icu_mgr.hpp"
#include "RX24T/port_map.hpp"

#elif defined(SIG_RX62N)
#include "RX62x/system.hpp"
#include "RX62x/icu.hpp"
#include "RX62x/icu_mgr.hpp"

#elif defined(SIG_RX64M) || defined(SIG_RX71M)
#include "RX600/peripheral.hpp"
#include "RX600/system.hpp"
#include "RX600/power_mgr.hpp"
#include "RX600/icu.hpp"
#include "RX600/icu_mgr.hpp"
#include "RX600/port_map.hpp"

#elif defined(SIG_RX65N)
#include "RX65x/peripheral.hpp"
#include "RX600/system.hpp"
#include "RX65x/power_mgr.hpp"
#include "RX65x/icu.hpp"
#include "RX65x/icu_mgr.hpp"
#include "RX65x/port_map.hpp"

#elif defined(SIG_RX66T)
#include "RX66T/peripheral.hpp"
#include "RX600/system.hpp"
#include "RX66T/power_mgr.hpp"
#include "RX66T/icu.hpp"
#include "RX66T/icu_mgr.hpp"
#include "RX66T/port_map.hpp"

#else
#  error "device.hpp: Requires SIG_XXX to be defined"
#endif
<|endoftext|>


<commit_before><commit_msg>vcl: vcldemo now shows rotated text<commit_after><|endoftext|>


<commit_before>/*
 * Composition.cc
 *
 * Copyright (C) 2015 Linas Vepstas
 *
 * Author: Linas Vepstas <linasvepstas@gmail.com>  aPRIL 2015
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <opencog/atoms/bind/BetaRedex.h>
#include <opencog/atoms/bind/SatisfactionLink.h>

#include "PatternMatchEngine.h"
#include "PatternMatchCallback.h"

using namespace opencog;

// Uncomment below to enable debug print
// #define DEBUG
#ifdef DEBUG
	#define dbgprt(f, varargs...) printf(f, ##varargs)
#else
	#define dbgprt(f, varargs...)
#endif

/* ================================================================= */

/* Reset the current variable grounding to the last grounding pushed
 * onto the stack. */
#define POPGND(soln,stack) {         \
	OC_ASSERT(not stack.empty(), "Unbalanced grounding stack"); \
	soln = stack.top();               \
	stack.pop();                      \
}

/* ================================================================= */

void PatternMatchEngine::push_redex(void)
{
	_stack_bound_vars.push(_bound_vars);
	_stack_cnf_clauses.push(_cnf_clauses);
	_stack_mandatory.push(_mandatory);
	_stack_optionals.push(_optionals);
	_stack_evaluatable.push(_evaluatable);
	_stack_connectivity_map.push(_connectivity_map);
}

void PatternMatchEngine::pop_redex(void)
{
	_bound_vars = _stack_bound_vars.top();
	_stack_bound_vars.pop();

	_cnf_clauses = _stack_cnf_clauses.top();
	_stack_cnf_clauses.pop();

	_mandatory = _stack_mandatory.top();
	_stack_mandatory.pop();

	_optionals = _stack_optionals.top();
	_stack_optionals.pop();

	_evaluatable = _stack_evaluatable.top();
	_stack_evaluatable.pop();

	_connectivity_map = _stack_connectivity_map.top();
	_stack_connectivity_map.pop();
}

bool PatternMatchEngine::redex_compare(const LinkPtr& lp,
                                       const LinkPtr& lg)
{
	// If we are here, the pattern is defined in a DefineLink. We
	// must match to that. There seem to be two strategies for doing
	// that:  Method A: rename all of the variables in the defined
	// pattern to be the variables we are actually using in the
	// top-level search.  This seems easy, but it is wrong, for two
	// reasons. One reason is that, after renaming, we will have
	// created a pattern that is probably not in the atomspace.
	// That means that the pattern will have atoms with invalid UUID's
	// in them, causing trouble down the line. The other problem is
	// that the variables in the defined target now look like perfectly
	// good grounding candidates, and so get found and reported as valid
	// grounds. So, for these two reasons, the simple, "obvious" method
	// A is out. Instead, we implement method B: we rename the variables
	// that the match engine is carrying, to correspond with the variable
	// names that are native to the definition. This way, insde the body
	// of the definition, everything looks "normal", and should thus
	// proceed as formal.  Of course, on exit, we have to unmasquerade.
	//
	// By "everything looks normal", we really mean "treat this as if it
	// was a brand-new pattern matching problem".  To do this, it is very
	// empting to just create a new PME, and let it run. The problem with
	// that is that we have no particularly good way of integrating the
	// new pme state, with the existing pme state. So we don't.  Instead,
	// we push all pme state, clear the decks, (almost as if strting from
	// scratch) and then pop all pme state when we are done.
	graph_stacks_push();

	BetaRedexPtr cpl(BetaRedexCast(lp));

	// To explore the defined pattern, we've got to get
	// into its local frame. Do this by masquerading
	// any grounded variables we may have so far.
	const HandleSeq& local_args(cpl->get_local_args());
	const HandleSeq& redex_args(cpl->get_args());

// XXX TODO respect  the type definitions, too!!!!
// XXX TODO handle clause_grounding as well

	SolnMap local_grounding;
	size_t sz = redex_args.size();
	for (size_t i=0; i< sz; i++)
	{
		// Relabel (masquerade) the grounded vars.
		auto iter = var_grounding.find(redex_args[i]);
		if (iter == var_grounding.end()) continue;
		local_grounding.insert({local_args[i], iter->second});
	}
	var_grounding = local_grounding;

	// Now, get the set of clauses to be grounded. We expect
	// the redex body to be a SatisfactionLink; we have already
	// remapped its variable declarations; now get its body.
	Handle hsat(cpl->get_definition());
	SatisfactionLinkPtr sat_link(SatisfactionLinkCast(hsat));
	if (NULL == sat_link)
		throw InvalidParamException(TRACE_INFO,
			"Expecting SatisfactionLink, got %s",
				hsat->toString().c_str());

	const HandleSeq& local_clauses(sat_link->get_clauses());
	if (1 != local_clauses.size())
		throw InvalidParamException(TRACE_INFO,
			"More than one clause - not implemented");

	Handle local_pattern(local_clauses[0]);
printf("duuuude ready to compare pat=%s to gnd=%s\n",
local_pattern->toString().c_str(), lg->toString().c_str());

	// Now, proceed as normal.
	std::set<Handle> saved_vars = _bound_vars;
	_bound_vars = cpl->get_local_argset();
	bool have_match = tree_compare(local_pattern, Handle(lg));
	_bound_vars = saved_vars;

	// No match; restore original grounding and quit
	if (not have_match)
	{
		graph_stacks_pop();
		return false;
	}

	// If there is a match, then maybe we grounded some variables.
	// If so, we need to unmasquerade them.
	local_grounding = var_grounding;
	graph_stacks_pop();
	for (size_t i=0; i< sz; i++)
	{
		auto iter = local_grounding.find(local_args[i]);
		if (iter != local_grounding.end())
			var_grounding.insert({redex_args[i], iter->second});
	}

	return true;
}

/* ===================== END OF FILE ===================== */
<commit_msg>Continuing development<commit_after>/*
 * Composition.cc
 *
 * Copyright (C) 2015 Linas Vepstas
 *
 * Author: Linas Vepstas <linasvepstas@gmail.com>  aPRIL 2015
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <opencog/atoms/bind/BetaRedex.h>
#include <opencog/atoms/bind/SatisfactionLink.h>

#include "PatternMatchEngine.h"
#include "PatternMatchCallback.h"

using namespace opencog;

// Uncomment below to enable debug print
// #define DEBUG
#ifdef DEBUG
	#define dbgprt(f, varargs...) printf(f, ##varargs)
#else
	#define dbgprt(f, varargs...)
#endif

/* ================================================================= */
/*
TODO:
-- for non-connected satisfaction links, the ctor should take each
   connected comp, and compute the mandatories, the optionals, the
   evaluatables and the connectivity map for each comp, and store
   thse in a meta-pseudo satisfactonLink.  Should invent a new link
   type for this...

-- where the heck is type enforcement done, again??? Need type
   enforcement during the redex...
*/
/* ================================================================= */

/* Reset the current variable grounding to the last grounding pushed
 * onto the stack. */
#define POPGND(soln,stack) {         \
	OC_ASSERT(not stack.empty(), "Unbalanced grounding stack"); \
	soln = stack.top();               \
	stack.pop();                      \
}

/* ================================================================= */

void PatternMatchEngine::push_redex(void)
{
	_stack_bound_vars.push(_bound_vars);
	_stack_cnf_clauses.push(_cnf_clauses);
	_stack_mandatory.push(_mandatory);
	_stack_optionals.push(_optionals);
	_stack_evaluatable.push(_evaluatable);
	_stack_connectivity_map.push(_connectivity_map);
}

void PatternMatchEngine::pop_redex(void)
{
	_bound_vars = _stack_bound_vars.top();
	_stack_bound_vars.pop();

	_cnf_clauses = _stack_cnf_clauses.top();
	_stack_cnf_clauses.pop();

	_mandatory = _stack_mandatory.top();
	_stack_mandatory.pop();

	_optionals = _stack_optionals.top();
	_stack_optionals.pop();

	_evaluatable = _stack_evaluatable.top();
	_stack_evaluatable.pop();

	_connectivity_map = _stack_connectivity_map.top();
	_stack_connectivity_map.pop();
}

bool PatternMatchEngine::redex_compare(const LinkPtr& lp,
                                       const LinkPtr& lg)
{
	// If we are here, the pattern is defined in a DefineLink. We
	// must match to that. There seem to be two strategies for doing
	// that:  Method A: rename all of the variables in the defined
	// pattern to be the variables we are actually using in the
	// top-level search.  This seems easy, but it is wrong, for two
	// reasons. One reason is that, after renaming, we will have
	// created a pattern that is probably not in the atomspace.
	// That means that the pattern will have atoms with invalid UUID's
	// in them, causing trouble down the line. The other problem is
	// that the variables in the defined target now look like perfectly
	// good grounding candidates, and so get found and reported as valid
	// grounds. So, for these two reasons, the simple, "obvious" method
	// A is out. Instead, we implement method B: we rename the variables
	// that the match engine is carrying, to correspond with the variable
	// names that are native to the definition. This way, insde the body
	// of the definition, everything looks "normal", and should thus
	// proceed as formal.  Of course, on exit, we have to unmasquerade.
	//
	// By "everything looks normal", we really mean "treat this as if it
	// was a brand-new pattern matching problem".  To do this, it is very
	// empting to just create a new PME, and let it run. The problem with
	// that is that we have no particularly good way of integrating the
	// new pme state, with the existing pme state. So we don't.  Instead,
	// we push all pme state, clear the decks, (almost as if strting from
	// scratch) and then pop all pme state when we are done.
	graph_stacks_push();
	push_redex();
	clear_redex();

	BetaRedexPtr cpl(BetaRedexCast(lp));

	// Get the variales and clauses that make up the redex.
	// We expect the redex body to be a SatisfactionLink.
	// XXX TODO perhaps we should create a special sat-redex-only
	// link type?
	Handle hsat(cpl->get_definition());
	SatisfactionLinkPtr sat_link(SatisfactionLinkCast(hsat));
	if (NULL == sat_link)
		throw InvalidParamException(TRACE_INFO,
			"Expecting SatisfactionLink, got %s",
				hsat->toString().c_str());

	const std::set<Handle>& local_vars(sat_link->get_varset());
	const HandleSeq& local_clauses(sat_link->get_clauses());
	setup_redex(local_vars, local_clauses);

	// To explore the defined pattern, we've got to get
	// into its local frame. Do this by masquerading
	// any grounded variables we may have so far.
	const HandleSeq& local_args(cpl->get_local_args());
	const HandleSeq& redex_args(cpl->get_args());

// XXX TODO handle clause_grounding as well

	SolnMap local_grounding;
	size_t sz = redex_args.size();
	for (size_t i=0; i< sz; i++)
	{
		// Relabel (masquerade) the grounded vars.
		auto iter = var_grounding.find(redex_args[i]);
		if (iter == var_grounding.end()) continue;
		local_grounding.insert({local_args[i], iter->second});
	}
	var_grounding = local_grounding;

/*****
	Handle local_pattern(local_clauses[0]);
printf("duuuude ready to compare pat=%s to gnd=%s\n",
local_pattern->toString().c_str(), lg->toString().c_str());

	// Now, proceed as normal.
	std::set<Handle> saved_vars = _bound_vars;
	_bound_vars = cpl->get_local_argset();
	bool have_match = tree_compare(local_pattern, Handle(lg));
	_bound_vars = saved_vars;
****/
bool have_match = false;

	// No match; restore original grounding and quit
	if (not have_match)
	{
		pop_redex();
		graph_stacks_pop();
		return false;
	}

	// If there is a match, then maybe we grounded some variables.
	// If so, we need to unmasquerade them.
	local_grounding = var_grounding;
	graph_stacks_pop();
	for (size_t i=0; i< sz; i++)
	{
		auto iter = local_grounding.find(local_args[i]);
		if (iter != local_grounding.end())
			var_grounding.insert({redex_args[i], iter->second});
	}

	pop_redex();
	return true;
}

/* ===================== END OF FILE ===================== */
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/hwpf/fapi2/include/utils.H $                       */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2011,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
/**
 * @file utils.H
 * @brief Defines common fapi2 utilities
 */

#ifndef FAPI2_UTILS_H_
#define FAPI2_UTILS_H_

#include <stdint.h>
#include <return_code.H>
#include <target_types.H>
#include <plat_utils.H>

namespace fapi2
{
#ifndef __PPE__
///
/// @brief Enable/Disable special wakeup on processor chip core(s)
///
/// Special Wakeup Enable must be done when a HWP is doing an operation that
/// requires core(s) to be awake (e.g. modifying the Hcode image). For
/// each Special Wakeup Enable call, there must be a subsequent Special Wakeup
/// Disable call.
///
/// This does not apply to SCOM operations, platforms must handle Special Wakeup
/// for SCOM operations internally.
///
/// If Special Wakeup is enabled, a core will not go to sleep (if already
/// sleeping, it is woken up). If Special Wakeup is disabled, if there are no
/// other active Enables, the core is allowed to sleep.
///
/// @note Implemented by platform code calling the cpu special wakeup HWP.
///       This is a FAPI2 function because each platform may do different things
///         Hostboot: Does nothing (cores cannot sleep while Hostboot running)
///         FSP: Uses an algorithm to decide when to disable special wakeup
///         Cronus: Does Special Wakeup enable/disable as requested
///
/// @param[in] i_target
///              TARGET_TYPE_PROC_CHIP: Enables/Disables Special Wakeup on all
///                cores (EX,EQ chiplets) of the specified chip target.
///              TARGET_TYPE_CORE: Enables/Disables Special Wakeup on the
///                specified core target (EX,EQ chiplets)
///              TARGET_TYPE_EX: Enables/Disables Special Wakeup on the
///                specified EX target.
///              TARGET_TYPE_EQ: Enables/Disables Special Wakeup on the
///                specified EQ target.
///
/// @param[in] i_enable true = enable. false = disable.
///
/// @return ReturnCode. FAPI2_RC_SUCCESS on success, else platform specified error.
///
///
template<TargetType T, typename V>
inline ReturnCode specialWakeup(const Target<T, V>& i_target,
                                const bool i_enable)
{
    // enforce the allowed target types
    static_assert( ((T == fapi2::TARGET_TYPE_PROC_CHIP) ||
                    (T == fapi2::TARGET_TYPE_CORE)      ||
                    (T == fapi2::TARGET_TYPE_EX)        ||
                    (T == fapi2::TARGET_TYPE_EQ)),
                   "Invalid target type for this function");

    ReturnCode l_rc = platSpecialWakeup( i_target, i_enable );

    return l_rc;
}

///
/// @brief Log an error.
///
/// @param[in,out] io_rc Reference to ReturnCode (Any references to data and error
///            target are removed and rc value is set to success after
///            function ends.)
/// @param[in] i_sev Fapi error log severity defaulted to unrecoverable
/// @param[in] i_unitTestError - flag to log error which does not cause a unit
///                              test to fail.
///
/// @note This function is called from the ffdc collection classes and no longer
/// needs to be called directly.
/// @note Implemented by platform code
///
void logError(
    fapi2::ReturnCode& io_rc,
    fapi2::errlSeverity_t i_sev = fapi2::FAPI2_ERRL_SEV_UNRECOVERABLE,
    bool i_unitTestError = false );

///
/// @brief Create a platform error log
///
///  This function will create a platform error log from the passed in
///  return code value and will populate the iv_platDataPtr of the return code
///  with a pointer to the newly created log.
///
/// @param[in,out] io_rc - Reference to ReturnCode
///
/// @param[in] i_sev Fapi error log severity defaulted to unrecoverable
//
//
/// @note Implemented by platform code
///
void createPlatLog(
    fapi2::ReturnCode& io_rc,
    fapi2::errlSeverity_t i_sev = fapi2::FAPI2_ERRL_SEV_UNRECOVERABLE
);

///
/// @brief Associate an error to PRD PLID
///
/// @param[in] i_target Reference to target
/// @param[in,out] io_rc Reference to ReturnCode
/// @param[in] i_sev Fapi error log severity defaulted to unrecoverable
/// @param[in] i_unitTestError - flag to log error which does not cause a unit
///                              test to fail.
///
/// @note Implemented by platform code
///
void log_related_error(
    const Target<TARGET_TYPE_ALL>& i_target,
    fapi2::ReturnCode& io_rc,
    const fapi2::errlSeverity_t i_sev = fapi2::FAPI2_ERRL_SEV_UNRECOVERABLE,
    const bool i_unitTestError = false );

#endif // __PPE__

///
/// @brief Delay this thread. Hostboot will use the nanoseconds parameter
/// and make a syscall to nanosleep. While in the syscall, the hostboot
/// kernel will continue to consume CPU cycles as it looks for a runnable
/// task.  When the delay time expires, the task becomes runnable and will soon
/// return from the syscall.  Callers of delay() in the hostboot environment
/// will likely have to know the mHz clock speed they are running on and
/// compute a non-zero value for i_nanoSeconds.
///
/// On the FSP, it was sometimes acceptable to just provide zero for the
/// sleep delay time, causing the task to yield its time slice. By the
/// time the calling task could run again, it was pretty certain enough
/// host cycles had past.  This is probably not acceptable in
/// the hostboot environment. Callers should calculate and provide a
/// sleep value in nanoseconds relative to host clock speed.
///
/// On FSP when VBU is the target, then the i_simCycles parameter will be
/// used instead.  The FSP needs to use the simdispatcher client/server
/// API and issue a command to the awan to advance the simulation the
/// specified number of cycles.
///
/// @param[in] i_nanoSeconds    nanoseconds to sleep
/// @param[in] i_simCycles      count of Awan cycles to advance
/// @param[in] i_fixed          Determination, for DFT, if this time is
///                             fixed or not. Defaults to non-fixed
///
/// @return ReturnCode. Zero on success, else platform specified error.
///
ReturnCode delay(uint64_t i_nanoSeconds, uint64_t i_simCycles,
                 bool i_fixed = false);

///
/// @brief Assert a condition, and halt
///
/// @param[in] i_expression a boolean representing the assertion
///
void Assert(bool i_expression);
};

#endif // FAPI2_UTILS_H_
<commit_msg>Add prototype for releasing platform data pointer storage function<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/hwpf/fapi2/include/utils.H $                       */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2011,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
/**
 * @file utils.H
 * @brief Defines common fapi2 utilities
 */

#ifndef FAPI2_UTILS_H_
#define FAPI2_UTILS_H_

#include <stdint.h>
#include <return_code.H>
#include <target_types.H>
#include <plat_utils.H>

namespace fapi2
{
#ifndef __PPE__
///
/// @brief Enable/Disable special wakeup on processor chip core(s)
///
/// Special Wakeup Enable must be done when a HWP is doing an operation that
/// requires core(s) to be awake (e.g. modifying the Hcode image). For
/// each Special Wakeup Enable call, there must be a subsequent Special Wakeup
/// Disable call.
///
/// This does not apply to SCOM operations, platforms must handle Special Wakeup
/// for SCOM operations internally.
///
/// If Special Wakeup is enabled, a core will not go to sleep (if already
/// sleeping, it is woken up). If Special Wakeup is disabled, if there are no
/// other active Enables, the core is allowed to sleep.
///
/// @note Implemented by platform code calling the cpu special wakeup HWP.
///       This is a FAPI2 function because each platform may do different things
///         Hostboot: Does nothing (cores cannot sleep while Hostboot running)
///         FSP: Uses an algorithm to decide when to disable special wakeup
///         Cronus: Does Special Wakeup enable/disable as requested
///
/// @param[in] i_target
///              TARGET_TYPE_PROC_CHIP: Enables/Disables Special Wakeup on all
///                cores (EX,EQ chiplets) of the specified chip target.
///              TARGET_TYPE_CORE: Enables/Disables Special Wakeup on the
///                specified core target (EX,EQ chiplets)
///              TARGET_TYPE_EX: Enables/Disables Special Wakeup on the
///                specified EX target.
///              TARGET_TYPE_EQ: Enables/Disables Special Wakeup on the
///                specified EQ target.
///
/// @param[in] i_enable true = enable. false = disable.
///
/// @return ReturnCode. FAPI2_RC_SUCCESS on success, else platform specified error.
///
///
template<TargetType T, typename V>
inline ReturnCode specialWakeup(const Target<T, V>& i_target,
                                const bool i_enable)
{
    // enforce the allowed target types
    static_assert( ((T == fapi2::TARGET_TYPE_PROC_CHIP) ||
                    (T == fapi2::TARGET_TYPE_CORE)      ||
                    (T == fapi2::TARGET_TYPE_EX)        ||
                    (T == fapi2::TARGET_TYPE_EQ)),
                   "Invalid target type for this function");

    ReturnCode l_rc = platSpecialWakeup( i_target, i_enable );

    return l_rc;
}

///
/// @brief Log an error.
///
/// @param[in,out] io_rc Reference to ReturnCode (Any references to data and error
///            target are removed and rc value is set to success after
///            function ends.)
/// @param[in] i_sev Fapi error log severity defaulted to unrecoverable
/// @param[in] i_unitTestError - flag to log error which does not cause a unit
///                              test to fail.
///
/// @note This function is called from the ffdc collection classes and no longer
/// needs to be called directly.
/// @note Implemented by platform code
///
void logError(
    fapi2::ReturnCode& io_rc,
    fapi2::errlSeverity_t i_sev = fapi2::FAPI2_ERRL_SEV_UNRECOVERABLE,
    bool i_unitTestError = false );

///
/// @brief Create a platform error log
///
///  This function will create a platform error log from the passed in
///  return code value and will populate the iv_platDataPtr of the return code
///  with a pointer to the newly created log.
///
/// @param[in,out] io_rc - Reference to ReturnCode
///
/// @param[in] i_sev Fapi error log severity defaulted to unrecoverable
//
//
/// @note Implemented by platform code
///
void createPlatLog(
    fapi2::ReturnCode& io_rc,
    fapi2::errlSeverity_t i_sev = fapi2::FAPI2_ERRL_SEV_UNRECOVERABLE
);

///
/// @brief delete platform data ptr - free platform data from the
//                                    passed in RC.
///
void deletePlatformDataPointer(fapi2::ReturnCode& io_rc);

///
/// @brief Associate an error to PRD PLID
///
/// @param[in] i_target Reference to target
/// @param[in,out] io_rc Reference to ReturnCode
/// @param[in] i_sev Fapi error log severity defaulted to unrecoverable
/// @param[in] i_unitTestError - flag to log error which does not cause a unit
///                              test to fail.
///
/// @note Implemented by platform code
///
void log_related_error(
    const Target<TARGET_TYPE_ALL>& i_target,
    fapi2::ReturnCode& io_rc,
    const fapi2::errlSeverity_t i_sev = fapi2::FAPI2_ERRL_SEV_UNRECOVERABLE,
    const bool i_unitTestError = false );

#endif // __PPE__

///
/// @brief Delay this thread. Hostboot will use the nanoseconds parameter
/// and make a syscall to nanosleep. While in the syscall, the hostboot
/// kernel will continue to consume CPU cycles as it looks for a runnable
/// task.  When the delay time expires, the task becomes runnable and will soon
/// return from the syscall.  Callers of delay() in the hostboot environment
/// will likely have to know the mHz clock speed they are running on and
/// compute a non-zero value for i_nanoSeconds.
///
/// On the FSP, it was sometimes acceptable to just provide zero for the
/// sleep delay time, causing the task to yield its time slice. By the
/// time the calling task could run again, it was pretty certain enough
/// host cycles had past.  This is probably not acceptable in
/// the hostboot environment. Callers should calculate and provide a
/// sleep value in nanoseconds relative to host clock speed.
///
/// On FSP when VBU is the target, then the i_simCycles parameter will be
/// used instead.  The FSP needs to use the simdispatcher client/server
/// API and issue a command to the awan to advance the simulation the
/// specified number of cycles.
///
/// @param[in] i_nanoSeconds    nanoseconds to sleep
/// @param[in] i_simCycles      count of Awan cycles to advance
/// @param[in] i_fixed          Determination, for DFT, if this time is
///                             fixed or not. Defaults to non-fixed
///
/// @return ReturnCode. Zero on success, else platform specified error.
///
ReturnCode delay(uint64_t i_nanoSeconds, uint64_t i_simCycles,
                 bool i_fixed = false);

///
/// @brief Assert a condition, and halt
///
/// @param[in] i_expression a boolean representing the assertion
///
void Assert(bool i_expression);
};

#endif // FAPI2_UTILS_H_
<|endoftext|>


<commit_before>#pragma once
//=====================================================================//
/*!	@file
	@brief	I2C テンプレートクラス (20MHz system clock) @n
			Copyright 2015 Kunihito Hiramatsu
	@author	平松邦仁 (hira@rvf-rc45.net)
*/
//=====================================================================//
#include <cstdint>
#include "common/delay.hpp"

namespace device {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  I2C テンプレートクラス @n
				PORT ポート指定クラス： @n
				class port { @n
				public: @n
					void scl_dir(bool val) const { } @n
					void scl_out(bool val) const { } @n
					bool scl_inp() const { return 0; } @n
					void sda_dir(bool val) const { } @n
					void sda_out(bool val) const { } @n
					bool sda_inp() const { return 0; } @n
				};
		@param[in]	PORT	ポート定義クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class PORT>
	class i2c_io {
		PORT		port_;
		uint8_t		clock_;
		uint16_t	busy_;

		static const uint8_t slow_clock_ = 10 / 2;
		static const uint8_t fast_clock_ = 4 / 2;

		void start_() const {
			port_.sda_out(0);
			utils::delay::micro_second(clock_);
			port_.scl_out(0);
			utils::delay::micro_second(clock_);
		}

		bool ack_() const {
			port_.sda_out(1);
			utils::delay::micro_second(clock_);
			port_.scl_out(1);
			port_.sda_dir(0);
			utils::delay::micro_second(clock_);
			bool f = port_.sda_inp();
			port_.sda_dir(1);
			port_.scl_out(0);
			return f;
		}

		void out_ack_(bool b) const {
			utils::delay::micro_second(clock_);
			port_.sda_out(b);
			port_.scl_out(1);
			utils::delay::micro_second(clock_);
			port_.scl_out(0);
		}

		bool wait_() const {
			uint16_t cnt = busy_;
			port_.scl_dir(0);
			while(port_.scl_inp() == 0) {
				utils::delay::micro_second(1);
				if(cnt) {
					--cnt;
				} else {
					port_.scl_dir(1);
					return false;  // wait stall
				}
			}
			port_.scl_dir(1);
			return true;
		}

		void stop_() const {
			utils::delay::micro_second(clock_);
			port_.scl_out(1);
			utils::delay::micro_second(clock_);
			port_.sda_out(1);
		}

		bool write_(uint8_t val, bool sync) const {
			for(uint8_t n = 0; n < 8; ++n) {
				if(val & 0x80) port_.sda_out(1); else port_.sda_out(0);
				utils::delay::micro_second(clock_);
				port_.scl_out(1);
				if(n == 0 && sync) {
					if(!wait_()) return false;
				}
				val <<= 1;
				utils::delay::micro_second(clock_);
				port_.scl_out(0);
			}
			return true;
		}

		bool write_(const uint8_t* src, uint8_t num) const {
			for(uint8_t n = 0; n < num; ++n) {
				if(!write_(*src, true)) {
					stop_();
					return false;
				}
				++src;
				if(ack_()) {
					stop_();
					return false;
				}
			}
			return true;
		}

		bool read_(uint8_t& val, bool sync) const {
			port_.sda_dir(0);
			for(uint8_t n = 0; n < 8; ++n) {
				utils::delay::micro_second(clock_);
				val <<= 1;
				port_.scl_out(1);
				if(n == 0 && sync) {
					if(!wait_()) {
						port_.sda_dir(1);
						return false;
					}
				}
				utils::delay::micro_second(clock_);
				if(port_.sda_inp()) val |= 1;
				port_.scl_out(0);
			}
			port_.sda_dir(1);
			return true;
		}

	public:
		//-----------------------------------------------------------------//
		/*!
			@brief  コンストラクター
		*/
		//-----------------------------------------------------------------//
		i2c_io() : clock_(slow_clock_), busy_(200) { }

		//-----------------------------------------------------------------//
		/*!
			@brief  初期化
		*/
		//-----------------------------------------------------------------//
		void init() const {
			port_.init();
			port_.scl_dir(1);
			port_.sda_dir(1);
			port_.scl_out(1);
			port_.sda_out(1);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  クロック設定
			@param[in]	clock	パルス５０％待ち時間（単位マイクロ秒）
		*/
		//-----------------------------------------------------------------//
		void set_clock(uint8_t clock) { clock_ = clock; }

		//-----------------------------------------------------------------//
		/*!
			@brief  低速指定（maybe 100KBPS）
		*/
		//-----------------------------------------------------------------//
		void set_slow() { clock_ = slow_clock_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  高速指定（maybe 400KBPS）
		*/
		//-----------------------------------------------------------------//
		void set_fast() { clock_ = fast_clock_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  スレーブデバイスの「待ち」時間の最大値を設定
			@param[in]	busy	待ち時間（単位マイクロ秒）
		*/
		//-----------------------------------------------------------------//
		void set_busy(uint16_t busy) { busy_ = busy; }

		//-----------------------------------------------------------------//
		/*!
			@brief  受信（リード）
			@param[in] address スレーブアドレス（７ビット）
			@param[out]	dst	先
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool recv(uint8_t address, uint8_t* dst, uint8_t num) const {
			start_();
			write_((address << 1) | 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			for(uint8_t n = 0; n < num; ++n) {
				if(!read_(*dst, true)) {
					stop_();
					return false;
				}
				bool f = 0;
				if(n == (num - 1)) f = 1;
				out_ack_(f);
				++dst;
			}
			stop_();
			return true;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  送信（ライト）
			@param[in] address スレーブアドレス（７ビット）
			@param[in]	src	元
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool send(uint8_t address, const uint8_t* src, uint8_t num) const {
			start_();
			write_(address << 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(src, num)) {
				stop_();
				return false;
			}
			stop_();
			return true;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  送信（ライト）
			@param[in] address スレーブアドレス（７ビット）
			@param[in]	first	ファーストデータ
			@param[in]	src	元
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool send(uint8_t address, uint8_t first, const uint8_t* src, uint8_t num) const {
			start_();
			write_(address << 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(first, false)) {
				stop_();
				return false;
			}

			if(!write_(src, num)) {
				stop_();
				return false;
			}
			stop_();
			return true;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  送信（ライト）
			@param[in] address スレーブアドレス（７ビット）
			@param[in]	first	ファースト・データ
			@param[in]	second	セカンド・データ
			@param[in]	src	元
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool send(uint8_t address, uint8_t first, uint8_t second, const uint8_t* src, uint8_t num) const {
			start_();
			write_(address << 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(first, false)) {
				stop_();
				return false;
			}

			if(!write_(second, false)) {
				stop_();
				return false;
			}

			if(!write_(src, num)) {
				stop_();
				return false;
			}
			stop_();
			return true;
		}
	};
}
<commit_msg>fix send ack_() for write<commit_after>#pragma once
//=====================================================================//
/*!	@file
	@brief	I2C テンプレートクラス (20MHz system clock) @n
			Copyright 2015 Kunihito Hiramatsu
	@author	平松邦仁 (hira@rvf-rc45.net)
*/
//=====================================================================//
#include <cstdint>
#include "common/delay.hpp"

namespace device {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  I2C テンプレートクラス @n
				PORT ポート指定クラス： @n
				class port { @n
				public: @n
					void scl_dir(bool val) const { } @n
					void scl_out(bool val) const { } @n
					bool scl_inp() const { return 0; } @n
					void sda_dir(bool val) const { } @n
					void sda_out(bool val) const { } @n
					bool sda_inp() const { return 0; } @n
				};
		@param[in]	PORT	ポート定義クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class PORT>
	class i2c_io {
		PORT		port_;
		uint8_t		clock_;
		uint16_t	busy_;

		static const uint8_t slow_clock_ = 10 / 2;
		static const uint8_t fast_clock_ = 4 / 2;

		void start_() const {
			port_.sda_out(0);
			utils::delay::micro_second(clock_);
			port_.scl_out(0);
			utils::delay::micro_second(clock_);
		}

		bool ack_() const {
			port_.sda_out(1);
			utils::delay::micro_second(clock_);
			port_.scl_out(1);
			port_.sda_dir(0);
			utils::delay::micro_second(clock_);
			bool f = port_.sda_inp();
			port_.sda_dir(1);
			port_.scl_out(0);
			return f;
		}

		void out_ack_(bool b) const {
			utils::delay::micro_second(clock_);
			port_.sda_out(b);
			port_.scl_out(1);
			utils::delay::micro_second(clock_);
			port_.scl_out(0);
		}

		bool wait_() const {
			uint16_t cnt = busy_;
			port_.scl_dir(0);
			while(port_.scl_inp() == 0) {
				utils::delay::micro_second(1);
				if(cnt) {
					--cnt;
				} else {
					port_.scl_dir(1);
					return false;  // wait stall
				}
			}
			port_.scl_dir(1);
			return true;
		}

		void stop_() const {
			utils::delay::micro_second(clock_);
			port_.scl_out(1);
			utils::delay::micro_second(clock_);
			port_.sda_out(1);
		}

		bool write_(uint8_t val, bool sync) const {
			for(uint8_t n = 0; n < 8; ++n) {
				if(val & 0x80) port_.sda_out(1); else port_.sda_out(0);
				utils::delay::micro_second(clock_);
				port_.scl_out(1);
				if(n == 0 && sync) {
					if(!wait_()) return false;
				}
				val <<= 1;
				utils::delay::micro_second(clock_);
				port_.scl_out(0);
			}
			return true;
		}

		bool write_(const uint8_t* src, uint8_t num) const {
			for(uint8_t n = 0; n < num; ++n) {
				if(!write_(*src, true)) {
					stop_();
					return false;
				}
				++src;
				if(ack_()) {
					stop_();
					return false;
				}
			}
			return true;
		}

		bool read_(uint8_t& val, bool sync) const {
			port_.sda_dir(0);
			for(uint8_t n = 0; n < 8; ++n) {
				utils::delay::micro_second(clock_);
				val <<= 1;
				port_.scl_out(1);
				if(n == 0 && sync) {
					if(!wait_()) {
						port_.sda_dir(1);
						return false;
					}
				}
				utils::delay::micro_second(clock_);
				if(port_.sda_inp()) val |= 1;
				port_.scl_out(0);
			}
			port_.sda_dir(1);
			return true;
		}

	public:
		//-----------------------------------------------------------------//
		/*!
			@brief  コンストラクター
		*/
		//-----------------------------------------------------------------//
		i2c_io() : clock_(slow_clock_), busy_(200) { }

		//-----------------------------------------------------------------//
		/*!
			@brief  初期化
		*/
		//-----------------------------------------------------------------//
		void init() const {
			port_.init();
			port_.scl_dir(1);
			port_.sda_dir(1);
			port_.scl_out(1);
			port_.sda_out(1);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  クロック設定
			@param[in]	clock	パルス５０％待ち時間（単位マイクロ秒）
		*/
		//-----------------------------------------------------------------//
		void set_clock(uint8_t clock) { clock_ = clock; }

		//-----------------------------------------------------------------//
		/*!
			@brief  低速指定（maybe 100KBPS）
		*/
		//-----------------------------------------------------------------//
		void set_slow() { clock_ = slow_clock_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  高速指定（maybe 400KBPS）
		*/
		//-----------------------------------------------------------------//
		void set_fast() { clock_ = fast_clock_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  スレーブデバイスの「待ち」時間の最大値を設定
			@param[in]	busy	待ち時間（単位マイクロ秒）
		*/
		//-----------------------------------------------------------------//
		void set_busy(uint16_t busy) { busy_ = busy; }

		//-----------------------------------------------------------------//
		/*!
			@brief  受信（リード）
			@param[in] address スレーブアドレス（７ビット）
			@param[out]	dst	先
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool recv(uint8_t address, uint8_t* dst, uint8_t num) const {
			start_();
			write_((address << 1) | 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			for(uint8_t n = 0; n < num; ++n) {
				if(!read_(*dst, true)) {
					stop_();
					return false;
				}
				bool f = 0;
				if(n == (num - 1)) f = 1;
				out_ack_(f);
				++dst;
			}
			stop_();
			return true;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  送信（ライト）
			@param[in] address スレーブアドレス（７ビット）
			@param[in]	src	元
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool send(uint8_t address, const uint8_t* src, uint8_t num) const {
			start_();
			write_(address << 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(src, num)) {
				stop_();
				return false;
			}
			stop_();
			return true;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  送信（ライト）
			@param[in] address スレーブアドレス（７ビット）
			@param[in]	first	ファーストデータ
			@param[in]	src	元
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool send(uint8_t address, uint8_t first, const uint8_t* src, uint8_t num) const {
			start_();
			write_(address << 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(first, false)) {
				stop_();
				return false;
			}
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(src, num)) {
				stop_();
				return false;
			}
			stop_();
			return true;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  送信（ライト）
			@param[in] address スレーブアドレス（７ビット）
			@param[in]	first	ファースト・データ
			@param[in]	second	セカンド・データ
			@param[in]	src	元
			@param[in]	num	数
			@return 失敗なら「false」が返る
		*/
		//-----------------------------------------------------------------//
		bool send(uint8_t address, uint8_t first, uint8_t second, const uint8_t* src, uint8_t num) const {
			start_();
			write_(address << 1, false);
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(first, false)) {
				stop_();
				return false;
			}
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(second, false)) {
				stop_();
				return false;
			}
			if(ack_()) {
				stop_();
				return false;
			}

			if(!write_(src, num)) {
				stop_();
				return false;
			}
			stop_();
			return true;
		}
	};
}
<|endoftext|>