<commit_before>/*************************************************************************
*    UrBackup - Client/Server backup system
*    Copyright (C) 2011-2016 Martin Raiber
*
*    This program is free software: you can redistribute it and/or modify
*    it under the terms of the GNU Affero General Public License as published by
*    the Free Software Foundation, either version 3 of the License, or
*    (at your option) any later version.
*
*    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 Affero 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, see <http://www.gnu.org/licenses/>.
**************************************************************************/

#ifndef CLIENT_ONLY

#include "action_header.h"
#include "../server_status.h"
#include "../database.h"

const size_t max_display=20;

void cleanupLastActs()
{
	IDatabase* db = Server->getDatabase(Server->getThreadID(), URBACKUPDB_SERVER);

	db->Write("CREATE INDEX IF NOT EXISTS del_stats_del_idx ON del_stats (clientid, created)");
	db->Write("DELETE FROM del_stats WHERE (SELECT COUNT(*) FROM del_stats b WHERE del_stats.clientid=b.clientid AND b.created<del_stats.created)>"+convert(max_display));
	db->Write("DROP INDEX del_stats_del_idx");
}

void getLastActs(Helper &helper, JSON::Object &ret, std::vector<int> clientids)
{
	std::string filter;
	if(!clientids.empty())
	{
		filter=" AND ( ";
		for(size_t i=0;i<clientids.size();++i)
		{
			filter+="clientid=?";
			if(i+1<clientids.size())
				filter+=" OR ";
		}
		filter+=" ) ";
	}

	IDatabase *db=helper.getDatabase();
	IQuery *q=db->Prepare("SELECT * FROM ("
	"SELECT a.id AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', a.backuptime) AS backuptime, backuptime AS bt,"
	 "incremental, (strftime('%s',running)-strftime('%s',a.backuptime)) AS duration, size_bytes, 0 AS image, 0 AS del, size_calculated, resumed, 0 AS restore, '' AS details "
	"FROM backups a INNER JOIN clients b ON a.clientid=b.id "
	 "WHERE complete=1 "+filter+
	"UNION ALL "
	"SELECT c.id AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', c.backuptime) AS backuptime, backuptime AS bt,"
	"incremental, (strftime('%s',running)-strftime('%s',c.backuptime)) AS duration, (size_bytes+IFNULL(0,("
	"SELECT SUM(size_bytes) FROM backup_images INNER JOIN (SELECT * FROM assoc_images WHERE img_id=c.id) ON assoc_id=id"
	")) ) AS size_bytes, 1 AS image, 0 AS del, 1 as size_calculated, 0 AS resumed, 0 AS restore, letter AS details "
	"FROM backup_images c INNER JOIN clients d ON c.clientid=d.id "
	"WHERE complete=1 AND length(letter)<=2 "+filter+
	"UNION ALL "
	"SELECT e.backupid AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', e.created) AS backuptime, created AS bt,"
	"incremental, (strftime('%s',stoptime)-strftime('%s',e.created)) AS duration, delsize AS size_bytes, image, 1 AS del, 1 AS size_calculated, 0 AS resumed, 0 AS restore, '' AS details "
	"FROM del_stats e INNER JOIN clients f ON e.clientid=f.id "
	"WHERE 1=1 "+filter+
	"UNION ALL "
	"SELECT g.id AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', g.created) AS backuptime, g.created as bt,"
	"0 AS incremental, (strftime('%s',g.finished)-strftime('%s',g.created)) AS duration, -1 AS size_bytes, image, 0 AS del, 0 AS size_calculated, 0 AS resumed, 1 AS restore,"
		" (CASE WHEN image=1 THEN letter ELSE path END) AS details "
	"FROM restores g INNER JOIN clients h ON g.clientid=h.id "
	"WHERE done=1 "+filter+
	") ORDER BY bt DESC LIMIT "+convert(max_display));

	for(size_t i=0;i<clientids.size();++i)
	{
		q->Bind(clientids[i]);
	}
	for(size_t i=0;i<clientids.size();++i)
	{
		q->Bind(clientids[i]);
	}

	db_results res=q->Read();
	q->Reset();

	JSON::Array lastacts;
	for(size_t i=0;i<res.size();++i)
	{
		JSON::Object obj;
		obj.set("id", watoi(res[i]["backupid"]));
		obj.set("clientid", watoi(res[i]["clientid"]));
		obj.set("name", res[i]["name"]);
		obj.set("backuptime", watoi64(res[i]["backuptime"]));
		obj.set("incremental", watoi(res[i]["incremental"]));
		obj.set("duration", watoi64(res[i]["duration"]));
		obj.set("resumed", watoi(res[i]["resumed"]));
		obj.set("restore", watoi(res[i]["restore"]));
		if(watoi(res[i]["size_calculated"])==1)
		{
			obj.set("size_bytes", watoi64(res[i]["size_bytes"]));
		}
		else
		{
			obj.set("size_bytes", -1);
		}
		obj.set("image", watoi(res[i]["image"]));
		obj.set("del", (res[i]["del"]=="1") );
		obj.set("details", res[i]["details"]);
		lastacts.add(obj);
	}
	ret.set("lastacts", lastacts);
}

ACTION_IMPL(lastacts)
{
	Helper helper(tid, &POST, &PARAMS);
	JSON::Object ret;
	SUser *session=helper.getSession();
	if(session!=NULL && session->id==SESSION_ID_INVALID) return;
	std::vector<int> clientids;
	std::string rights=helper.getRights("lastacts");
	if(rights!="none" && rights!="all")
	{
		std::vector<std::string> s_cid;
		Tokenize(rights, s_cid, ",");
		for(size_t i=0;i<s_cid.size();++i)
		{
			clientids.push_back(atoi(s_cid[i].c_str()));
		}
	}

	if(session!=NULL && (rights=="all" || clientids.empty()) )
	{
		getLastActs(helper, ret, clientids);
	}
	else
	{
		ret.set("error", 1);
	}
    helper.Write(ret.stringify(false));
}

#endif //CLIENT_ONLY
<commit_msg>Fix sql<commit_after>/*************************************************************************
*    UrBackup - Client/Server backup system
*    Copyright (C) 2011-2016 Martin Raiber
*
*    This program is free software: you can redistribute it and/or modify
*    it under the terms of the GNU Affero General Public License as published by
*    the Free Software Foundation, either version 3 of the License, or
*    (at your option) any later version.
*
*    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 Affero 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, see <http://www.gnu.org/licenses/>.
**************************************************************************/

#ifndef CLIENT_ONLY

#include "action_header.h"
#include "../server_status.h"
#include "../database.h"

const size_t max_display=20;

void cleanupLastActs()
{
	IDatabase* db = Server->getDatabase(Server->getThreadID(), URBACKUPDB_SERVER);

	db->Write("CREATE INDEX IF NOT EXISTS del_stats_del_idx ON del_stats (clientid, created)");
	db->Write("DELETE FROM del_stats WHERE (SELECT COUNT(*) FROM del_stats b WHERE del_stats.clientid=b.clientid AND b.created<del_stats.created)>"+convert(max_display));
	db->Write("DROP INDEX del_stats_del_idx");
}

void getLastActs(Helper &helper, JSON::Object &ret, std::vector<int> clientids)
{
	std::string filter;
	if(!clientids.empty())
	{
		filter=" AND ( ";
		for(size_t i=0;i<clientids.size();++i)
		{
			filter+="clientid=?";
			if(i+1<clientids.size())
				filter+=" OR ";
		}
		filter+=" ) ";
	}

	IDatabase *db=helper.getDatabase();
	IQuery *q=db->Prepare("SELECT * FROM ("
	"SELECT a.id AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', a.backuptime) AS backuptime, backuptime AS bt,"
	 "incremental, (strftime('%s',running)-strftime('%s',a.backuptime)) AS duration, size_bytes, 0 AS image, 0 AS del, size_calculated, resumed, 0 AS restore, '' AS details "
	"FROM backups a INNER JOIN clients b ON a.clientid=b.id "
	 "WHERE complete=1 "+filter+
	"UNION ALL "
	"SELECT c.id AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', c.backuptime) AS backuptime, backuptime AS bt,"
	"incremental, (strftime('%s',running)-strftime('%s',c.backuptime)) AS duration, (size_bytes+IFNULL(0,("
	"SELECT SUM(size_bytes) FROM backup_images INNER JOIN (SELECT * FROM assoc_images WHERE img_id=c.id) ON assoc_id=id"
	")) ) AS size_bytes, 1 AS image, 0 AS del, 1 as size_calculated, 0 AS resumed, 0 AS restore, letter AS details "
	"FROM backup_images c INNER JOIN clients d ON c.clientid=d.id "
	"WHERE complete=1 AND length(letter)<=2 "+filter+
	"UNION ALL "
	"SELECT e.backupid AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', e.created) AS backuptime, e.created AS bt,"
	"incremental, (strftime('%s',stoptime)-strftime('%s',e.created)) AS duration, delsize AS size_bytes, image, 1 AS del, 1 AS size_calculated, 0 AS resumed, 0 AS restore, '' AS details "
	"FROM del_stats e INNER JOIN clients f ON e.clientid=f.id "
	"WHERE 1=1 "+filter+
	"UNION ALL "
	"SELECT g.id AS backupid, clientid, name, strftime('"+helper.getTimeFormatString()+"', g.created) AS backuptime, g.created as bt,"
	"0 AS incremental, (strftime('%s',g.finished)-strftime('%s',g.created)) AS duration, -1 AS size_bytes, image, 0 AS del, 0 AS size_calculated, 0 AS resumed, 1 AS restore,"
		" (CASE WHEN image=1 THEN letter ELSE path END) AS details "
	"FROM restores g INNER JOIN clients h ON g.clientid=h.id "
	"WHERE done=1 "+filter+
	") ORDER BY bt DESC LIMIT "+convert(max_display));

	for(size_t i=0;i<clientids.size();++i)
	{
		q->Bind(clientids[i]);
	}
	for(size_t i=0;i<clientids.size();++i)
	{
		q->Bind(clientids[i]);
	}

	db_results res=q->Read();
	q->Reset();

	JSON::Array lastacts;
	for(size_t i=0;i<res.size();++i)
	{
		JSON::Object obj;
		obj.set("id", watoi(res[i]["backupid"]));
		obj.set("clientid", watoi(res[i]["clientid"]));
		obj.set("name", res[i]["name"]);
		obj.set("backuptime", watoi64(res[i]["backuptime"]));
		obj.set("incremental", watoi(res[i]["incremental"]));
		obj.set("duration", watoi64(res[i]["duration"]));
		obj.set("resumed", watoi(res[i]["resumed"]));
		obj.set("restore", watoi(res[i]["restore"]));
		if(watoi(res[i]["size_calculated"])==1)
		{
			obj.set("size_bytes", watoi64(res[i]["size_bytes"]));
		}
		else
		{
			obj.set("size_bytes", -1);
		}
		obj.set("image", watoi(res[i]["image"]));
		obj.set("del", (res[i]["del"]=="1") );
		obj.set("details", res[i]["details"]);
		lastacts.add(obj);
	}
	ret.set("lastacts", lastacts);
}

ACTION_IMPL(lastacts)
{
	Helper helper(tid, &POST, &PARAMS);
	JSON::Object ret;
	SUser *session=helper.getSession();
	if(session!=NULL && session->id==SESSION_ID_INVALID) return;
	std::vector<int> clientids;
	std::string rights=helper.getRights("lastacts");
	if(rights!="none" && rights!="all")
	{
		std::vector<std::string> s_cid;
		Tokenize(rights, s_cid, ",");
		for(size_t i=0;i<s_cid.size();++i)
		{
			clientids.push_back(atoi(s_cid[i].c_str()));
		}
	}

	if(session!=NULL && (rights=="all" || clientids.empty()) )
	{
		getLastActs(helper, ret, clientids);
	}
	else
	{
		ret.set("error", 1);
	}
    helper.Write(ret.stringify(false));
}

#endif //CLIENT_ONLY
<|endoftext|>


<commit_before>/*=========================================================================
 *
 *  Copyright UMC Utrecht and contributors
 *
 *  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.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include "itkAdvancedBSplineDeformableTransform.h" // original elastix
#include "itkRecursiveBSplineTransform.h"          // recursive version

// Report timings
#include "itkTimeProbe.h"
#include "itkTimeProbesCollectorBase.h"

#include <fstream>
#include <iomanip>

//-------------------------------------------------------------------------------------

int
main( int argc, char * argv[] )
{
  /** Some basic type definitions.
   * NOTE: don't change the dimension or the spline order, since the
   * hard-coded ground truth depends on this.
   */
  const unsigned int Dimension   = 3;
  const unsigned int SplineOrder = 3;
  typedef double CoordinateRepresentationType;

  /** The number of calls to Evaluate(). Distinguish between
   * Debug and Release mode.
   */
#ifndef NDEBUG
  unsigned int N = static_cast< unsigned int >( 1e3 );
#else
  unsigned int N = static_cast< unsigned int >( 1e5 );
#endif
  std::cerr << "N = " << N << std::endl;

  /** Check. */
  if( argc != 2 )
  {
    std::cerr << "ERROR: You should specify a text file with the B-spline "
              << "transformation parameters." << std::endl;
    return 1;
  }

  /** Typedefs. */
  typedef itk::AdvancedBSplineDeformableTransform<
    CoordinateRepresentationType, Dimension, SplineOrder >    TransformType;
  typedef itk::RecursiveBSplineTransform<
    CoordinateRepresentationType, Dimension, SplineOrder >    RecursiveTransformType;

  typedef TransformType::NumberOfParametersType     NumberOfParametersType;
  typedef TransformType::InputPointType             InputPointType;
  typedef TransformType::ParametersType             ParametersType;
  typedef TransformType::NonZeroJacobianIndicesType NonZeroJacobianIndicesType;
  typedef TransformType::DerivativeType             DerivativeType;
  typedef TransformType::JacobianType               JacobianType;
  typedef TransformType::MovingImageGradientType    MovingImageGradientType;

  typedef itk::Image< CoordinateRepresentationType,
    Dimension >                                         InputImageType;
  typedef InputImageType::RegionType    RegionType;
  typedef InputImageType::SizeType      SizeType;
  typedef InputImageType::IndexType     IndexType;
  typedef InputImageType::SpacingType   SpacingType;
  typedef InputImageType::PointType     OriginType;
  typedef InputImageType::DirectionType DirectionType;

  /** Create the transform. */
  TransformType::Pointer          transform          = TransformType::New();
  RecursiveTransformType::Pointer recursiveTransform = RecursiveTransformType::New();

  /** Setup the B-spline transform:
   * (GridSize 44 43 35)
   * (GridIndex 0 0 0)
   * (GridSpacing 10.7832773148 11.2116431394 11.8648235177)
   * (GridOrigin -237.6759555555 -239.9488431747 -344.2315805162)
   */
  SizeType gridSize;
  gridSize[ 0 ] = 44; gridSize[ 1 ] = 43; gridSize[ 2 ] = 35;
  IndexType gridIndex;
  gridIndex.Fill( 0 );
  RegionType gridRegion;
  gridRegion.SetSize( gridSize );
  gridRegion.SetIndex( gridIndex );
  SpacingType gridSpacing;
  gridSpacing[ 0 ] = 10.7832773148;
  gridSpacing[ 1 ] = 11.2116431394;
  gridSpacing[ 2 ] = 11.8648235177;
  OriginType gridOrigin;
  gridOrigin[ 0 ] = -237.6759555555;
  gridOrigin[ 1 ] = -239.9488431747;
  gridOrigin[ 2 ] = -344.2315805162;
  DirectionType gridDirection;
  gridDirection.SetIdentity();

  transform->SetGridOrigin( gridOrigin );
  transform->SetGridSpacing( gridSpacing );
  transform->SetGridRegion( gridRegion );
  transform->SetGridDirection( gridDirection );

  recursiveTransform->SetGridOrigin( gridOrigin );
  recursiveTransform->SetGridSpacing( gridSpacing );
  recursiveTransform->SetGridRegion( gridRegion );
  recursiveTransform->SetGridDirection( gridDirection );

  /** Now read the parameters as defined in the file par.txt. */
  ParametersType parameters( transform->GetNumberOfParameters() );
  std::ifstream  input( argv[ 1 ] );
  if( input.is_open() )
  {
    for( unsigned int i = 0; i < parameters.GetSize(); ++i )
    {
      input >> parameters[ i ];
    }
  }
  else
  {
    std::cerr << "ERROR: could not open the text file containing the "
              << "parameter values." << std::endl;
    return 1;
  }
  transform->SetParameters( parameters );
  recursiveTransform->SetParameters( parameters );

  /** Declare variables. */
  InputPointType          inputPoint; inputPoint.Fill( 4.1 );
  MovingImageGradientType movingImageGradient;
  movingImageGradient[ 0 ] = 29.43; movingImageGradient[ 0 ] = 18.21; movingImageGradient[ 0 ] = 1.7;
  const NumberOfParametersType nnzji = transform->GetNumberOfNonZeroJacobianIndices();
  JacobianType                 jacobian( Dimension, nnzji );
  DerivativeType               imageJacobian_old( nnzji );
  DerivativeType               imageJacobian_new( nnzji );
  DerivativeType               imageJacobian_recursive( nnzji );
  NonZeroJacobianIndicesType   nzji( nnzji );
  itk::TimeProbesCollectorBase timeCollector;
  double                       sum = 0.0;

  /** Time the plain old way. */
  timeCollector.Start( "JacobianGradient plain old" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Get the TransformJacobian dT/dmu. */
    transform->GetJacobian( inputPoint, jacobian, nzji );

    /** Compute the inner products (dM/dx)^T (dT/dmu). */
    const unsigned int numberOfParametersPerDimension = nnzji / Dimension;
    unsigned int       counter                        = 0;
    for( unsigned int dim = 0; dim < Dimension; ++dim )
    {
      const double imDeriv = movingImageGradient[ dim ];
      for( unsigned int mu = 0; mu < numberOfParametersPerDimension; ++mu )
      {
        imageJacobian_old( counter ) = jacobian( dim, counter ) * imDeriv;
        ++counter;
      }
    }

    sum += imageJacobian_old( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient plain old" );

  /** Time the plain new way. */
  timeCollector.Start( "JacobianGradient plain new" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Compute the inner product of the transform Jacobian dT/dmu and the moving image gradient dM/dx. */
    transform->EvaluateJacobianWithImageGradientProduct(
      inputPoint, movingImageGradient,
      imageJacobian_new, nzji );

    sum += imageJacobian_new( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient plain new" );

  /** Time the recursive old way. */
  timeCollector.Start( "JacobianGradient recursive old" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Compute the inner product of the transform Jacobian dT/dmu and the moving image gradient dM/dx. */
    recursiveTransform->GetJacobian( inputPoint, jacobian, nzji );

    /** Compute the inner products (dM/dx)^T (dT/dmu). */
    const unsigned int numberOfParametersPerDimension = nnzji / Dimension;
    unsigned int       counter                        = 0;
    for( unsigned int dim = 0; dim < Dimension; ++dim )
    {
      const double imDeriv = movingImageGradient[ dim ];
      for( unsigned int mu = 0; mu < numberOfParametersPerDimension; ++mu )
      {
        imageJacobian_old( counter ) = jacobian( dim, counter ) * imDeriv;
        ++counter;
      }
    }

    sum += imageJacobian_old( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient recursive old" );

  /** Time the recursive new way. */
  timeCollector.Start( "JacobianGradient recursive new" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Compute the inner product of the transform Jacobian dT/dmu and the moving image gradient dM/dx. */
    recursiveTransform->EvaluateJacobianWithImageGradientProduct(
      inputPoint, movingImageGradient,
      imageJacobian_new, nzji );

    sum += imageJacobian_new( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient recursive new" );

  /** Report timings. */
  timeCollector.Report();

  // Avoid compiler optimizations, so use sum
  std::cerr << sum << std::endl; // works but ugly on screen

  /**
   *
   * Test accuracy
   *
   */

  transform->EvaluateJacobianWithImageGradientProduct(
    inputPoint, movingImageGradient,
    imageJacobian_old, nzji );

  recursiveTransform->EvaluateJacobianWithImageGradientProduct(
    inputPoint, movingImageGradient,
    imageJacobian_new, nzji );

  double diffNorm = ( imageJacobian_old - imageJacobian_new ).magnitude();
  std::cerr << "Recursive B-spline MSD with previous: " << diffNorm << std::endl;
  if( diffNorm > 1e-5 )
  {
    std::cerr << "ERROR: Recursive B-spline EvaluateJacobianWithImageGradientProduct() returning incorrect result." << std::endl;
    return EXIT_FAILURE;
  }

  /** Return a value. */
  return EXIT_SUCCESS;

} // end main
<commit_msg>BUG: Ensure all elements of movingImageGradient are initialized to avoid propagating uninitialized values.  Makes valgrind happier.<commit_after>/*=========================================================================
 *
 *  Copyright UMC Utrecht and contributors
 *
 *  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.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include "itkAdvancedBSplineDeformableTransform.h" // original elastix
#include "itkRecursiveBSplineTransform.h"          // recursive version

// Report timings
#include "itkTimeProbe.h"
#include "itkTimeProbesCollectorBase.h"

#include <fstream>
#include <iomanip>

//-------------------------------------------------------------------------------------

int
main( int argc, char * argv[] )
{
  /** Some basic type definitions.
   * NOTE: don't change the dimension or the spline order, since the
   * hard-coded ground truth depends on this.
   */
  const unsigned int Dimension   = 3;
  const unsigned int SplineOrder = 3;
  typedef double CoordinateRepresentationType;

  /** The number of calls to Evaluate(). Distinguish between
   * Debug and Release mode.
   */
#ifndef NDEBUG
  unsigned int N = static_cast< unsigned int >( 1e3 );
#else
  unsigned int N = static_cast< unsigned int >( 1e5 );
#endif
  std::cerr << "N = " << N << std::endl;

  /** Check. */
  if( argc != 2 )
  {
    std::cerr << "ERROR: You should specify a text file with the B-spline "
              << "transformation parameters." << std::endl;
    return 1;
  }

  /** Typedefs. */
  typedef itk::AdvancedBSplineDeformableTransform<
    CoordinateRepresentationType, Dimension, SplineOrder >    TransformType;
  typedef itk::RecursiveBSplineTransform<
    CoordinateRepresentationType, Dimension, SplineOrder >    RecursiveTransformType;

  typedef TransformType::NumberOfParametersType     NumberOfParametersType;
  typedef TransformType::InputPointType             InputPointType;
  typedef TransformType::ParametersType             ParametersType;
  typedef TransformType::NonZeroJacobianIndicesType NonZeroJacobianIndicesType;
  typedef TransformType::DerivativeType             DerivativeType;
  typedef TransformType::JacobianType               JacobianType;
  typedef TransformType::MovingImageGradientType    MovingImageGradientType;

  typedef itk::Image< CoordinateRepresentationType,
    Dimension >                                         InputImageType;
  typedef InputImageType::RegionType    RegionType;
  typedef InputImageType::SizeType      SizeType;
  typedef InputImageType::IndexType     IndexType;
  typedef InputImageType::SpacingType   SpacingType;
  typedef InputImageType::PointType     OriginType;
  typedef InputImageType::DirectionType DirectionType;

  /** Create the transform. */
  TransformType::Pointer          transform          = TransformType::New();
  RecursiveTransformType::Pointer recursiveTransform = RecursiveTransformType::New();

  /** Setup the B-spline transform:
   * (GridSize 44 43 35)
   * (GridIndex 0 0 0)
   * (GridSpacing 10.7832773148 11.2116431394 11.8648235177)
   * (GridOrigin -237.6759555555 -239.9488431747 -344.2315805162)
   */
  SizeType gridSize;
  gridSize[ 0 ] = 44; gridSize[ 1 ] = 43; gridSize[ 2 ] = 35;
  IndexType gridIndex;
  gridIndex.Fill( 0 );
  RegionType gridRegion;
  gridRegion.SetSize( gridSize );
  gridRegion.SetIndex( gridIndex );
  SpacingType gridSpacing;
  gridSpacing[ 0 ] = 10.7832773148;
  gridSpacing[ 1 ] = 11.2116431394;
  gridSpacing[ 2 ] = 11.8648235177;
  OriginType gridOrigin;
  gridOrigin[ 0 ] = -237.6759555555;
  gridOrigin[ 1 ] = -239.9488431747;
  gridOrigin[ 2 ] = -344.2315805162;
  DirectionType gridDirection;
  gridDirection.SetIdentity();

  transform->SetGridOrigin( gridOrigin );
  transform->SetGridSpacing( gridSpacing );
  transform->SetGridRegion( gridRegion );
  transform->SetGridDirection( gridDirection );

  recursiveTransform->SetGridOrigin( gridOrigin );
  recursiveTransform->SetGridSpacing( gridSpacing );
  recursiveTransform->SetGridRegion( gridRegion );
  recursiveTransform->SetGridDirection( gridDirection );

  /** Now read the parameters as defined in the file par.txt. */
  ParametersType parameters( transform->GetNumberOfParameters() );
  std::ifstream  input( argv[ 1 ] );
  if( input.is_open() )
  {
    for( unsigned int i = 0; i < parameters.GetSize(); ++i )
    {
      input >> parameters[ i ];
    }
  }
  else
  {
    std::cerr << "ERROR: could not open the text file containing the "
              << "parameter values." << std::endl;
    return 1;
  }
  transform->SetParameters( parameters );
  recursiveTransform->SetParameters( parameters );

  /** Declare variables. */
  InputPointType          inputPoint; inputPoint.Fill( 4.1 );
  MovingImageGradientType movingImageGradient;
  movingImageGradient[ 0 ] = 29.43; movingImageGradient[ 1 ] = 18.21; movingImageGradient[ 2 ] = 1.7;
  const NumberOfParametersType nnzji = transform->GetNumberOfNonZeroJacobianIndices();
  JacobianType                 jacobian( Dimension, nnzji );
  DerivativeType               imageJacobian_old( nnzji );
  DerivativeType               imageJacobian_new( nnzji );
  DerivativeType               imageJacobian_recursive( nnzji );
  NonZeroJacobianIndicesType   nzji( nnzji );
  itk::TimeProbesCollectorBase timeCollector;
  double                       sum = 0.0;

  /** Time the plain old way. */
  timeCollector.Start( "JacobianGradient plain old" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Get the TransformJacobian dT/dmu. */
    transform->GetJacobian( inputPoint, jacobian, nzji );

    /** Compute the inner products (dM/dx)^T (dT/dmu). */
    const unsigned int numberOfParametersPerDimension = nnzji / Dimension;
    unsigned int       counter                        = 0;
    for( unsigned int dim = 0; dim < Dimension; ++dim )
    {
      const double imDeriv = movingImageGradient[ dim ];
      for( unsigned int mu = 0; mu < numberOfParametersPerDimension; ++mu )
      {
        imageJacobian_old( counter ) = jacobian( dim, counter ) * imDeriv;
        ++counter;
      }
    }

    sum += imageJacobian_old( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient plain old" );

  /** Time the plain new way. */
  timeCollector.Start( "JacobianGradient plain new" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Compute the inner product of the transform Jacobian dT/dmu and the moving image gradient dM/dx. */
    transform->EvaluateJacobianWithImageGradientProduct(
      inputPoint, movingImageGradient,
      imageJacobian_new, nzji );

    sum += imageJacobian_new( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient plain new" );

  /** Time the recursive old way. */
  timeCollector.Start( "JacobianGradient recursive old" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Compute the inner product of the transform Jacobian dT/dmu and the moving image gradient dM/dx. */
    recursiveTransform->GetJacobian( inputPoint, jacobian, nzji );

    /** Compute the inner products (dM/dx)^T (dT/dmu). */
    const unsigned int numberOfParametersPerDimension = nnzji / Dimension;
    unsigned int       counter                        = 0;
    for( unsigned int dim = 0; dim < Dimension; ++dim )
    {
      const double imDeriv = movingImageGradient[ dim ];
      for( unsigned int mu = 0; mu < numberOfParametersPerDimension; ++mu )
      {
        imageJacobian_old( counter ) = jacobian( dim, counter ) * imDeriv;
        ++counter;
      }
    }

    sum += imageJacobian_old( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient recursive old" );

  /** Time the recursive new way. */
  timeCollector.Start( "JacobianGradient recursive new" );
  for( unsigned int i = 0; i < N; ++i )
  {
    /** Compute the inner product of the transform Jacobian dT/dmu and the moving image gradient dM/dx. */
    recursiveTransform->EvaluateJacobianWithImageGradientProduct(
      inputPoint, movingImageGradient,
      imageJacobian_new, nzji );

    sum += imageJacobian_new( 0 ); // just to avoid compiler to optimize away
  }
  timeCollector.Stop( "JacobianGradient recursive new" );

  /** Report timings. */
  timeCollector.Report();

  // Avoid compiler optimizations, so use sum
  std::cerr << sum << std::endl; // works but ugly on screen

  /**
   *
   * Test accuracy
   *
   */

  transform->EvaluateJacobianWithImageGradientProduct(
    inputPoint, movingImageGradient,
    imageJacobian_old, nzji );

  recursiveTransform->EvaluateJacobianWithImageGradientProduct(
    inputPoint, movingImageGradient,
    imageJacobian_new, nzji );

  double diffNorm = ( imageJacobian_old - imageJacobian_new ).magnitude();
  std::cerr << "Recursive B-spline MSD with previous: " << diffNorm << std::endl;
  if( diffNorm > 1e-5 )
  {
    std::cerr << "ERROR: Recursive B-spline EvaluateJacobianWithImageGradientProduct() returning incorrect result." << std::endl;
    return EXIT_FAILURE;
  }

  /** Return a value. */
  return EXIT_SUCCESS;

} // end main
<|endoftext|>


<commit_before><commit_msg>Update token.cpp<commit_after><|endoftext|>


<commit_before>/******************************************************************************
*	Main script for the 2017 RoboFishy Scripps AUV
******************************************************************************/

#include "Mapper.h"

// Multithreading
#include <pthread.h>
#include <sched.h>
#include <unistd.h>

// Sampling Values
#define SAMPLE_RATE 200 // sample rate of main control loop (Hz)
#define DT 0.005				// timestep; make sure this is equal to 1/SAMPLE_RATE!

// Conversion Factors
#define UNITS_KPA 0.1 // converts pressure from mbar to kPa

/******************************************************************************
* Controller Gains
******************************************************************************/

// Yaw Controller
#define KP_YAW 0.01
#define KI_YAW 0
#define KD_YAW 1

// Depth Controller
#define KP_DEPTH 0
#define KI_DEPTH 0
#define KD_DEPTH 0

// Saturation Constants
#define YAW_SAT 1			// upper limit of yaw controller
#define DEPTH_SAT 1		// upper limit of depth controller
#define INT_SAT 10		// upper limit of integral windup
#define DINT_SAT 10		// upper limit of depth integral windup

// Filter Values
#define A1 0.3			// for MS5837 pressure sensor
#define A2 0.4			// for MS5837 pressure sensor

// Fluid Densities in kg/m^3
#define DENSITY_FRESHWATER 997
#define DENSITY_SALTWATER 1029

// Acceleration Due to Gravity in m/s^2
#define GRAVITY 9.81

// Depth Start Value
#define DEPTH_START 50 // starting depth (mm)

// Stop Timer
#define STOP_TIME 4		// seconds

// Leak Sensor Inpu and Power Pin
#define LEAKPIN 27		// connected to GPIO 27
#define LEAKPOWERPIN 17 // providing Vcc to leak board

/******************************************************************************
 * Declare Threads
******************************************************************************/

void *navigation(void* arg);
void *depth_thread(void* arg);
void *safety_thread(void* arg);

/******************************************************************************
 * Global Variables
******************************************************************************/

// Holds the setpoint data structure with current setpoints
//setpoint_t setpoint;

// Holds the system state structure with current system statesystem_state_t sstate;
//system_state_t sstate;

// Holds the calibration values for the MS5837 pressure sensor
pressure_calib_t pressure_calib;

// Holds the latest pressure value from the MS5837 pressure sensor
ms5837_t ms5837;

// Create structure for storing IMU data
//bno055_t bno055;

// Holds the latest temperature value from the DS18B20 temperature sensor
ds18b20_t ds18b20;

// Holds the constants and latest errors of the yaw pid controller
pid_data_t yaw_pid;

// Holds the constants and latest errors of the depth pid controller
pid_data_t depth_pid;

// Motor channels
int motor_channels[] = {CHANNEL_1, CHANNEL_2, CHANNEL_3};

// Ignoring sstate
float depth = 0;

//yaw_controller intialization
float motor_percent = 0;

/******************************************************************************
* Main Function
******************************************************************************/

int main()
{
	// Initialize Python interpreter
	Py_Initialize();

	// Set up RasPi GPIO pins through wiringPi
	wiringPiSetupGpio();

	// Check if AUV is initialized correctly
	if( initialize_sensors() < 0 )
	{
		return -1;
	}
	printf("\nAll components are initialized\n");
	substate.mode = INITIALIZING;
    substate.laserarmed = ARMED;

	printf("Starting Threads\n");
	initializeTAttr();

	// Thread handles
	//pthread_t navigationThread;
	//pthread_t depthThread;
	pthread_t safetyThread;
	//pthread_t disarmlaserThread;

	// Create threads using modified attributes
/*	pthread_create (&navigationThread, &tattrmed, navigation, NULL);
	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
	pthread_create (&disarmlaserThread, &tattrlow, disarmLaser, NULL);
*/	pthread_create (&safetyThread, &tattrlow, safety_thread, NULL);

// Destroy the thread attributes
 	destroyTAttr();

	// Start timer!
	time_t start = time(0);

	// Run main while loop, wait until it's time to stop
	while(substate.mode != STOPPED)
	{
		// Check if we've passed the stop time
		if(difftime(time(0),start) > STOP_TIME)
			substate.mode = STOPPED;

		// Sleep a little
		usleep(100000);
	}

	// Exit cleanly
	cleanup_auv();
	return 0;
}

/******************************************************************************
* Depth Thread
*
* For Recording Depth & Determining If AUV is in Water or Not
******************************************************************************/
void *depth_thread(void* arg)
{
	// Initialize pressure sensor
	pressure_calib = init_pressure_sensor();

	while(substate.mode!=STOPPED)
	{
		// Read pressure sensor by passing calibration structure
		ms5837 = ms5837_read(pressure_calib);

		// Calculate depth (no idea what the magic numbers are)
		depth = (ms5837.pressure-1013)*10.197-88.8; // units?
		// 1013: ambient pressure (mbar)
		// 10.197*p_mbar = p_mmH20

		usleep(10000);
	}

	pthread_exit(NULL);
}//*/

/******************************************************************************
 * Navigation Thread
 *
 * For yaw control
 *****************************************************************************/
/*void *navigation(void* arg)
{

	initialize_motors(motor_channels, HERTZ);

	//float output_port;	  // port motor output
	//float output_starboard; // starboard motor output

	float yaw = 0; 			  //Local variable for if statements

	//////////////yaw controller initialization////////////////////////////////
	yaw_pid.old = 0;	    // Initialize old imu data
	yaw_pid.setpoint = 0;   // Initialize setpoint for yaw_controller

	yaw_pid.derr = 0;
	yaw_pid.ierr = 0;	    // Initialize yaw_controller error values
	yaw_pid.kerr = 0;

	yaw_pid.kp = 0.01;
	yaw_pid.kd = 1;		    // Initialize yaw_controller gain values
	yaw_pid.ki = .1;

	yaw_pid.isat = 1;	    // Initialize saturation values
	yaw_pid.SAT  = .2;

	yaw_pid.DT   = .005;    // initialize time step

	/////////////depth controller initialization///////////////////////////////
	depth_pid.setpoint = 2; 	// Range-from-bottom setpoint (meters)
	depth_pid.old	   = 0; 	// Initialize old depth
	depth_pid.DT 	   = .005;	// Initialize depth controller time step

	depth_pid.kp = 0.001;
	depth_pid.kd = 1.0;		// Depth controller gain initialization
	depth_pid.ki = 0.01;

	depth_pid.kerr = 0;
	depth_pid.ierr = 0;	    // Initialize depth controller error values
	depth_pid.derr = 0;

	depth_pid.isat = 1; 	//Depth controller saturation values
	depth_pid.SAT  = .4

	while(substate.mode!=STOPPED)
	{
		// read IMU values from fifo file
		//bno055 = read_imu_fifo();
		substate.imuorientation = read_imu_fifo();

	    if (substate.imuorientation.yaw < 180) //AUV pointed right
		{
			yaw = substate.imuorientation.yaw;
		}
		else //AUV pointed left
		{
			yaw =(substate.imuorientation.yaw-360);
		}

		// Write captured values to screen
	    //printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
			 bno055.yaw, bno055.pitch, bno055.roll,
			 bno055.p, bno055.q, bno055.r,
			 bno055.sys, bno055.gyro, bno055.accel,
			 bno055.mag);*/

		//calculate yaw controller output
/*		motor_percent = marchPID(substate.imuorientation, yaw);

		// Set port motor
		//set_motor(0,motor_percent);

		// Set starboard motor
		//set_motor(1, motor_percent);

		// Sleep for 5 ms //
	  if (substate.imuorientation.yaw < 180)
		{
			yaw_pid.err = abs(substate.imuorientation.yaw - yaw_pid.setpoint);
		}
		else
		{
			yaw_pid.err =abs((substate.imuorientation.yaw - 360) - yaw_pid.setpoint);
		}

		// Write captured values to screen
	  printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
					 bno055.yaw, bno055.pitch, bno055.roll,
					 bno055.p, bno055.q, bno055.r,
					 bno055.sys, bno055.gyro, bno055.accel,
					 bno055.mag);

		//printf("\nYawPID_err: %f Motor Percent: %f ", yaw_pid.err, motor_percent);

		// Sanity test: Check if yaw control works

		// Call yaw controller function
		yaw_controller();

		// Set port motor
		set_motor(0,motor_percent);

		// Set starboard motor
		set_motor(1, motor_percent);

		// Sleep for 5 ms
		usleep(5000);
	}

	set_motor(0, 0);
	set_motor(1, 0);
	set_motor(2, 0);

	pthread_exit(NULL);
}//*/

/******************************************************************************
 * Safety Thread
 *
 * Shuts down AUV if vehicle goes belows 10m, temperature gets too high, or
 * water intrusion is detected
 *****************************************************************************/
void *safety_thread(void* arg)
{
	// Set up WiringPi for use // (not sure if actually needed)
	wiringPiSetup();

	// Leak detection pins
	pinMode(LEAKPIN, INPUT);					// set LEAKPIN as an INPUT
	pinMode(LEAKPOWERPIN, OUTPUT);		// set as output to provide Vcc
	digitalWrite(LEAKPOWERPIN, HIGH);	// write high to provide Vcc

	// Leak checking variables
	int leakState;	// holds the state (HIGH or LOW) of the LEAKPIN

	// Test if temp sensor reads anything
	ds18b20 = read_temp_fifo();
	printf("Temperature: %f degC\n", ds18b20.temperature);

	while( substate.mode != STOPPED )
	{
		// Check if depth threshold has been exceeded
		/*if( substate.fdepth > DEPTH_STOP )
		{
			substate.mode = STOPPED;
			printf("We're too deep! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

		// Check temperature
		// Shut down AUV if housing temperature gets too high

		if( ds18b20.temperature > TEMP_STOP )
		{
			substate.mode = STOPPED;
			printf("It's too hot! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}//*/

		// Check for leak
		/*leakState = digitalRead(LEAKPIN);	// check the state of LEAKPIN
		if( leakState == HIGH )
		{
			substate.mode = STOPPED;
			printf("LEAK DETECTED! Shutting down...\n");
			continue;
		}
		else if (leakState == LOW)
		{
			// We're still good
			substate.mode = RUNNING;
		}*/

		// Check IMU accelerometer for collision (1+ g detected)
		if( (float)fabs(substate.imuorientation.x_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.y_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.z_acc) > 1.0*GRAVITY )
		{
			substate.mode = STOPPED;
			printf("Collision detected. Shutting down...");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

	}
    pthread_exit(NULL);

}

/******************************************************************************
 * Logging Thread
 *
 * Logs the sensor output data into a file
 *****************************************************************************/
/*
PI_THREAD (logging_thread)
{
	while(substate.mode!=STOPPED){
		FILE *fd = fopen("log.txt", "a");
		char buffer[100] = {0};
		// add logging values to the next line
		sprintf(buffer, "%f %f %f %f %i %i %i %i %f %f %f %f\n",sstate.roll, sstate.pitch[0], sstate.yaw[0], sstate.depth[0],sstate.x[0],
		sstate.y[0], sstate.radius[0], setpoint.x - sstate.x[0], sstate.esc_out[0], sstate.esc_out[1], sstate.esc_out[2], sstate.esc_out[3]);
		fputs(buffer, fd);
		fclose(fd);
		//sleep for 100 ms

		usleep(100000);
	}
	return 0;
}
*/
<commit_msg>Changing global variables<commit_after>/******************************************************************************
*	Main script for the 2017 RoboFishy Scripps AUV
******************************************************************************/

#include "Mapper.h"

// Multithreading
#include <pthread.h>
#include <sched.h>
#include <unistd.h>

// Sampling Values
#define SAMPLE_RATE 200 // sample rate of main control loop (Hz)
#define DT 0.005				// timestep; make sure this is equal to 1/SAMPLE_RATE!

// Conversion Factors
#define UNITS_KPA 0.1 // converts pressure from mbar to kPa

/******************************************************************************
* Controller Gains
******************************************************************************/

// Yaw Controller
#define KP_YAW 0.01
#define KI_YAW 0
#define KD_YAW 1

// Depth Controller
#define KP_DEPTH 0
#define KI_DEPTH 0
#define KD_DEPTH 0

// Saturation Constants
#define YAW_SAT 1			// upper limit of yaw controller
#define DEPTH_SAT 1		// upper limit of depth controller
#define INT_SAT 10		// upper limit of integral windup
#define DINT_SAT 10		// upper limit of depth integral windup

// Filter Values
#define A1 0.3			// for MS5837 pressure sensor
#define A2 0.4			// for MS5837 pressure sensor

// Fluid Densities in kg/m^3
#define DENSITY_FRESHWATER 997
#define DENSITY_SALTWATER 1029

// Acceleration Due to Gravity in m/s^2
#define GRAVITY 9.81

// Depth Start Value
#define DEPTH_START 50 // starting depth (mm)

// Stop Timer
#define STOP_TIME 4		// seconds

// Leak Sensor Inpu and Power Pin
#define LEAKPIN 27		// connected to GPIO 27
#define LEAKPOWERPIN 17 // providing Vcc to leak board

/******************************************************************************
 * Declare Threads
******************************************************************************/

void *navigation(void* arg);
void *depth_thread(void* arg);
void *safety_thread(void* arg);

/******************************************************************************
 * Global Variables
******************************************************************************/

// Holds the setpoint data structure with current setpoints
//setpoint_t setpoint;

// Holds the system state structure with current system statesystem_state_t sstate;
//system_state_t sstate;

// Holds the calibration values for the MS5837 pressure sensor
pressure_calib_t pressure_calib;

// Holds the latest pressure value from the MS5837 pressure sensor
ms5837_t ms5837;

// Create structure for storing IMU data
//bno055_t bno055;

// Holds the latest temperature value from the DS18B20 temperature sensor
ds18b20_t ds18b20;

// Holds the constants and latest errors of the yaw pid controller
pid_data_t yaw_pid;

// Holds the constants and latest errors of the depth pid controller
pid_data_t depth_pid;

// Motor channels
int motor_channels[] = {CHANNEL_1, CHANNEL_2, CHANNEL_3};

// Ignoring sstate
float depth = 0;

//yaw_controller intialization
float motor_percent = 0;

/******************************************************************************
* Main Function
******************************************************************************/

int main()
{
	// Initialize Python interpreter
	Py_Initialize();

	// Set up RasPi GPIO pins through wiringPi
	wiringPiSetupGpio();

	// Check if AUV is initialized correctly
	if( initialize_sensors() < 0 )
	{
		return -1;
	}
	printf("\nAll components are initialized\n");
	substate.mode = INITIALIZING;
    substate.laserarmed = ARMED;

	printf("Starting Threads\n");
	initializeTAttr();

	// Thread handles
	//pthread_t navigationThread;
	//pthread_t depthThread;
	pthread_t safetyThread;
	//pthread_t disarmlaserThread;

	// Create threads using modified attributes
/*	pthread_create (&navigationThread, &tattrmed, navigation, NULL);
	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
	pthread_create (&disarmlaserThread, &tattrlow, disarmLaser, NULL);
*/	pthread_create (&safetyThread, &tattrlow, safety_thread, NULL);

// Destroy the thread attributes
 	destroyTAttr();

	// Start timer!
	time_t start = time(0);

	// Run main while loop, wait until it's time to stop
	while(substate.mode != STOPPED)
	{
		// Check if we've passed the stop time
		if(difftime(time(0),start) > STOP_TIME)
			substate.mode = STOPPED;

		// Sleep a little
		usleep(100000);
	}

	// Exit cleanly
	cleanup_auv();
	return 0;
}

/******************************************************************************
* Depth Thread
*
* For Recording Depth & Determining If AUV is in Water or Not
******************************************************************************/
void *depth_thread(void* arg)
{
	// Initialize pressure sensor
	pressure_calib = init_pressure_sensor();

	while(substate.mode!=STOPPED)
	{
		// Read pressure sensor by passing calibration structure
		ms5837 = ms5837_read(pressure_calib);

		// Calculate depth (no idea what the magic numbers are)
		depth = (ms5837.pressure-1013)*10.197-88.8; // units?
		// 1013: ambient pressure (mbar)
		// 10.197*p_mbar = p_mmH20

		usleep(10000);
	}

	pthread_exit(NULL);
}//*/

/******************************************************************************
 * Navigation Thread
 *
 * For yaw control
 *****************************************************************************/
/*void *navigation(void* arg)
{

	initialize_motors(motor_channels, HERTZ);

	//float output_port;	  // port motor output
	//float output_starboard; // starboard motor output

	float yaw = 0; 			  //Local variable for if statements

	//////////////yaw controller initialization////////////////////////////////
	yaw_pid.old = 0;	    // Initialize old imu data
	yaw_pid.setpoint = 0;   // Initialize setpoint for yaw_controller

	yaw_pid.derr = 0;
	yaw_pid.ierr = 0;	    // Initialize yaw_controller error values
	yaw_pid.kerr = 0;

	yaw_pid.kp = KP_YAW;
	yaw_pid.kd = KD_YAW;	// Initialize yaw_controller gain values
	yaw_pid.ki = KI_YAW;

	yaw_pid.isat = INT_SAT;	// Initialize saturation values
	yaw_pid.SAT  = YAW_SAT;

	yaw_pid.DT   = DY;      // initialize time step

	/////////////depth controller initialization///////////////////////////////
	depth_pid.setpoint = 2; 	// Range-from-bottom setpoint (meters)
	depth_pid.old	   = 0; 	// Initialize old depth
	depth_pid.DT 	   = DT;	// Initialize depth controller time step

	depth_pid.kp = 0.001;
	depth_pid.kd = 1.0;		// Depth controller gain initialization
	depth_pid.ki = 0.01;

	depth_pid.kerr = 0;
	depth_pid.ierr = 0;	    // Initialize depth controller error values
	depth_pid.derr = 0;

	depth_pid.isat = 1; 	//Depth controller saturation values
	depth_pid.SAT  = .4

	while(substate.mode!=STOPPED)
	{
		// read IMU values from fifo file
		//bno055 = read_imu_fifo();
		substate.imuorientation = read_imu_fifo();

	    if (substate.imuorientation.yaw < 180) //AUV pointed right
		{
			yaw = substate.imuorientation.yaw;
		}
		else //AUV pointed left
		{
			yaw =(substate.imuorientation.yaw-360);
		}

		// Write captured values to screen
	    //printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
			 bno055.yaw, bno055.pitch, bno055.roll,
			 bno055.p, bno055.q, bno055.r,
			 bno055.sys, bno055.gyro, bno055.accel,
			 bno055.mag);*/

		//calculate yaw controller output
/*		motor_percent = marchPID(substate.imuorientation, yaw);

		// Set port motor
		//set_motor(0,motor_percent);

		// Set starboard motor
		//set_motor(1, motor_percent);

		// Sleep for 5 ms //
	  if (substate.imuorientation.yaw < 180)
		{
			yaw_pid.err = abs(substate.imuorientation.yaw - yaw_pid.setpoint);
		}
		else
		{
			yaw_pid.err =abs((substate.imuorientation.yaw - 360) - yaw_pid.setpoint);
		}

		// Write captured values to screen
	  printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
					 bno055.yaw, bno055.pitch, bno055.roll,
					 bno055.p, bno055.q, bno055.r,
					 bno055.sys, bno055.gyro, bno055.accel,
					 bno055.mag);

		//printf("\nYawPID_err: %f Motor Percent: %f ", yaw_pid.err, motor_percent);

		// Sanity test: Check if yaw control works

		// Call yaw controller function
		yaw_controller();

		// Set port motor
		set_motor(0,motor_percent);

		// Set starboard motor
		set_motor(1, motor_percent);

		// Sleep for 5 ms
		usleep(5000);
	}

	set_motor(0, 0);
	set_motor(1, 0);
	set_motor(2, 0);

	pthread_exit(NULL);
}//*/

/******************************************************************************
 * Safety Thread
 *
 * Shuts down AUV if vehicle goes belows 10m, temperature gets too high, or
 * water intrusion is detected
 *****************************************************************************/
void *safety_thread(void* arg)
{
	// Set up WiringPi for use // (not sure if actually needed)
	wiringPiSetup();

	// Leak detection pins
	pinMode(LEAKPIN, INPUT);					// set LEAKPIN as an INPUT
	pinMode(LEAKPOWERPIN, OUTPUT);		// set as output to provide Vcc
	digitalWrite(LEAKPOWERPIN, HIGH);	// write high to provide Vcc

	// Leak checking variables
	int leakState;	// holds the state (HIGH or LOW) of the LEAKPIN

	// Test if temp sensor reads anything
	ds18b20 = read_temp_fifo();
	printf("Temperature: %f degC\n", ds18b20.temperature);

	while( substate.mode != STOPPED )
	{
		// Check if depth threshold has been exceeded
		/*if( substate.fdepth > DEPTH_STOP )
		{
			substate.mode = STOPPED;
			printf("We're too deep! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

		// Check temperature
		// Shut down AUV if housing temperature gets too high

		if( ds18b20.temperature > TEMP_STOP )
		{
			substate.mode = STOPPED;
			printf("It's too hot! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}//*/

		// Check for leak
		/*leakState = digitalRead(LEAKPIN);	// check the state of LEAKPIN
		if( leakState == HIGH )
		{
			substate.mode = STOPPED;
			printf("LEAK DETECTED! Shutting down...\n");
			continue;
		}
		else if (leakState == LOW)
		{
			// We're still good
			substate.mode = RUNNING;
		}*/

		// Check IMU accelerometer for collision (1+ g detected)
		if( (float)fabs(substate.imuorientation.x_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.y_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.z_acc) > 1.0*GRAVITY )
		{
			substate.mode = STOPPED;
			printf("Collision detected. Shutting down...");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

	}
    pthread_exit(NULL);

}

/******************************************************************************
 * Logging Thread
 *
 * Logs the sensor output data into a file
 *****************************************************************************/
/*
PI_THREAD (logging_thread)
{
	while(substate.mode!=STOPPED){
		FILE *fd = fopen("log.txt", "a");
		char buffer[100] = {0};
		// add logging values to the next line
		sprintf(buffer, "%f %f %f %f %i %i %i %i %f %f %f %f\n",sstate.roll, sstate.pitch[0], sstate.yaw[0], sstate.depth[0],sstate.x[0],
		sstate.y[0], sstate.radius[0], setpoint.x - sstate.x[0], sstate.esc_out[0], sstate.esc_out[1], sstate.esc_out[2], sstate.esc_out[3]);
		fputs(buffer, fd);
		fclose(fd);
		//sleep for 100 ms

		usleep(100000);
	}
	return 0;
}
*/
<|endoftext|>


<commit_before><?hh //strict

/**
 * This file is part of hhpack\typesafety package.
 *
 * (c) Noritaka Horio <holy.shared.design@gmail.com>
 *
 * This source file is subject to the MIT license that is bundled
 * with this source code in the file LICENSE.
 */

namespace hhpack\typesafety\message;

use hhpack\publisher\Message;
use hhpack\typechecker\check\Result;

final class StoppedMessage implements Message
{

    public function __construct(
        private Result $result
    )
    {
    }

}
<commit_msg>add methods<commit_after><?hh //strict

/**
 * This file is part of hhpack\typesafety package.
 *
 * (c) Noritaka Horio <holy.shared.design@gmail.com>
 *
 * This source file is subject to the MIT license that is bundled
 * with this source code in the file LICENSE.
 */

namespace hhpack\typesafety\message;

use hhpack\publisher\Message;
use hhpack\typechecker\check\Result;
use hhpack\typechecker\check\Error;
use Generator;

final class StoppedMessage implements Message
{

    public function __construct(
        private Result $result
    )
    {
    }

    public function isPassed() : bool
    {
        return $this->result->isPassed();
    }

    public function isFailed() : bool
    {
        return $this->result->isPassed() === false;
    }

    public function errors() : Generator<int, Error, void>
    {
        foreach ($this->result->getErrors() as $error) {
            yield $error;
        }
    }

}
<|endoftext|>


<commit_before>/*
 *
 *    Copyright (c) 2020-2021 Project CHIP Authors
 *
 *    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.
 */

/**
 *    @file
 *      This file implements the ExchangeManager class.
 *
 */

#ifndef __STDC_FORMAT_MACROS
#define __STDC_FORMAT_MACROS
#endif

#ifndef __STDC_LIMIT_MACROS
#define __STDC_LIMIT_MACROS
#endif

#include <cstring>
#include <inttypes.h>
#include <stddef.h>

#include <crypto/RandUtils.h>
#include <lib/core/CHIPCore.h>
#include <lib/core/CHIPEncoding.h>
#include <lib/support/CHIPFaultInjection.h>
#include <lib/support/CodeUtils.h>
#include <lib/support/logging/CHIPLogging.h>
#include <messaging/ExchangeContext.h>
#include <messaging/ExchangeMgr.h>
#include <protocols/Protocols.h>

using namespace chip::Encoding;
using namespace chip::Inet;
using namespace chip::System;

namespace chip {
namespace Messaging {

/**
 *  Constructor for the ExchangeManager class.
 *  It sets the state to kState_NotInitialized.
 *
 *  @note
 *    The class must be initialized via ExchangeManager::Init()
 *    prior to use.
 *
 */
ExchangeManager::ExchangeManager() : mReliableMessageMgr(mContextPool)
{
    mState = State::kState_NotInitialized;
}

CHIP_ERROR ExchangeManager::Init(SessionManager * sessionManager)
{
    CHIP_ERROR err = CHIP_NO_ERROR;

    VerifyOrReturnError(mState == State::kState_NotInitialized, err = CHIP_ERROR_INCORRECT_STATE);

    mSessionManager = sessionManager;

    mNextExchangeId = chip::Crypto::GetRandU16();
    mNextKeyId      = 0;

    for (auto & handler : UMHandlerPool)
    {
        // Mark all handlers as unallocated.  This handles both initial
        // initialization and the case when the consumer shuts us down and
        // then re-initializes without removing registered handlers.
        handler.Reset();
    }

    sessionManager->RegisterReleaseDelegate(*this);
    sessionManager->SetMessageDelegate(this);

    mReliableMessageMgr.Init(sessionManager->SystemLayer());
    ReturnErrorOnFailure(mDefaultExchangeDispatch.Init(mSessionManager));

    mState = State::kState_Initialized;

    return err;
}

CHIP_ERROR ExchangeManager::Shutdown()
{
    mReliableMessageMgr.Shutdown();

    mContextPool.ForEachActiveObject([](auto * ec) {
        // There should be no active object in the pool
        VerifyOrDie(false);
        return Loop::Continue;
    });

    if (mSessionManager != nullptr)
    {
        mSessionManager->SetMessageDelegate(nullptr);
        mSessionManager->UnregisterReleaseDelegate(*this);
        mSessionManager = nullptr;
    }

    mState = State::kState_NotInitialized;

    return CHIP_NO_ERROR;
}

ExchangeContext * ExchangeManager::NewContext(SessionHandle session, ExchangeDelegate * delegate)
{
    return mContextPool.CreateObject(this, mNextExchangeId++, session, true, delegate);
}

CHIP_ERROR ExchangeManager::RegisterUnsolicitedMessageHandlerForProtocol(Protocols::Id protocolId, ExchangeDelegate * delegate)
{
    return RegisterUMH(protocolId, kAnyMessageType, delegate);
}

CHIP_ERROR ExchangeManager::RegisterUnsolicitedMessageHandlerForType(Protocols::Id protocolId, uint8_t msgType,
                                                                     ExchangeDelegate * delegate)
{
    return RegisterUMH(protocolId, static_cast<int16_t>(msgType), delegate);
}

CHIP_ERROR ExchangeManager::UnregisterUnsolicitedMessageHandlerForProtocol(Protocols::Id protocolId)
{
    return UnregisterUMH(protocolId, kAnyMessageType);
}

CHIP_ERROR ExchangeManager::UnregisterUnsolicitedMessageHandlerForType(Protocols::Id protocolId, uint8_t msgType)
{
    return UnregisterUMH(protocolId, static_cast<int16_t>(msgType));
}

CHIP_ERROR ExchangeManager::RegisterUMH(Protocols::Id protocolId, int16_t msgType, ExchangeDelegate * delegate)
{
    UnsolicitedMessageHandler * selected = nullptr;

    for (auto & umh : UMHandlerPool)
    {
        if (!umh.IsInUse())
        {
            if (selected == nullptr)
                selected = &umh;
        }
        else if (umh.Matches(protocolId, msgType))
        {
            umh.Delegate = delegate;
            return CHIP_NO_ERROR;
        }
    }

    if (selected == nullptr)
        return CHIP_ERROR_TOO_MANY_UNSOLICITED_MESSAGE_HANDLERS;

    selected->Delegate    = delegate;
    selected->ProtocolId  = protocolId;
    selected->MessageType = msgType;

    SYSTEM_STATS_INCREMENT(chip::System::Stats::kExchangeMgr_NumUMHandlers);

    return CHIP_NO_ERROR;
}

CHIP_ERROR ExchangeManager::UnregisterUMH(Protocols::Id protocolId, int16_t msgType)
{
    for (auto & umh : UMHandlerPool)
    {
        if (umh.IsInUse() && umh.Matches(protocolId, msgType))
        {
            umh.Reset();
            SYSTEM_STATS_DECREMENT(chip::System::Stats::kExchangeMgr_NumUMHandlers);
            return CHIP_NO_ERROR;
        }
    }

    return CHIP_ERROR_NO_UNSOLICITED_MESSAGE_HANDLER;
}

void ExchangeManager::OnMessageReceived(const PacketHeader & packetHeader, const PayloadHeader & payloadHeader,
                                        SessionHandle session, const Transport::PeerAddress & source, DuplicateMessage isDuplicate,
                                        System::PacketBufferHandle && msgBuf)
{
    UnsolicitedMessageHandler * matchingUMH = nullptr;

    ChipLogProgress(ExchangeManager,
                    "Received message of type " ChipLogFormatMessageType " with protocolId " ChipLogFormatProtocolId
                    " and MessageCounter:" ChipLogFormatMessageCounter " on exchange " ChipLogFormatExchangeId,
                    payloadHeader.GetMessageType(), ChipLogValueProtocolId(payloadHeader.GetProtocolID()),
                    packetHeader.GetMessageCounter(), ChipLogValueExchangeIdFromReceivedHeader(payloadHeader));

    MessageFlags msgFlags;
    if (isDuplicate == DuplicateMessage::Yes)
    {
        msgFlags.Set(MessageFlagValues::kDuplicateMessage);
    }

    // Skip retrieval of exchange for group message since no exchange is stored
    // for group msg (optimization)
    if (!packetHeader.IsGroupSession())
    {
        // Search for an existing exchange that the message applies to. If a match is found...
        bool found = false;
        mContextPool.ForEachActiveObject([&](auto * ec) {
            if (ec->MatchExchange(session, packetHeader, payloadHeader))
            {
                // Found a matching exchange. Set flag for correct subsequent MRP
                // retransmission timeout selection.
                if (!ec->HasRcvdMsgFromPeer())
                {
                    ec->SetMsgRcvdFromPeer(true);
                }

                ChipLogDetail(ExchangeManager, "Found matching exchange: " ChipLogFormatExchange ", Delegate: 0x%p",
                              ChipLogValueExchange(ec), ec->GetDelegate());

                // Matched ExchangeContext; send to message handler.
                ec->HandleMessage(packetHeader.GetMessageCounter(), payloadHeader, source, msgFlags, std::move(msgBuf));
                found = true;
                return Loop::Break;
            }
            return Loop::Continue;
        });

        if (found)
        {
            return;
        }
    }

    // If it's not a duplicate message, search for an unsolicited message handler if it is marked as being sent by an initiator.
    // Since we didn't find an existing exchange that matches the message, it must be an unsolicited message. However all
    // unsolicited messages must be marked as being from an initiator.
    if (!msgFlags.Has(MessageFlagValues::kDuplicateMessage) && payloadHeader.IsInitiator())
    {
        // Search for an unsolicited message handler that can handle the message. Prefer handlers that can explicitly
        // handle the message type over handlers that handle all messages for a profile.
        matchingUMH = nullptr;

        for (auto & umh : UMHandlerPool)
        {
            if (umh.IsInUse() && payloadHeader.HasProtocol(umh.ProtocolId))
            {
                if (umh.MessageType == payloadHeader.GetMessageType())
                {
                    matchingUMH = &umh;
                    break;
                }

                if (umh.MessageType == kAnyMessageType)
                    matchingUMH = &umh;
            }
        }
    }
    // Discard the message if it isn't marked as being sent by an initiator and the message does not need to send
    // an ack to the peer.
    else if (!payloadHeader.NeedsAck())
    {
        // Using same error message for all errors to reduce code size.
        ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(CHIP_ERROR_UNSOLICITED_MSG_NO_ORIGINATOR));
        return;
    }

    // If we found a handler or we need to send an ack, create an exchange to
    // handle the message.
    if (matchingUMH != nullptr || payloadHeader.NeedsAck())
    {
        ExchangeDelegate * delegate = matchingUMH ? matchingUMH->Delegate : nullptr;
        // If rcvd msg is from initiator then this exchange is created as not Initiator.
        // If rcvd msg is not from initiator then this exchange is created as Initiator.
        // Note that if matchingUMH is not null then rcvd msg if from initiator.
        // TODO: Figure out which channel to use for the received message
        ExchangeContext * ec =
            mContextPool.CreateObject(this, payloadHeader.GetExchangeID(), session, !payloadHeader.IsInitiator(), delegate);

        if (ec == nullptr)
        {
            // Using same error message for all errors to reduce code size.
            ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(CHIP_ERROR_NO_MEMORY));
            return;
        }

        ChipLogDetail(ExchangeManager, "Handling via exchange: " ChipLogFormatExchange ", Delegate: 0x%p", ChipLogValueExchange(ec),
                      ec->GetDelegate());

        if (ec->IsEncryptionRequired() != packetHeader.IsEncrypted())
        {
            ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(CHIP_ERROR_INVALID_MESSAGE_TYPE));
            ec->Close();
            return;
        }

        CHIP_ERROR err = ec->HandleMessage(packetHeader.GetMessageCounter(), payloadHeader, source, msgFlags, std::move(msgBuf));
        if (err != CHIP_NO_ERROR)
        {
            // Using same error message for all errors to reduce code size.
            ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(err));
        }
    }
}

void ExchangeManager::OnSessionReleased(SessionHandle session)
{
    ExpireExchangesForSession(session);
}

void ExchangeManager::ExpireExchangesForSession(SessionHandle session)
{
    mContextPool.ForEachActiveObject([&](auto * ec) {
        if (ec->mSession.HasValue() && ec->mSession.Value() == session)
        {
            ec->OnConnectionExpired();
            // Continue to iterate because there can be multiple exchanges
            // associated with the connection.
        }
        return Loop::Continue;
    });
}

void ExchangeManager::CloseAllContextsForDelegate(const ExchangeDelegate * delegate)
{
    mContextPool.ForEachActiveObject([&](auto * ec) {
        if (ec->GetDelegate() == delegate)
        {
            // Make sure to null out the delegate before closing the context, so
            // we don't notify the delegate that the context is closing.  We
            // have to do this, because the delegate might be partially
            // destroyed by this point.
            ec->SetDelegate(nullptr);
            ec->Close();
        }
        return Loop::Continue;
    });
}

} // namespace Messaging
} // namespace chip
<commit_msg>Remove redundant 0x from before %p in log formats. (#12519)<commit_after>/*
 *
 *    Copyright (c) 2020-2021 Project CHIP Authors
 *
 *    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.
 */

/**
 *    @file
 *      This file implements the ExchangeManager class.
 *
 */

#ifndef __STDC_FORMAT_MACROS
#define __STDC_FORMAT_MACROS
#endif

#ifndef __STDC_LIMIT_MACROS
#define __STDC_LIMIT_MACROS
#endif

#include <cstring>
#include <inttypes.h>
#include <stddef.h>

#include <crypto/RandUtils.h>
#include <lib/core/CHIPCore.h>
#include <lib/core/CHIPEncoding.h>
#include <lib/support/CHIPFaultInjection.h>
#include <lib/support/CodeUtils.h>
#include <lib/support/logging/CHIPLogging.h>
#include <messaging/ExchangeContext.h>
#include <messaging/ExchangeMgr.h>
#include <protocols/Protocols.h>

using namespace chip::Encoding;
using namespace chip::Inet;
using namespace chip::System;

namespace chip {
namespace Messaging {

/**
 *  Constructor for the ExchangeManager class.
 *  It sets the state to kState_NotInitialized.
 *
 *  @note
 *    The class must be initialized via ExchangeManager::Init()
 *    prior to use.
 *
 */
ExchangeManager::ExchangeManager() : mReliableMessageMgr(mContextPool)
{
    mState = State::kState_NotInitialized;
}

CHIP_ERROR ExchangeManager::Init(SessionManager * sessionManager)
{
    CHIP_ERROR err = CHIP_NO_ERROR;

    VerifyOrReturnError(mState == State::kState_NotInitialized, err = CHIP_ERROR_INCORRECT_STATE);

    mSessionManager = sessionManager;

    mNextExchangeId = chip::Crypto::GetRandU16();
    mNextKeyId      = 0;

    for (auto & handler : UMHandlerPool)
    {
        // Mark all handlers as unallocated.  This handles both initial
        // initialization and the case when the consumer shuts us down and
        // then re-initializes without removing registered handlers.
        handler.Reset();
    }

    sessionManager->RegisterReleaseDelegate(*this);
    sessionManager->SetMessageDelegate(this);

    mReliableMessageMgr.Init(sessionManager->SystemLayer());
    ReturnErrorOnFailure(mDefaultExchangeDispatch.Init(mSessionManager));

    mState = State::kState_Initialized;

    return err;
}

CHIP_ERROR ExchangeManager::Shutdown()
{
    mReliableMessageMgr.Shutdown();

    mContextPool.ForEachActiveObject([](auto * ec) {
        // There should be no active object in the pool
        VerifyOrDie(false);
        return Loop::Continue;
    });

    if (mSessionManager != nullptr)
    {
        mSessionManager->SetMessageDelegate(nullptr);
        mSessionManager->UnregisterReleaseDelegate(*this);
        mSessionManager = nullptr;
    }

    mState = State::kState_NotInitialized;

    return CHIP_NO_ERROR;
}

ExchangeContext * ExchangeManager::NewContext(SessionHandle session, ExchangeDelegate * delegate)
{
    return mContextPool.CreateObject(this, mNextExchangeId++, session, true, delegate);
}

CHIP_ERROR ExchangeManager::RegisterUnsolicitedMessageHandlerForProtocol(Protocols::Id protocolId, ExchangeDelegate * delegate)
{
    return RegisterUMH(protocolId, kAnyMessageType, delegate);
}

CHIP_ERROR ExchangeManager::RegisterUnsolicitedMessageHandlerForType(Protocols::Id protocolId, uint8_t msgType,
                                                                     ExchangeDelegate * delegate)
{
    return RegisterUMH(protocolId, static_cast<int16_t>(msgType), delegate);
}

CHIP_ERROR ExchangeManager::UnregisterUnsolicitedMessageHandlerForProtocol(Protocols::Id protocolId)
{
    return UnregisterUMH(protocolId, kAnyMessageType);
}

CHIP_ERROR ExchangeManager::UnregisterUnsolicitedMessageHandlerForType(Protocols::Id protocolId, uint8_t msgType)
{
    return UnregisterUMH(protocolId, static_cast<int16_t>(msgType));
}

CHIP_ERROR ExchangeManager::RegisterUMH(Protocols::Id protocolId, int16_t msgType, ExchangeDelegate * delegate)
{
    UnsolicitedMessageHandler * selected = nullptr;

    for (auto & umh : UMHandlerPool)
    {
        if (!umh.IsInUse())
        {
            if (selected == nullptr)
                selected = &umh;
        }
        else if (umh.Matches(protocolId, msgType))
        {
            umh.Delegate = delegate;
            return CHIP_NO_ERROR;
        }
    }

    if (selected == nullptr)
        return CHIP_ERROR_TOO_MANY_UNSOLICITED_MESSAGE_HANDLERS;

    selected->Delegate    = delegate;
    selected->ProtocolId  = protocolId;
    selected->MessageType = msgType;

    SYSTEM_STATS_INCREMENT(chip::System::Stats::kExchangeMgr_NumUMHandlers);

    return CHIP_NO_ERROR;
}

CHIP_ERROR ExchangeManager::UnregisterUMH(Protocols::Id protocolId, int16_t msgType)
{
    for (auto & umh : UMHandlerPool)
    {
        if (umh.IsInUse() && umh.Matches(protocolId, msgType))
        {
            umh.Reset();
            SYSTEM_STATS_DECREMENT(chip::System::Stats::kExchangeMgr_NumUMHandlers);
            return CHIP_NO_ERROR;
        }
    }

    return CHIP_ERROR_NO_UNSOLICITED_MESSAGE_HANDLER;
}

void ExchangeManager::OnMessageReceived(const PacketHeader & packetHeader, const PayloadHeader & payloadHeader,
                                        SessionHandle session, const Transport::PeerAddress & source, DuplicateMessage isDuplicate,
                                        System::PacketBufferHandle && msgBuf)
{
    UnsolicitedMessageHandler * matchingUMH = nullptr;

    ChipLogProgress(ExchangeManager,
                    "Received message of type " ChipLogFormatMessageType " with protocolId " ChipLogFormatProtocolId
                    " and MessageCounter:" ChipLogFormatMessageCounter " on exchange " ChipLogFormatExchangeId,
                    payloadHeader.GetMessageType(), ChipLogValueProtocolId(payloadHeader.GetProtocolID()),
                    packetHeader.GetMessageCounter(), ChipLogValueExchangeIdFromReceivedHeader(payloadHeader));

    MessageFlags msgFlags;
    if (isDuplicate == DuplicateMessage::Yes)
    {
        msgFlags.Set(MessageFlagValues::kDuplicateMessage);
    }

    // Skip retrieval of exchange for group message since no exchange is stored
    // for group msg (optimization)
    if (!packetHeader.IsGroupSession())
    {
        // Search for an existing exchange that the message applies to. If a match is found...
        bool found = false;
        mContextPool.ForEachActiveObject([&](auto * ec) {
            if (ec->MatchExchange(session, packetHeader, payloadHeader))
            {
                // Found a matching exchange. Set flag for correct subsequent MRP
                // retransmission timeout selection.
                if (!ec->HasRcvdMsgFromPeer())
                {
                    ec->SetMsgRcvdFromPeer(true);
                }

                ChipLogDetail(ExchangeManager, "Found matching exchange: " ChipLogFormatExchange ", Delegate: %p",
                              ChipLogValueExchange(ec), ec->GetDelegate());

                // Matched ExchangeContext; send to message handler.
                ec->HandleMessage(packetHeader.GetMessageCounter(), payloadHeader, source, msgFlags, std::move(msgBuf));
                found = true;
                return Loop::Break;
            }
            return Loop::Continue;
        });

        if (found)
        {
            return;
        }
    }

    // If it's not a duplicate message, search for an unsolicited message handler if it is marked as being sent by an initiator.
    // Since we didn't find an existing exchange that matches the message, it must be an unsolicited message. However all
    // unsolicited messages must be marked as being from an initiator.
    if (!msgFlags.Has(MessageFlagValues::kDuplicateMessage) && payloadHeader.IsInitiator())
    {
        // Search for an unsolicited message handler that can handle the message. Prefer handlers that can explicitly
        // handle the message type over handlers that handle all messages for a profile.
        matchingUMH = nullptr;

        for (auto & umh : UMHandlerPool)
        {
            if (umh.IsInUse() && payloadHeader.HasProtocol(umh.ProtocolId))
            {
                if (umh.MessageType == payloadHeader.GetMessageType())
                {
                    matchingUMH = &umh;
                    break;
                }

                if (umh.MessageType == kAnyMessageType)
                    matchingUMH = &umh;
            }
        }
    }
    // Discard the message if it isn't marked as being sent by an initiator and the message does not need to send
    // an ack to the peer.
    else if (!payloadHeader.NeedsAck())
    {
        // Using same error message for all errors to reduce code size.
        ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(CHIP_ERROR_UNSOLICITED_MSG_NO_ORIGINATOR));
        return;
    }

    // If we found a handler or we need to send an ack, create an exchange to
    // handle the message.
    if (matchingUMH != nullptr || payloadHeader.NeedsAck())
    {
        ExchangeDelegate * delegate = matchingUMH ? matchingUMH->Delegate : nullptr;
        // If rcvd msg is from initiator then this exchange is created as not Initiator.
        // If rcvd msg is not from initiator then this exchange is created as Initiator.
        // Note that if matchingUMH is not null then rcvd msg if from initiator.
        // TODO: Figure out which channel to use for the received message
        ExchangeContext * ec =
            mContextPool.CreateObject(this, payloadHeader.GetExchangeID(), session, !payloadHeader.IsInitiator(), delegate);

        if (ec == nullptr)
        {
            // Using same error message for all errors to reduce code size.
            ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(CHIP_ERROR_NO_MEMORY));
            return;
        }

        ChipLogDetail(ExchangeManager, "Handling via exchange: " ChipLogFormatExchange ", Delegate: %p", ChipLogValueExchange(ec),
                      ec->GetDelegate());

        if (ec->IsEncryptionRequired() != packetHeader.IsEncrypted())
        {
            ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(CHIP_ERROR_INVALID_MESSAGE_TYPE));
            ec->Close();
            return;
        }

        CHIP_ERROR err = ec->HandleMessage(packetHeader.GetMessageCounter(), payloadHeader, source, msgFlags, std::move(msgBuf));
        if (err != CHIP_NO_ERROR)
        {
            // Using same error message for all errors to reduce code size.
            ChipLogError(ExchangeManager, "OnMessageReceived failed, err = %s", ErrorStr(err));
        }
    }
}

void ExchangeManager::OnSessionReleased(SessionHandle session)
{
    ExpireExchangesForSession(session);
}

void ExchangeManager::ExpireExchangesForSession(SessionHandle session)
{
    mContextPool.ForEachActiveObject([&](auto * ec) {
        if (ec->mSession.HasValue() && ec->mSession.Value() == session)
        {
            ec->OnConnectionExpired();
            // Continue to iterate because there can be multiple exchanges
            // associated with the connection.
        }
        return Loop::Continue;
    });
}

void ExchangeManager::CloseAllContextsForDelegate(const ExchangeDelegate * delegate)
{
    mContextPool.ForEachActiveObject([&](auto * ec) {
        if (ec->GetDelegate() == delegate)
        {
            // Make sure to null out the delegate before closing the context, so
            // we don't notify the delegate that the context is closing.  We
            // have to do this, because the delegate might be partially
            // destroyed by this point.
            ec->SetDelegate(nullptr);
            ec->Close();
        }
        return Loop::Continue;
    });
}

} // namespace Messaging
} // namespace chip
<|endoftext|>