<commit_before>#include "FusionEKF.h"
#include "tools.h"
#include "Eigen/Dense"
#include <iostream>

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::vector;

/*
 * Constructor.
 */
FusionEKF::FusionEKF() {
  is_initialized_ = false;

  previous_timestamp_ = 0;

  // initializing matrices
  R_laser_ = MatrixXd(2, 2);
  R_radar_ = MatrixXd(3, 3);
  H_laser_ = MatrixXd(2, 4);
  Hj_ = MatrixXd(3, 4);

  //measurement covariance matrix - laser
  R_laser_ << 0.0225, 0,
        0, 0.0225;

  //measurement covariance matrix - radar
  R_radar_ << 0.09, 0, 0,
        0, 0.0009, 0,
        0, 0, 0.09;

  /**
  TODO:
    * Finish initializing the FusionEKF.
    * Set the process and measurement noises
    measurement noise is matrix R
    prosess noise is matrix Q
  */
  F_ = MatrixXd(4,4)
  Q_ = MatrixXd(4,4)
}

/**
* Destructor.
*/
FusionEKF::~FusionEKF() {}

void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {

  /*****************************************************************************
   *  Initialization
   ****************************************************************************/
  if (!is_initialized_) {
    /**
    TODO:
      * Initialize the state ekf_.x_ with the first measurement.
      * Create the covariance matrix.
      * Remember: you'll need to convert radar from polar to cartesian coordinates.
    */
    // first measurement
    cout << "EKF: " << endl;
    ekf_.x_ = VectorXd(4);
    ekf_.x_ << 1, 1, 1, 1;

    if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
      /**
      Convert radar from polar to cartesian coordinates and initialize state.
      */

    }
    else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
      /**
      Initialize state.
      */
      efk_.x_(0) = measurement_pack.raw_measurements_(0);
      efk_.x_(1) = measurement_pack.raw_measurements_(1);
    }

    // done initializing, no need to predict or update
    is_initialized_ = true;
    return;
  }

  /*****************************************************************************
   *  Prediction
   ****************************************************************************/

  /**
   TODO:
     * Update the state transition matrix F according to the new elapsed time.
      - Time is measured in seconds.
     * Update the process noise covariance matrix.
     * Use noise_ax = 9 and noise_ay = 9 for your Q matrix.
   */

  ekf_.Predict();

  /*****************************************************************************
   *  Update
   ****************************************************************************/

  /**
   TODO:
     * Use the sensor type to perform the update step.
     * Update the state and covariance matrices.
   */

  if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
    // Radar updates
  } else {
    // Laser updates
  }

  // print the output
  cout << "x_ = " << ekf_.x_ << endl;
  cout << "P_ = " << ekf_.P_ << endl;
}
<commit_msg>Update kf project<commit_after>#include "FusionEKF.h"
#include "tools.h"
#include "Eigen/Dense"
#include <iostream>

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::vector;

/*
 * Constructor.
 */
FusionEKF::FusionEKF() {
  is_initialized_ = false;

  previous_timestamp_ = 0;

  // initializing matrices
  R_laser_ = MatrixXd(2, 2);
  R_radar_ = MatrixXd(3, 3);
  H_laser_ = MatrixXd(2, 4);
  Hj_ = MatrixXd(3, 4);

  //measurement covariance matrix - laser
  R_laser_ << 0.0225, 0,
        0, 0.0225;

  //measurement covariance matrix - radar
  R_radar_ << 0.09, 0, 0,
        0, 0.0009, 0,
        0, 0, 0.09;

  /**
  TODO:
    * Finish initializing the FusionEKF.
    * Set the process and measurement noises
    measurement noise is matrix R
    prosess noise is matrix Q
  */
  F_ = MatrixXd(4,4)
  Q_ = MatrixXd(4,4)
}

/**
* Destructor.
*/
FusionEKF::~FusionEKF() {}

void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {

  /*****************************************************************************
   *  Initialization
   ****************************************************************************/
  if (!is_initialized_) {
    /**
    TODO:
      * Initialize the state ekf_.x_ with the first measurement.
      * Create the covariance matrix.
      * Remember: you'll need to convert radar from polar to cartesian coordinates.
    */
    // first measurement
    cout << "EKF: " << endl;
    ekf_.x_ = VectorXd(4);
    ekf_.x_ << 1, 1, 1, 1;

    if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
      /**
      Convert radar from polar to cartesian coordinates and initialize state.
      */

    }
    else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
      /**
      Initialize state.
      */
      efk_.x_(0) = measurement_pack.raw_measurements_(0);
      efk_.x_(1) = measurement_pack.raw_measurements_(1);
    }

    // done initializing, no need to predict or update
    is_initialized_ = true;
    return;
  }

  /*****************************************************************************
   *  Prediction
   ****************************************************************************/

  /**
   TODO:
     * Update the state transition matrix F according to the new elapsed time.
      - Time is measured in seconds.
     * Update the process noise covariance matrix.
     * Use noise_ax = 9 and noise_ay = 9 for your Q matrix.
   */

  ekf_.Predict();

  /*****************************************************************************
   *  Update
   ****************************************************************************/

  /**
   TODO:
     * Use the sensor type to perform the update step.
     * Update the state and covariance matrices.
   */

  if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
    // Radar updates
  } else {
    // Laser updates
  }

  // print the output
  cout << "x_ = " << ekf_.x_ << endl;
  cout << "P_ = " << ekf_.P_ << endl;
}
<|endoftext|>


<commit_before>/*
 *			GPAC - Multimedia Framework C SDK
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005
 *			Copyright (c) ENST 2008 -
 *					All rights reserved
 *
 *  This file is part of GPAC
 *
 *  GPAC is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *
 *  GPAC is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

/*this file is only used with Win32&MSVC to export the symbols from libgpac(static) to libgpac(dynamic) based on the GPAC configuration*/
#include <gpac/setup.h>

#ifdef _WIN32_WCE
#define EXPORT_SYMBOL(a) "/export:"#a
#else
#define EXPORT_SYMBOL(a) "/export:_"#a
#endif

#pragma comment (linker, EXPORT_SYMBOL(QueryInterfaces) )
#pragma comment (linker, EXPORT_SYMBOL(LoadInterface) )
#pragma comment (linker, EXPORT_SYMBOL(ShutdownInterface) )

<commit_msg>Fixed headers<commit_after>/*
 *			GPAC - Multimedia Framework C SDK
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005
 *			Copyright (c) ENST 2008 -
 *					All rights reserved
 *
 *  This file is part of GPAC
 *
 *  GPAC is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *
 *  GPAC is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

/*this file is only used with Win32&MSVC to export the module interface symbols from each module DLL*/
#include <gpac/setup.h>

#ifdef _WIN32_WCE
#define EXPORT_SYMBOL(a) "/export:"#a
#else
#define EXPORT_SYMBOL(a) "/export:_"#a
#endif

#pragma comment (linker, EXPORT_SYMBOL(QueryInterfaces) )
#pragma comment (linker, EXPORT_SYMBOL(LoadInterface) )
#pragma comment (linker, EXPORT_SYMBOL(ShutdownInterface) )

<|endoftext|>


<commit_before>/* bzflag
 * Copyright (c) 1993 - 2005 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#ifdef _MSC_VER
#pragma warning( 4: 4786 )
#endif

// class-interface header
#include "WorldWeapons.h"

#include "WorldInfo.h"
// system headers
#include <vector>

// common-interface headers
#include "TimeKeeper.h"
#include "ShotUpdate.h"
#include "Protocol.h"
#include "Address.h"
#include "StateDatabase.h"

extern WorldInfo *world;

extern bz_eTeamType convertTeam ( TeamColor team );
extern TeamColor convertTeam( bz_eTeamType team );

char *getDirectMessageBuffer();
void broadcastMessage(uint16_t code, int len, const void *msg);

int fireWorldWep(FlagType* type, float lifetime, PlayerId player, float *pos,
		 float tilt, float direction, int shotID, float dt)
{
  void *buf, *bufStart = getDirectMessageBuffer();

  FiringInfo firingInfo;
  firingInfo.flagType = type;
  firingInfo.lifetime = lifetime;
  firingInfo.shot.player = player;
  memmove(firingInfo.shot.pos, pos, 3 * sizeof(float));
  float shotSpeed = BZDB.eval(StateDatabase::BZDB_SHOTSPEED);
  const float tiltFactor = cosf(tilt);
  firingInfo.shot.vel[0] = shotSpeed * tiltFactor * cosf(direction);
  firingInfo.shot.vel[1] = shotSpeed * tiltFactor * sinf(direction);
  firingInfo.shot.vel[2] = shotSpeed * sinf(tilt);
  firingInfo.shot.id = shotID;
  firingInfo.shot.dt = dt;

  buf = firingInfo.pack(bufStart);

  if (BZDB.isTrue(StateDatabase::BZDB_WEAPONS)) {
    broadcastMessage(MsgShotBegin, (char *)buf - (char *)bufStart, bufStart);
  }
  return shotID;
}

WorldWeapons::WorldWeapons()
: worldShotId(0)
{
}

WorldWeapons::~WorldWeapons()
{
  clear();
}

void WorldWeapons::clear(void)
{
  for (std::vector<Weapon*>::iterator it = weapons.begin();
       it != weapons.end(); ++it) {
    Weapon *w = *it;
    delete w;
  }
  weapons.clear();
}

float WorldWeapons::nextTime ()
{
  TimeKeeper nextShot = TimeKeeper::getSunExplodeTime();
  for (std::vector<Weapon*>::iterator it = weapons.begin();
       it != weapons.end(); ++it) {
    Weapon *w = *it;
    if (w->nextTime <= nextShot) {
      nextShot = w->nextTime;
    }
  }
  return (float)(nextShot - TimeKeeper::getCurrent());
}

void WorldWeapons::fire()
{
  TimeKeeper nowTime = TimeKeeper::getCurrent();

  for (std::vector<Weapon*>::iterator it = weapons.begin();
       it != weapons.end(); ++it) {
    Weapon *w = *it;
    if (w->nextTime <= nowTime) {

      fireWorldWep((FlagType*)w->type, BZDB.eval(StateDatabase::BZDB_RELOADTIME),
		   ServerPlayer, w->origin, w->tilt, w->direction, getNewWorldShotID(), 0);

      //Set up timer for next shot, and eat any shots that have been missed
      while (w->nextTime <= nowTime) {
	w->nextTime += w->delay[w->nextDelay];
	w->nextDelay++;
	if (w->nextDelay == (int)w->delay.size()) {
	  w->nextDelay = 0;
	}
      }
    }
  }
}

void WorldWeapons::add(const FlagType *type, const float *origin,
                       float direction, float tilt,
		       float initdelay, const std::vector<float> &delay,
		       TimeKeeper &sync)
{
  Weapon *w = new Weapon();
  w->type = type;
  memmove(&w->origin, origin, 3*sizeof(float));
  w->direction = direction;
  w->tilt = tilt;
  w->nextTime = sync;
  w->nextTime += initdelay;
  w->initDelay = initdelay;
  w->nextDelay = 0;
  w->delay = delay;

  weapons.push_back(w);
}

unsigned int WorldWeapons::count(void)
{
  return weapons.size();
}

void * WorldWeapons::pack(void *buf) const
{
  buf = nboPackUInt(buf, weapons.size());

  for (unsigned int i=0 ; i < weapons.size(); i++) {
    const Weapon *w = (const Weapon *) weapons[i];
    buf = w->type->pack (buf);
    buf = nboPackVector(buf, w->origin);
    buf = nboPackFloat(buf, w->direction);
    buf = nboPackFloat(buf, w->initDelay);
    buf = nboPackUShort(buf, w->delay.size());
    for (unsigned int j = 0; j < w->delay.size(); j++) {
      buf = nboPackFloat(buf, w->delay[j]);
    }
  }
  return buf;
}

int WorldWeapons::packSize(void) const
{
  int fullSize = 0;

  fullSize += sizeof(uint32_t);

  for (unsigned int i=0 ; i < weapons.size(); i++) {
    const Weapon *w = (const Weapon *) weapons[i];
    fullSize += FlagType::packSize; // flag type
    fullSize += sizeof(float[3]); // pos
    fullSize += sizeof(float);    // direction
    fullSize += sizeof(float);    // init delay
    fullSize += sizeof(uint16_t); // delay count
    for (unsigned int j = 0; j < w->delay.size(); j++) {
      fullSize += sizeof(float);
    }
  }

  return fullSize;
}

int WorldWeapons::getNewWorldShotID ( void )
{
	if (worldShotId > _MAX_WORLD_SHOTS)
		worldShotId = 0;

	return worldShotId++;
}

//----------WorldWeaponGlobalEventHandler---------------------
// where we do the world weapon handling for event based shots since they are not really done by the "world"
WorldWeaponGlobalEventHandler::WorldWeaponGlobalEventHandler(FlagType *_type,
							     const float *_origin,
							     float _direction,
							     float _tilt,TeamColor teamColor )
{
  type = _type;
  if (_origin)
    memcpy(origin,_origin,sizeof(float)*3);
  else
    origin[0] = origin[1] = origin[2] = 0.0f;

  direction = _direction;
  tilt = _tilt;
  team = convertTeam(teamColor);
}

WorldWeaponGlobalEventHandler::~WorldWeaponGlobalEventHandler()
{
}

void WorldWeaponGlobalEventHandler::process (bz_EventData *eventData)
{
  if (!eventData || eventData->eventType != bz_eCaptureEvent)
    return;

  bz_CTFCaptureEventData *capEvent = (bz_CTFCaptureEventData*)eventData;

  if ( capEvent->teamCapped != team )
	  return;

  fireWorldWep( type,BZDB.eval(StateDatabase::BZDB_RELOADTIME),
  ServerPlayer,origin,tilt,direction,world->worldWeapons.getNewWorldShotID(),0);
}

// Local Variables: ***
// mode: C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<commit_msg>init all the fields in firing info.<commit_after>/* bzflag
 * Copyright (c) 1993 - 2005 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#ifdef _MSC_VER
#pragma warning( 4: 4786 )
#endif

// class-interface header
#include "WorldWeapons.h"

#include "WorldInfo.h"
// system headers
#include <vector>

// common-interface headers
#include "TimeKeeper.h"
#include "ShotUpdate.h"
#include "Protocol.h"
#include "Address.h"
#include "StateDatabase.h"

extern WorldInfo *world;

extern bz_eTeamType convertTeam ( TeamColor team );
extern TeamColor convertTeam( bz_eTeamType team );

char *getDirectMessageBuffer();
void broadcastMessage(uint16_t code, int len, const void *msg);

int fireWorldWep(FlagType* type, float lifetime, PlayerId player, float *pos,
		 float tilt, float direction, int shotID, float dt)
{
  void *buf, *bufStart = getDirectMessageBuffer();

  FiringInfo firingInfo;
  firingInfo.timeSent = 0;
  firingInfo.flagType = type;
  firingInfo.lifetime = lifetime;
  firingInfo.shot.player = player;
  memmove(firingInfo.shot.pos, pos, 3 * sizeof(float));
  float shotSpeed = BZDB.eval(StateDatabase::BZDB_SHOTSPEED);
  const float tiltFactor = cosf(tilt);
  firingInfo.shot.vel[0] = shotSpeed * tiltFactor * cosf(direction);
  firingInfo.shot.vel[1] = shotSpeed * tiltFactor * sinf(direction);
  firingInfo.shot.vel[2] = shotSpeed * sinf(tilt);
  firingInfo.shot.id = shotID;
  firingInfo.shot.dt = dt;

  buf = firingInfo.pack(bufStart);

  if (BZDB.isTrue(StateDatabase::BZDB_WEAPONS)) {
    broadcastMessage(MsgShotBegin, (char *)buf - (char *)bufStart, bufStart);
  }
  return shotID;
}

WorldWeapons::WorldWeapons()
: worldShotId(0)
{
}

WorldWeapons::~WorldWeapons()
{
  clear();
}

void WorldWeapons::clear(void)
{
  for (std::vector<Weapon*>::iterator it = weapons.begin();
       it != weapons.end(); ++it) {
    Weapon *w = *it;
    delete w;
  }
  weapons.clear();
}

float WorldWeapons::nextTime ()
{
  TimeKeeper nextShot = TimeKeeper::getSunExplodeTime();
  for (std::vector<Weapon*>::iterator it = weapons.begin();
       it != weapons.end(); ++it) {
    Weapon *w = *it;
    if (w->nextTime <= nextShot) {
      nextShot = w->nextTime;
    }
  }
  return (float)(nextShot - TimeKeeper::getCurrent());
}

void WorldWeapons::fire()
{
  TimeKeeper nowTime = TimeKeeper::getCurrent();

  for (std::vector<Weapon*>::iterator it = weapons.begin();
       it != weapons.end(); ++it) {
    Weapon *w = *it;
    if (w->nextTime <= nowTime) {

      fireWorldWep((FlagType*)w->type, BZDB.eval(StateDatabase::BZDB_RELOADTIME),
		   ServerPlayer, w->origin, w->tilt, w->direction, getNewWorldShotID(), 0);

      //Set up timer for next shot, and eat any shots that have been missed
      while (w->nextTime <= nowTime) {
	w->nextTime += w->delay[w->nextDelay];
	w->nextDelay++;
	if (w->nextDelay == (int)w->delay.size()) {
	  w->nextDelay = 0;
	}
      }
    }
  }
}

void WorldWeapons::add(const FlagType *type, const float *origin,
                       float direction, float tilt,
		       float initdelay, const std::vector<float> &delay,
		       TimeKeeper &sync)
{
  Weapon *w = new Weapon();
  w->type = type;
  memmove(&w->origin, origin, 3*sizeof(float));
  w->direction = direction;
  w->tilt = tilt;
  w->nextTime = sync;
  w->nextTime += initdelay;
  w->initDelay = initdelay;
  w->nextDelay = 0;
  w->delay = delay;

  weapons.push_back(w);
}

unsigned int WorldWeapons::count(void)
{
  return weapons.size();
}

void * WorldWeapons::pack(void *buf) const
{
  buf = nboPackUInt(buf, weapons.size());

  for (unsigned int i=0 ; i < weapons.size(); i++) {
    const Weapon *w = (const Weapon *) weapons[i];
    buf = w->type->pack (buf);
    buf = nboPackVector(buf, w->origin);
    buf = nboPackFloat(buf, w->direction);
    buf = nboPackFloat(buf, w->initDelay);
    buf = nboPackUShort(buf, w->delay.size());
    for (unsigned int j = 0; j < w->delay.size(); j++) {
      buf = nboPackFloat(buf, w->delay[j]);
    }
  }
  return buf;
}

int WorldWeapons::packSize(void) const
{
  int fullSize = 0;

  fullSize += sizeof(uint32_t);

  for (unsigned int i=0 ; i < weapons.size(); i++) {
    const Weapon *w = (const Weapon *) weapons[i];
    fullSize += FlagType::packSize; // flag type
    fullSize += sizeof(float[3]); // pos
    fullSize += sizeof(float);    // direction
    fullSize += sizeof(float);    // init delay
    fullSize += sizeof(uint16_t); // delay count
    for (unsigned int j = 0; j < w->delay.size(); j++) {
      fullSize += sizeof(float);
    }
  }

  return fullSize;
}

int WorldWeapons::getNewWorldShotID ( void )
{
	if (worldShotId > _MAX_WORLD_SHOTS)
		worldShotId = 0;

	return worldShotId++;
}

//----------WorldWeaponGlobalEventHandler---------------------
// where we do the world weapon handling for event based shots since they are not really done by the "world"
WorldWeaponGlobalEventHandler::WorldWeaponGlobalEventHandler(FlagType *_type,
							     const float *_origin,
							     float _direction,
							     float _tilt,TeamColor teamColor )
{
  type = _type;
  if (_origin)
    memcpy(origin,_origin,sizeof(float)*3);
  else
    origin[0] = origin[1] = origin[2] = 0.0f;

  direction = _direction;
  tilt = _tilt;
  team = convertTeam(teamColor);
}

WorldWeaponGlobalEventHandler::~WorldWeaponGlobalEventHandler()
{
}

void WorldWeaponGlobalEventHandler::process (bz_EventData *eventData)
{
  if (!eventData || eventData->eventType != bz_eCaptureEvent)
    return;

  bz_CTFCaptureEventData *capEvent = (bz_CTFCaptureEventData*)eventData;

  if ( capEvent->teamCapped != team )
	  return;

  fireWorldWep( type,BZDB.eval(StateDatabase::BZDB_RELOADTIME),
  ServerPlayer,origin,tilt,direction,world->worldWeapons.getNewWorldShotID(),0);
}

// Local Variables: ***
// mode: C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<|endoftext|>


<commit_before>#include "krypto.h"
#include <QDateTime>

inline const unsigned char * toUChar(const QByteArray & a) { return reinterpret_cast<const unsigned char *>(a.operator const char *()); };

Krypto::Krypto()
{

}

void Krypto::createCert(QByteArray &priv, QByteArray &request, const QString common) {
	mbedtls_entropy_context entropy;
	mbedtls_ctr_drbg_context ctr_drbg;
	mbedtls_pk_context pk; //rsa key pair
	mbedtls_x509write_csr req;
	unsigned char output[4096];
	mbedtls_pk_init(&pk);
	mbedtls_pk_setup(&pk, mbedtls_pk_info_from_type(MBEDTLS_PK_RSA));

	//generate RSA pair
	const char *pers = "rsa_genkey";
	mbedtls_ctr_drbg_init(&ctr_drbg);
	mbedtls_entropy_init(&entropy);
	if (mbedtls_ctr_drbg_seed(&ctr_drbg, mbedtls_entropy_func, &entropy,(const unsigned char *)pers,strlen(pers))
		!= 0)
	{
		mbedtls_ctr_drbg_free(&ctr_drbg);
		mbedtls_entropy_free(&entropy);
		throw new KryptoException("Generating entropy for RSA pair failed.");
	}

	if (mbedtls_rsa_gen_key(mbedtls_pk_rsa(pk), mbedtls_ctr_drbg_random, &ctr_drbg, RSA_SIZE, RSA_EXPONENT)
		!= 0)
	{
		mbedtls_ctr_drbg_free(&ctr_drbg);
		mbedtls_entropy_free(&entropy);
		throw new KryptoException("Generating RSA pair failed.");
	}

	//generate cert request

	mbedtls_x509write_csr_init(&req);
	mbedtls_x509write_csr_set_md_alg(&req, MBEDTLS_MD_SHA256);

	mbedtls_x509write_csr_set_subject_name(&req, common.toStdString().c_str());
	mbedtls_x509write_csr_set_key(&req, &pk);

	//exporting key and cert req

	memset(output, 0, 4096);
	mbedtls_x509write_csr_pem(&req, output, 4096, mbedtls_ctr_drbg_random, &ctr_drbg);
	request = QByteArray(reinterpret_cast<const char *>(output), strlen(reinterpret_cast<const char *>(output)));
	memset(output, 0, 4096);
	mbedtls_pk_write_key_pem(&pk, output, 4096);
	priv = QByteArray(reinterpret_cast<const char *>(output), strlen(reinterpret_cast<const char *>(output)));

	//clean
	mbedtls_pk_free(&pk);
	mbedtls_ctr_drbg_free(&ctr_drbg);
	mbedtls_entropy_free(&entropy);
}

SessionKey::HelperMpi SessionKey::helper;

void SessionKey::setDH(QByteArray dh) {
    std::lock_guard<std::mutex> dhmLock(dhmUse);
    if (mbedtls_dhm_read_public(&dhmc, toUChar(dh), dh.length())) throw KryptoException("setDH: can't read DH");
    other = true;
}

QByteArray SessionKey::getDH() {
    QByteArray ret;

    std::lock_guard<std::mutex> dhmLock(dhmUse);
    ret.resize(dhmc.len);
    if (mbedtls_dhm_make_public(&dhmc, ENCRYPTION_KEY_SIZE, reinterpret_cast<uchar *>(ret.data()), dhmc.len, mbedtls_ctr_drbg_random, &random)) throw KryptoException("getDH: can't generate public dh.");
    my = true;
    return ret;
}

QByteArray SessionKey::conditionalGetDH() {
    QByteArray ret;

    std::lock_guard<std::mutex> dhmLock(dhmUse);
    if (my) return ret;
    ret.resize(dhmc.len);
    if (mbedtls_dhm_make_public(&dhmc, ENCRYPTION_KEY_SIZE, reinterpret_cast<uchar *>(ret.data()), dhmc.len, mbedtls_ctr_drbg_random, &random)) throw KryptoException("getDH: can't generate public dh.");
    my = true;
    return ret;
}

QByteArray SessionKey::protect(const QByteArray & message, const QByteArray & data) {
    mbedtls_gcm_context ctx;
    mbedtls_gcm_init(&ctx);

    {
        std::lock_guard<std::mutex> keyLock(keyUse);
        if (key_enc_uses.fetch_add(1) >= MAX_MESSAGES_WITH_ONE_KEY) throw KryptoOveruseException("Key was already used for 10 encryptions.");
        mbedtls_gcm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, toUChar(currentkey), 256);
    }

    unsigned char iv[IV_LENGTH], tag[TAG_LENGTH];
    mbedtls_ctr_drbg_random(&random, iv, IV_LENGTH);

    QByteArray ret(1, keyid);
    ret.resize(1 + IV_LENGTH + TAG_LENGTH + message.length()); // 1 for keyid + IV + tag + message
    ret.replace(1, IV_LENGTH, reinterpret_cast<const char *>(iv), IV_LENGTH);

    mbedtls_gcm_crypt_and_tag(&ctx, MBEDTLS_GCM_ENCRYPT, message.length(), iv, IV_LENGTH, toUChar(data), data.length(), toUChar(message), reinterpret_cast<uchar *>(ret.data() + 1 + IV_LENGTH + TAG_LENGTH), TAG_LENGTH, tag);

    ret.replace(1 + IV_LENGTH, TAG_LENGTH, reinterpret_cast<const char *>(tag), TAG_LENGTH);
    mbedtls_gcm_free(&ctx);
    return ret;
}

QByteArray SessionKey::unprotect(const QByteArray & message, const QByteArray & data) {
    //message = keyId(1)/iv(16)/tag(TAG_LENGTH)/encData

    unsigned char messageKeyId = message[0];
    mbedtls_gcm_context ctx;
    mbedtls_gcm_init(&ctx);

    {
        std::lock_guard<std::mutex> keyLock(keyUse);
        if (messageKeyId == keyid) {
            if (key_dec_uses.fetch_add(1) >= MAX_MESSAGES_WITH_ONE_KEY) throw KryptoOveruseException("Key was already used for 10 decryptions.");
            mbedtls_gcm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, toUChar(currentkey), 256);
        }
        else if (messageKeyId == keyid - 1) {
            if (key_old_dec_uses.fetch_add(1) >= MAX_MESSAGES_WITH_ONE_KEY) throw KryptoOveruseException("Key was already used for 10 decryptions.");
            mbedtls_gcm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, toUChar(oldkey), 256);
        }
        else {
            throw KryptoException("key is too old");
        }
    }

    size_t dataLength = message.length() - IV_LENGTH - TAG_LENGTH - 1; // iv + tag + keyId
    unsigned char * output = new unsigned char[dataLength];

    int ret_success;
    if ((ret_success = mbedtls_gcm_auth_decrypt(&ctx, dataLength, toUChar(message) + 1, IV_LENGTH, toUChar(data), data.length(), toUChar(message) + 1 + IV_LENGTH, TAG_LENGTH, toUChar(message) + 1 + IV_LENGTH +TAG_LENGTH, output))) {
		if (ret_success == MBEDTLS_ERR_GCM_AUTH_FAILED)	{
			mbedtls_gcm_free(&ctx);
			throw KryptoException("Authentication of message failed");
		}
		else if (ret_success == MBEDTLS_ERR_GCM_BAD_INPUT) {
			mbedtls_gcm_free(&ctx);
			throw KryptoException("Bad message data for unprotect");
		}
		else {
			mbedtls_gcm_free(&ctx);
			throw KryptoException("Unknown Error");
		}
    }

    QByteArray ret(reinterpret_cast<const char *> (output), dataLength);
    delete[] output;
    mbedtls_gcm_free(&ctx);
    return ret;
}

bool SessionKey::generateKey() {
    std::lock_guard<std::mutex> dhmLock (dhmUse);
    if (!my || !other) return false;

    std::lock_guard<std::mutex> keyLock(keyUse);
    oldkey = currentkey;
    ++keyid;

    size_t olen;
    currentkey.resize(ENCRYPTION_KEY_SIZE);
    if (mbedtls_dhm_calc_secret(&dhmc, reinterpret_cast<unsigned char *>(currentkey.data()), ENCRYPTION_KEY_SIZE, &olen, mbedtls_ctr_drbg_random, &random)) throw KryptoException("generateKey: Can't calculate secret.");
    my = other = false;
    key_old_dec_uses = key_dec_uses.exchange(0);
    key_enc_uses = 0;
	keysReady = true;
    return true;
}
<commit_msg>Better error message on key overuse.<commit_after>#include "krypto.h"
#include <QDateTime>

inline const unsigned char * toUChar(const QByteArray & a) { return reinterpret_cast<const unsigned char *>(a.operator const char *()); };

Krypto::Krypto()
{

}

void Krypto::createCert(QByteArray &priv, QByteArray &request, const QString common) {
	mbedtls_entropy_context entropy;
	mbedtls_ctr_drbg_context ctr_drbg;
	mbedtls_pk_context pk; //rsa key pair
	mbedtls_x509write_csr req;
	unsigned char output[4096];
	mbedtls_pk_init(&pk);
	mbedtls_pk_setup(&pk, mbedtls_pk_info_from_type(MBEDTLS_PK_RSA));

	//generate RSA pair
	const char *pers = "rsa_genkey";
	mbedtls_ctr_drbg_init(&ctr_drbg);
	mbedtls_entropy_init(&entropy);
	if (mbedtls_ctr_drbg_seed(&ctr_drbg, mbedtls_entropy_func, &entropy,(const unsigned char *)pers,strlen(pers))
		!= 0)
	{
		mbedtls_ctr_drbg_free(&ctr_drbg);
		mbedtls_entropy_free(&entropy);
		throw new KryptoException("Generating entropy for RSA pair failed.");
	}

	if (mbedtls_rsa_gen_key(mbedtls_pk_rsa(pk), mbedtls_ctr_drbg_random, &ctr_drbg, RSA_SIZE, RSA_EXPONENT)
		!= 0)
	{
		mbedtls_ctr_drbg_free(&ctr_drbg);
		mbedtls_entropy_free(&entropy);
		throw new KryptoException("Generating RSA pair failed.");
	}

	//generate cert request

	mbedtls_x509write_csr_init(&req);
	mbedtls_x509write_csr_set_md_alg(&req, MBEDTLS_MD_SHA256);

	mbedtls_x509write_csr_set_subject_name(&req, common.toStdString().c_str());
	mbedtls_x509write_csr_set_key(&req, &pk);

	//exporting key and cert req

	memset(output, 0, 4096);
	mbedtls_x509write_csr_pem(&req, output, 4096, mbedtls_ctr_drbg_random, &ctr_drbg);
	request = QByteArray(reinterpret_cast<const char *>(output), strlen(reinterpret_cast<const char *>(output)));
	memset(output, 0, 4096);
	mbedtls_pk_write_key_pem(&pk, output, 4096);
	priv = QByteArray(reinterpret_cast<const char *>(output), strlen(reinterpret_cast<const char *>(output)));

	//clean
	mbedtls_pk_free(&pk);
	mbedtls_ctr_drbg_free(&ctr_drbg);
	mbedtls_entropy_free(&entropy);
}

SessionKey::HelperMpi SessionKey::helper;

void SessionKey::setDH(QByteArray dh) {
    std::lock_guard<std::mutex> dhmLock(dhmUse);
    if (mbedtls_dhm_read_public(&dhmc, toUChar(dh), dh.length())) throw KryptoException("setDH: can't read DH");
    other = true;
}

QByteArray SessionKey::getDH() {
    QByteArray ret;

    std::lock_guard<std::mutex> dhmLock(dhmUse);
    ret.resize(dhmc.len);
    if (mbedtls_dhm_make_public(&dhmc, ENCRYPTION_KEY_SIZE, reinterpret_cast<uchar *>(ret.data()), dhmc.len, mbedtls_ctr_drbg_random, &random)) throw KryptoException("getDH: can't generate public dh.");
    my = true;
    return ret;
}

QByteArray SessionKey::conditionalGetDH() {
    QByteArray ret;

    std::lock_guard<std::mutex> dhmLock(dhmUse);
    if (my) return ret;
    ret.resize(dhmc.len);
    if (mbedtls_dhm_make_public(&dhmc, ENCRYPTION_KEY_SIZE, reinterpret_cast<uchar *>(ret.data()), dhmc.len, mbedtls_ctr_drbg_random, &random)) throw KryptoException("getDH: can't generate public dh.");
    my = true;
    return ret;
}

QByteArray SessionKey::protect(const QByteArray & message, const QByteArray & data) {
    mbedtls_gcm_context ctx;
    mbedtls_gcm_init(&ctx);

    {
        std::lock_guard<std::mutex> keyLock(keyUse);
        if (key_enc_uses.fetch_add(1) >= MAX_MESSAGES_WITH_ONE_KEY) throw KryptoOveruseException("Key was already used for max amount of encryptions.");
        mbedtls_gcm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, toUChar(currentkey), 256);
    }

    unsigned char iv[IV_LENGTH], tag[TAG_LENGTH];
    mbedtls_ctr_drbg_random(&random, iv, IV_LENGTH);

    QByteArray ret(1, keyid);
    ret.resize(1 + IV_LENGTH + TAG_LENGTH + message.length()); // 1 for keyid + IV + tag + message
    ret.replace(1, IV_LENGTH, reinterpret_cast<const char *>(iv), IV_LENGTH);

    mbedtls_gcm_crypt_and_tag(&ctx, MBEDTLS_GCM_ENCRYPT, message.length(), iv, IV_LENGTH, toUChar(data), data.length(), toUChar(message), reinterpret_cast<uchar *>(ret.data() + 1 + IV_LENGTH + TAG_LENGTH), TAG_LENGTH, tag);

    ret.replace(1 + IV_LENGTH, TAG_LENGTH, reinterpret_cast<const char *>(tag), TAG_LENGTH);
    mbedtls_gcm_free(&ctx);
    return ret;
}

QByteArray SessionKey::unprotect(const QByteArray & message, const QByteArray & data) {
    //message = keyId(1)/iv(16)/tag(TAG_LENGTH)/encData

    unsigned char messageKeyId = message[0];
    mbedtls_gcm_context ctx;
    mbedtls_gcm_init(&ctx);

    {
        std::lock_guard<std::mutex> keyLock(keyUse);
        if (messageKeyId == keyid) {
            if (key_dec_uses.fetch_add(1) >= MAX_MESSAGES_WITH_ONE_KEY) throw KryptoOveruseException("Key was already used for 10 decryptions.");
            mbedtls_gcm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, toUChar(currentkey), 256);
        }
        else if (messageKeyId == keyid - 1) {
            if (key_old_dec_uses.fetch_add(1) >= MAX_MESSAGES_WITH_ONE_KEY) throw KryptoOveruseException("Key was already used for 10 decryptions.");
            mbedtls_gcm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, toUChar(oldkey), 256);
        }
        else {
            throw KryptoException("key is too old");
        }
    }

    size_t dataLength = message.length() - IV_LENGTH - TAG_LENGTH - 1; // iv + tag + keyId
    unsigned char * output = new unsigned char[dataLength];

    int ret_success;
    if ((ret_success = mbedtls_gcm_auth_decrypt(&ctx, dataLength, toUChar(message) + 1, IV_LENGTH, toUChar(data), data.length(), toUChar(message) + 1 + IV_LENGTH, TAG_LENGTH, toUChar(message) + 1 + IV_LENGTH +TAG_LENGTH, output))) {
		if (ret_success == MBEDTLS_ERR_GCM_AUTH_FAILED)	{
			mbedtls_gcm_free(&ctx);
			throw KryptoException("Authentication of message failed");
		}
		else if (ret_success == MBEDTLS_ERR_GCM_BAD_INPUT) {
			mbedtls_gcm_free(&ctx);
			throw KryptoException("Bad message data for unprotect");
		}
		else {
			mbedtls_gcm_free(&ctx);
			throw KryptoException("Unknown Error");
		}
    }

    QByteArray ret(reinterpret_cast<const char *> (output), dataLength);
    delete[] output;
    mbedtls_gcm_free(&ctx);
    return ret;
}

bool SessionKey::generateKey() {
    std::lock_guard<std::mutex> dhmLock (dhmUse);
    if (!my || !other) return false;

    std::lock_guard<std::mutex> keyLock(keyUse);
    oldkey = currentkey;
    ++keyid;

    size_t olen;
    currentkey.resize(ENCRYPTION_KEY_SIZE);
    if (mbedtls_dhm_calc_secret(&dhmc, reinterpret_cast<unsigned char *>(currentkey.data()), ENCRYPTION_KEY_SIZE, &olen, mbedtls_ctr_drbg_random, &random)) throw KryptoException("generateKey: Can't calculate secret.");
    my = other = false;
    key_old_dec_uses = key_dec_uses.exchange(0);
    key_enc_uses = 0;
	keysReady = true;
    return true;
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
 *   Author: @author Mark Charlebois <charlebm#gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file px4_posix_tasks.c
 * Implementation of existing task API for Linux
 */

#include "px4_log.h"
#include "px4_posix.h"
#include "px4_workqueue.h"
#include "hrt_work.h"
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <stdbool.h>

#if !defined(__PX4_QURT)
#include <signal.h>
#endif

#include <fcntl.h>
#include <sched.h>
#include <unistd.h>
#include <string.h>

#include <sys/stat.h>
#include <sys/types.h>
#include <string>

#include <px4_tasks.h>

#define MAX_CMD_LEN 100

#define PX4_MAX_TASKS 50
#define SHELL_TASK_ID (PX4_MAX_TASKS+1)

pthread_t _shell_task_id = 0;

struct task_entry {
	pthread_t pid;
	std::string name;
	bool isused;
	task_entry() : isused(false) {}
};

static task_entry taskmap[PX4_MAX_TASKS];

typedef struct {
	px4_main_t entry;
	int argc;
	char *argv[];
	// strings are allocated after the
} pthdata_t;

static void *entry_adapter(void *ptr)
{
	pthdata_t *data;
	data = (pthdata_t *) ptr;

	data->entry(data->argc, data->argv);
	PX4_WARN("Before waiting infinte busy loop");
	//for( ;; )
	//{
	//   volatile int x = 0;
	//   ++x;
	// }
	free(ptr);
	px4_task_exit(0);

	return NULL;
}

void
px4_systemreset(bool to_bootloader)
{
	PX4_WARN("Called px4_system_reset");
}

px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
			      char *const argv[])
{
	int rv;
	int argc = 0;
	int i;
	unsigned int len = 0;
	unsigned long offset;
	unsigned long structsize;
	char *p = (char *)argv;

	PX4_DEBUG("Creating %s\n", name);
	pthread_t task;
	pthread_attr_t attr;
	struct sched_param param;

	// Calculate argc
	while (p != (char *)0) {
		p = argv[argc];

		if (p == (char *)0) {
			break;
		}

		++argc;
		len += strlen(p) + 1;
	}

	structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *);
	pthdata_t *taskdata;

	// not safe to pass stack data to the thread creation
	taskdata = (pthdata_t *)malloc(structsize + len);
	offset = ((unsigned long)taskdata) + structsize;

	taskdata->entry = entry;
	taskdata->argc = argc;

	for (i = 0; i < argc; i++) {
		PX4_DEBUG("arg %d %s\n", i, argv[i]);
		taskdata->argv[i] = (char *)offset;
		strcpy((char *)offset, argv[i]);
		offset += strlen(argv[i]) + 1;
	}

	// Must add NULL at end of argv
	taskdata->argv[argc] = (char *)0;

	rv = pthread_attr_init(&attr);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs");
		return (rv < 0) ? rv : -rv;
	}

	rv = pthread_attr_getschedparam(&attr, &param);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param");
		return (rv < 0) ? rv : -rv;
	}

#if 0
	rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched");
		return (rv < 0) ? rv : -rv;
	}

	rv = pthread_attr_setschedpolicy(&attr, scheduler);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to set sched policy");
		return (rv < 0) ? rv : -rv;
	}

#endif
	size_t fixed_stacksize = -1;
	pthread_attr_getstacksize(&attr, &fixed_stacksize);
	PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size);
	fixed_stacksize = 8 * 1024;
	fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize;

	PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize);
	pthread_attr_setstacksize(&attr, fixed_stacksize);
	//pthread_attr_setstacksize(&attr, stack_size);

	param.sched_priority = priority;

	rv = pthread_attr_setschedparam(&attr, &param);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to set sched param");
		return (rv < 0) ? rv : -rv;
	}

	rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata);

	if (rv != 0) {

		return (rv < 0) ? rv : -rv;
	}

	for (i = 0; i < PX4_MAX_TASKS; ++i) {
		if (taskmap[i].isused == false) {
			taskmap[i].pid = task;
			taskmap[i].name = name;
			taskmap[i].isused = true;
			break;
		}
	}

	if (i >= PX4_MAX_TASKS) {
		return -ENOSPC;
	}

	return i;
}

int px4_task_delete(px4_task_t id)
{
	int rv = 0;
	pthread_t pid;
	PX4_WARN("Called px4_task_delete");

	if (id < PX4_MAX_TASKS && taskmap[id].isused) {
		pid = taskmap[id].pid;

	} else {
		return -EINVAL;
	}

	// If current thread then exit, otherwise cancel
	if (pthread_self() == pid) {
		taskmap[id].isused = false;
		pthread_exit(0);

	} else {
		rv = pthread_cancel(pid);
	}

	taskmap[id].isused = false;

	return rv;
}

void px4_task_exit(int ret)
{
	int i;
	pthread_t pid = pthread_self();

	// Get pthread ID from the opaque ID
	for (i = 0; i < PX4_MAX_TASKS; ++i) {
		if (taskmap[i].pid == pid) {
			taskmap[i].isused = false;
			break;
		}
	}

	if (i >= PX4_MAX_TASKS)  {
		PX4_ERR("px4_task_exit: self task not found!");

	} else {
		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str());
	}

	//pthread_exit((void *)(unsigned long)ret);
}

int px4_task_kill(px4_task_t id, int sig)
{
	int rv = 0;
	pthread_t pid;
	PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str());

	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) {
		pid = taskmap[id].pid;

	} else {
		return -EINVAL;
	}

	// If current thread then exit, otherwise cancel
	rv = pthread_kill(pid, sig);

	return rv;
}

void px4_show_tasks()
{
	int idx;
	int count = 0;

	PX4_INFO("Active Tasks:");

	for (idx = 0; idx < PX4_MAX_TASKS; idx++) {
		if (taskmap[idx].isused) {
			PX4_INFO("   %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid);
			count++;
		}
	}

	if (count == 0) {
		PX4_INFO("   No running tasks");
	}

}

__BEGIN_DECLS

unsigned long px4_getpid()
{
	pthread_t pid = pthread_self();
//
//	if (pid == _shell_task_id)
//		return SHELL_TASK_ID;

	// Get pthread ID from the opaque ID
	for (int i = 0; i < PX4_MAX_TASKS; ++i) {
		if (taskmap[i].isused && taskmap[i].pid == pid) {
			return i;
		}
	}

	PX4_ERR("px4_getpid() called from unknown thread context!");
	return ~0;
}

const char *getprogname();
const char *getprogname()
{
	pthread_t pid = pthread_self();

	for (int i = 0; i < PX4_MAX_TASKS; i++) {
		if (taskmap[i].isused && taskmap[i].pid == pid) {
			return taskmap[i].name.c_str();
		}
	}

	return "Unknown App";
}

static void timer_cb(void *data)
{
	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);

	sem_post(sem);
}

int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
{
	void *result;
	work_s _hpwork = {};

	// Create a timer to unblock
	uint32_t timeout = ts->tv_sec*1000000+ (ts->tv_nsec/1000);
	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout);
	sem_wait(sem);
	hrt_work_cancel(&_hpwork);
	return 0;
}

int px4_prctl(int option, const char *arg2, unsigned pid)
{
	int rv;

	switch (option) {
	case PR_SET_NAME:
		// set the threads name - Not supported
		// rv = pthread_setname_np(pthread_self(), arg2);
		rv = -1;
		break;

	default:
		rv = -1;
		PX4_WARN("FAILED SETTING TASK NAME");
		break;
	}

	return rv;
}
__END_DECLS
<commit_msg>Fix QuRT formatting<commit_after>/****************************************************************************
 *
 *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
 *   Author: @author Mark Charlebois <charlebm#gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file px4_posix_tasks.c
 * Implementation of existing task API for Linux
 */

#include "px4_log.h"
#include "px4_posix.h"
#include "px4_workqueue.h"
#include "hrt_work.h"
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <stdbool.h>

#if !defined(__PX4_QURT)
#include <signal.h>
#endif

#include <fcntl.h>
#include <sched.h>
#include <unistd.h>
#include <string.h>

#include <sys/stat.h>
#include <sys/types.h>
#include <string>

#include <px4_tasks.h>

#define MAX_CMD_LEN 100

#define PX4_MAX_TASKS 50
#define SHELL_TASK_ID (PX4_MAX_TASKS+1)

pthread_t _shell_task_id = 0;

struct task_entry {
	pthread_t pid;
	std::string name;
	bool isused;
	task_entry() : isused(false) {}
};

static task_entry taskmap[PX4_MAX_TASKS];

typedef struct {
	px4_main_t entry;
	int argc;
	char *argv[];
	// strings are allocated after the
} pthdata_t;

static void *entry_adapter(void *ptr)
{
	pthdata_t *data;
	data = (pthdata_t *) ptr;

	data->entry(data->argc, data->argv);
	PX4_WARN("Before waiting infinte busy loop");
	//for( ;; )
	//{
	//   volatile int x = 0;
	//   ++x;
	// }
	free(ptr);
	px4_task_exit(0);

	return NULL;
}

void
px4_systemreset(bool to_bootloader)
{
	PX4_WARN("Called px4_system_reset");
}

px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
			      char *const argv[])
{
	int rv;
	int argc = 0;
	int i;
	unsigned int len = 0;
	unsigned long offset;
	unsigned long structsize;
	char *p = (char *)argv;

	PX4_DEBUG("Creating %s\n", name);
	pthread_t task;
	pthread_attr_t attr;
	struct sched_param param;

	// Calculate argc
	while (p != (char *)0) {
		p = argv[argc];

		if (p == (char *)0) {
			break;
		}

		++argc;
		len += strlen(p) + 1;
	}

	structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *);
	pthdata_t *taskdata;

	// not safe to pass stack data to the thread creation
	taskdata = (pthdata_t *)malloc(structsize + len);
	offset = ((unsigned long)taskdata) + structsize;

	taskdata->entry = entry;
	taskdata->argc = argc;

	for (i = 0; i < argc; i++) {
		PX4_DEBUG("arg %d %s\n", i, argv[i]);
		taskdata->argv[i] = (char *)offset;
		strcpy((char *)offset, argv[i]);
		offset += strlen(argv[i]) + 1;
	}

	// Must add NULL at end of argv
	taskdata->argv[argc] = (char *)0;

	rv = pthread_attr_init(&attr);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs");
		return (rv < 0) ? rv : -rv;
	}

	rv = pthread_attr_getschedparam(&attr, &param);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param");
		return (rv < 0) ? rv : -rv;
	}

#if 0
	rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched");
		return (rv < 0) ? rv : -rv;
	}

	rv = pthread_attr_setschedpolicy(&attr, scheduler);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to set sched policy");
		return (rv < 0) ? rv : -rv;
	}

#endif
	size_t fixed_stacksize = -1;
	pthread_attr_getstacksize(&attr, &fixed_stacksize);
	PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size);
	fixed_stacksize = 8 * 1024;
	fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize;

	PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize);
	pthread_attr_setstacksize(&attr, fixed_stacksize);
	//pthread_attr_setstacksize(&attr, stack_size);

	param.sched_priority = priority;

	rv = pthread_attr_setschedparam(&attr, &param);

	if (rv != 0) {
		PX4_WARN("px4_task_spawn_cmd: failed to set sched param");
		return (rv < 0) ? rv : -rv;
	}

	rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata);

	if (rv != 0) {

		return (rv < 0) ? rv : -rv;
	}

	for (i = 0; i < PX4_MAX_TASKS; ++i) {
		if (taskmap[i].isused == false) {
			taskmap[i].pid = task;
			taskmap[i].name = name;
			taskmap[i].isused = true;
			break;
		}
	}

	if (i >= PX4_MAX_TASKS) {
		return -ENOSPC;
	}

	return i;
}

int px4_task_delete(px4_task_t id)
{
	int rv = 0;
	pthread_t pid;
	PX4_WARN("Called px4_task_delete");

	if (id < PX4_MAX_TASKS && taskmap[id].isused) {
		pid = taskmap[id].pid;

	} else {
		return -EINVAL;
	}

	// If current thread then exit, otherwise cancel
	if (pthread_self() == pid) {
		taskmap[id].isused = false;
		pthread_exit(0);

	} else {
		rv = pthread_cancel(pid);
	}

	taskmap[id].isused = false;

	return rv;
}

void px4_task_exit(int ret)
{
	int i;
	pthread_t pid = pthread_self();

	// Get pthread ID from the opaque ID
	for (i = 0; i < PX4_MAX_TASKS; ++i) {
		if (taskmap[i].pid == pid) {
			taskmap[i].isused = false;
			break;
		}
	}

	if (i >= PX4_MAX_TASKS)  {
		PX4_ERR("px4_task_exit: self task not found!");

	} else {
		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str());
	}

	//pthread_exit((void *)(unsigned long)ret);
}

int px4_task_kill(px4_task_t id, int sig)
{
	int rv = 0;
	pthread_t pid;
	PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str());

	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) {
		pid = taskmap[id].pid;

	} else {
		return -EINVAL;
	}

	// If current thread then exit, otherwise cancel
	rv = pthread_kill(pid, sig);

	return rv;
}

void px4_show_tasks()
{
	int idx;
	int count = 0;

	PX4_INFO("Active Tasks:");

	for (idx = 0; idx < PX4_MAX_TASKS; idx++) {
		if (taskmap[idx].isused) {
			PX4_INFO("   %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid);
			count++;
		}
	}

	if (count == 0) {
		PX4_INFO("   No running tasks");
	}

}

__BEGIN_DECLS

unsigned long px4_getpid()
{
	pthread_t pid = pthread_self();
//
//	if (pid == _shell_task_id)
//		return SHELL_TASK_ID;

	// Get pthread ID from the opaque ID
	for (int i = 0; i < PX4_MAX_TASKS; ++i) {
		if (taskmap[i].isused && taskmap[i].pid == pid) {
			return i;
		}
	}

	PX4_ERR("px4_getpid() called from unknown thread context!");
	return ~0;
}

const char *getprogname();
const char *getprogname()
{
	pthread_t pid = pthread_self();

	for (int i = 0; i < PX4_MAX_TASKS; i++) {
		if (taskmap[i].isused && taskmap[i].pid == pid) {
			return taskmap[i].name.c_str();
		}
	}

	return "Unknown App";
}

static void timer_cb(void *data)
{
	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);

	sem_post(sem);
}

int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
{
	void *result;
	work_s _hpwork = {};

	// Create a timer to unblock
	uint32_t timeout = ts->tv_sec * 1000000 + (ts->tv_nsec / 1000);
	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout);
	sem_wait(sem);
	hrt_work_cancel(&_hpwork);
	return 0;
}

int px4_prctl(int option, const char *arg2, unsigned pid)
{
	int rv;

	switch (option) {
	case PR_SET_NAME:
		// set the threads name - Not supported
		// rv = pthread_setname_np(pthread_self(), arg2);
		rv = -1;
		break;

	default:
		rv = -1;
		PX4_WARN("FAILED SETTING TASK NAME");
		break;
	}

	return rv;
}
__END_DECLS
<|endoftext|>


<commit_before>/*=============================================================================
	Copyright (c) 2012-2013 Richard Otis

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

// evaluate.cpp -- evaluate energies from a Database

#include "libgibbs/include/libgibbs_pch.hpp"
#include "libgibbs/include/equilibrium.hpp"
#include "libtdb/include/database.hpp"
#include "libtdb/include/structure.hpp"
#include "libtdb/include/exceptions.hpp"
#include "libgibbs/include/optimizer/optimizer.hpp"
#include "libgibbs/include/optimizer/opt_Gibbs.hpp"
#include "external/coin/IpIpoptApplication.hpp"
#include "external/coin/IpSolveStatistics.hpp"
#include <iostream>
#include <fstream>
#include <algorithm>
#include <sstream>
#include <boost/io/ios_state.hpp>

using namespace Ipopt;

Equilibrium::Equilibrium(const Database &DB, const evalconditions &conds)
: sourcename(DB.get_info()), conditions(conds) {
	Phase_Collection phase_col;
	for (auto i = DB.get_phase_iterator(); i != DB.get_phase_iterator_end(); ++i) {
		if (conds.phases.find(i->first) != conds.phases.end()) {
			if (conds.phases.at(i->first) == true) phase_col[i->first] = i->second;
		}
	}

	const Phase_Collection::const_iterator phase_iter = phase_col.cbegin();
	const Phase_Collection::const_iterator phase_end = phase_col.cend();

	// TODO: check validity of conditions

	// Create an instance of your nlp...
	SmartPtr<TNLP> mynlp = new GibbsOpt(phase_iter, phase_end, conds);

	// Create an instance of the IpoptApplication
	//
	// We are using the factory, since this allows us to compile this
	// example with an Ipopt Windows DLL
	SmartPtr<IpoptApplication> app = IpoptApplicationFactory();

	app->Options()->SetStringValue("derivative_test","first-order");
	app->Options()->SetStringValue("hessian_approximation","limited-memory");
	//app->Options()->SetNumericValue("bound_relax_factor",1e-7);
	//app->Options()->SetIntegerValue("print_level",12);
	//app->Options()->SetStringValue("derivative_test_print_all","yes");

	// Initialize the IpoptApplication and process the options
	ApplicationReturnStatus status;
	status = app->Initialize();
	if (status != Solve_Succeeded) {
		std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
		BOOST_THROW_EXCEPTION(equilibrium_error() << str_errinfo("Error initializing solver"));
	}

	status = app->OptimizeTNLP(mynlp);

	if (status == Solve_Succeeded ||
			status == Solved_To_Acceptable_Level ||
			status == Search_Direction_Becomes_Too_Small) {
		// Retrieve some statistics about the solve
		Index iter_count = app->Statistics()->IterationCount();
		std::cout << std::endl << std::endl << "*** The problem solved in " << iter_count << " iterations!" << std::endl;

		Number final_obj = app->Statistics()->FinalObjective();
		mingibbs = final_obj * conds.statevars.find('N')->second;
		GibbsOpt* opt_ptr = dynamic_cast<GibbsOpt*> (Ipopt::GetRawPtr(mynlp));
		if (!opt_ptr)
			BOOST_THROW_EXCEPTION(equilibrium_error() << str_errinfo("Internal memory error") << specific_errinfo("dynamic_cast<GibbsOpt*>"));
		ph_map = opt_ptr->get_phase_map();
	}
	else {
		BOOST_THROW_EXCEPTION(equilibrium_error() << str_errinfo("Solver failed to find equilibrium"));
	}
}

std::ostream& operator<< (std::ostream& stream, const Equilibrium& eq) {
	boost::io::ios_flags_saver  ifs( stream ); // preserve original state of the stream once we leave scope
	stream << "Output from LIBGIBBS, equilibrium number = ??" << std::endl;
	stream << "Conditions:" << std::endl;

	// We want the individual phase information to appear AFTER
	// the global system data, but the most efficient ordering
	// requires us to iterate through all phases first.
	// The simple solution is to save the output to a temporary
	// buffer, and then flush it to the output stream later.
	std::stringstream temp_buf;
    temp_buf << std::scientific;

	const auto sv_end = eq.conditions.statevars.cend();
	const auto xf_end = eq.conditions.xfrac.cend();
	for (auto i = eq.conditions.xfrac.cbegin(); i !=xf_end; ++i) {
		stream << "X(" << i->first << ")=" << i->second << ", ";
	}
	for (auto i = eq.conditions.statevars.cbegin(); i != sv_end; ++i) {
		stream << i->first << "=" << i->second;
		// if this isn't the last one, add a comma
		if (std::distance(i,sv_end) != 1) stream << ", ";
	}
	stream << std::endl;
	// should be c + 2 - conditions, where c is the number of components
	size_t dof = eq.conditions.elements.size() + 2 - (eq.conditions.xfrac.size()+1) - eq.conditions.statevars.size();
	stream << "DEGREES OF FREEDOM " << dof << std::endl;

	stream << std::endl;

	double T,P,N;
	T = eq.conditions.statevars.find('T')->second;
	P = eq.conditions.statevars.find('P')->second;
	N = eq.conditions.statevars.find('N')->second;
	stream << "Temperature " << T << " K (" << (T-273.15) << " C), " << "Pressure " << P << " Pa" << std::endl;

	stream << std::scientific; // switch to scientific notation for doubles
    stream << "Number of moles of components " << N << ", Mass ????" << std::endl;
    stream << "Total Gibbs energy " << eq.mingibbs << " Enthalpy ???? " << "Volume ????" << std::endl;

    stream << std::endl;

    const auto ph_end = eq.ph_map.cend();
    // double/double pair is for separate storage of numerator/denominator pieces of fraction
    std::map<std::string,std::pair<double,double>> global_comp;
    for (auto i = eq.ph_map.cbegin(); i != ph_end; ++i) {
    	const auto subl_begin = i->second.second.cbegin();
    	const auto subl_end = i->second.second.cend();
    	const double phasefrac = i->second.first;
    	std::map<std::string,std::pair<double,double>> phase_comp;
    	temp_buf << i->first << "\tStatus ENTERED  Driving force 0" << std::endl; // phase name
    	temp_buf << "Number of moles " << i->second.first * N << ", Mass ???? ";
    	temp_buf << "Mole fractions:" << std::endl;
    	for (auto j = subl_begin; j != subl_end; ++j) {
    		const double stoi_coef = j->first;
    		const double den = stoi_coef;
    		const auto cond_spec_begin = eq.conditions.elements.cbegin();
    		const auto cond_spec_end = eq.conditions.elements.cend();
    		const auto spec_begin = j->second.cbegin();
    		const auto spec_end = j->second.cend();
    		const auto vacancy_iterator = (j->second).find("VA"); // this may point to spec_end
    		/*
    		 * To make sure all mole fractions sum to 1,
    		 * we have to normalize everything using the same
    		 * sublattices. That means even species which don't
    		 * appear in a given sublattice need to have that
    		 * sublattice's coefficient appear in its denominator.
    		 * To accomplish this task, we perform two loops in each sublattice:
    		 * 1) all species (except VA), to add to the denominator
    		 * 2) only species in that sublattice, to add to the numerator
    		 * With this method, all mole fractions will sum properly.
    		 */
    		for (auto k = cond_spec_begin; k != cond_spec_end; ++k) {
    			if (*k == "VA") continue; // vacancies don't contribute to mole fractions
				if (vacancy_iterator != spec_end) {
					phase_comp[*k].second += den * (1 - vacancy_iterator->second);
					global_comp[*k].second += phasefrac * den * (1 - vacancy_iterator->second);
				}
				else {
					phase_comp[*k].second += den;
					global_comp[*k].second += phasefrac * den;
				}
    		}
    		for (auto k = spec_begin; k != spec_end; ++k) {
    			if (k->first == "VA") continue; // vacancies don't contribute to mole fractions
                double num = k->second * stoi_coef;
                phase_comp[k->first].first += num;
                global_comp[k->first].first += phasefrac * num;
    		}
    	}
    	const auto cmp_begin = phase_comp.cbegin();
    	const auto cmp_end = phase_comp.cend();
    	for (auto g = cmp_begin; g != cmp_end; ++g) {
    		temp_buf << g->first << " " << (g->second.first / g->second.second) << "  ";
    	}
    	temp_buf << std::endl << "Constitution:" << std::endl;

    	for (auto j = subl_begin; j != subl_end; ++j) {
    		double stoi_coef = j->first;
    		temp_buf << "Sublattice " << std::distance(subl_begin,j)+1 << ", Number of sites " << stoi_coef << std::endl;
    		const auto spec_begin = j->second.cbegin();
    		const auto spec_end = j->second.cend();
    		for (auto k = spec_begin; k != spec_end; ++k) {
                temp_buf << k->first << " " << k->second << " ";
    		}
    		temp_buf << std::endl;
    	}

    	// if we're at the last phase, don't add an extra newline
    	if (std::distance(i,ph_end) != 1) temp_buf << std::endl;
    }
    const auto glob_begin = global_comp.cbegin();
    const auto glob_end = global_comp.cend();
    stream << "Component\tMoles\tW-Fraction\tActivity\tPotential\tRef.state" << std::endl;
    for (auto h = glob_begin; h != glob_end; ++h) {
    	stream << h->first << " " << (h->second.first / h->second.second) * N << " ???? ???? ???? ????" << std::endl;
    }
    stream << std::endl;

    stream << temp_buf.rdbuf(); // include the temporary buffer with all the phase data

	return stream;
}
<commit_msg>Fixed display issue for how Equilibrium calculates overall composition.<commit_after>/*=============================================================================
	Copyright (c) 2012-2013 Richard Otis

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

// evaluate.cpp -- evaluate energies from a Database

#include "libgibbs/include/libgibbs_pch.hpp"
#include "libgibbs/include/equilibrium.hpp"
#include "libtdb/include/database.hpp"
#include "libtdb/include/structure.hpp"
#include "libtdb/include/exceptions.hpp"
#include "libgibbs/include/optimizer/optimizer.hpp"
#include "libgibbs/include/optimizer/opt_Gibbs.hpp"
#include "external/coin/IpIpoptApplication.hpp"
#include "external/coin/IpSolveStatistics.hpp"
#include <iostream>
#include <fstream>
#include <algorithm>
#include <sstream>
#include <boost/io/ios_state.hpp>

using namespace Ipopt;

Equilibrium::Equilibrium(const Database &DB, const evalconditions &conds)
: sourcename(DB.get_info()), conditions(conds) {
	Phase_Collection phase_col;
	for (auto i = DB.get_phase_iterator(); i != DB.get_phase_iterator_end(); ++i) {
		if (conds.phases.find(i->first) != conds.phases.end()) {
			if (conds.phases.at(i->first) == true) phase_col[i->first] = i->second;
		}
	}

	const Phase_Collection::const_iterator phase_iter = phase_col.cbegin();
	const Phase_Collection::const_iterator phase_end = phase_col.cend();

	// TODO: check validity of conditions

	// Create an instance of your nlp...
	SmartPtr<TNLP> mynlp = new GibbsOpt(phase_iter, phase_end, conds);

	// Create an instance of the IpoptApplication
	//
	// We are using the factory, since this allows us to compile this
	// example with an Ipopt Windows DLL
	SmartPtr<IpoptApplication> app = IpoptApplicationFactory();

	app->Options()->SetStringValue("derivative_test","first-order");
	app->Options()->SetStringValue("hessian_approximation","limited-memory");
	//app->Options()->SetNumericValue("bound_relax_factor",1e-7);
	//app->Options()->SetIntegerValue("print_level",12);
	//app->Options()->SetStringValue("derivative_test_print_all","yes");

	// Initialize the IpoptApplication and process the options
	ApplicationReturnStatus status;
	status = app->Initialize();
	if (status != Solve_Succeeded) {
		std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
		BOOST_THROW_EXCEPTION(equilibrium_error() << str_errinfo("Error initializing solver"));
	}

	status = app->OptimizeTNLP(mynlp);

	if (status == Solve_Succeeded ||
			status == Solved_To_Acceptable_Level ||
			status == Search_Direction_Becomes_Too_Small) {
		// Retrieve some statistics about the solve
		Index iter_count = app->Statistics()->IterationCount();
		std::cout << std::endl << std::endl << "*** The problem solved in " << iter_count << " iterations!" << std::endl;

		Number final_obj = app->Statistics()->FinalObjective();
		mingibbs = final_obj * conds.statevars.find('N')->second;
		GibbsOpt* opt_ptr = dynamic_cast<GibbsOpt*> (Ipopt::GetRawPtr(mynlp));
		if (!opt_ptr)
			BOOST_THROW_EXCEPTION(equilibrium_error() << str_errinfo("Internal memory error") << specific_errinfo("dynamic_cast<GibbsOpt*>"));
		ph_map = opt_ptr->get_phase_map();
	}
	else {
		BOOST_THROW_EXCEPTION(equilibrium_error() << str_errinfo("Solver failed to find equilibrium"));
	}
}

std::ostream& operator<< (std::ostream& stream, const Equilibrium& eq) {
	boost::io::ios_flags_saver  ifs( stream ); // preserve original state of the stream once we leave scope
	stream << "Output from LIBGIBBS, equilibrium number = ??" << std::endl;
	stream << "Conditions:" << std::endl;

	// We want the individual phase information to appear AFTER
	// the global system data, but the most efficient ordering
	// requires us to iterate through all phases first.
	// The simple solution is to save the output to a temporary
	// buffer, and then flush it to the output stream later.
	std::stringstream temp_buf;
    temp_buf << std::scientific;

	const auto sv_end = eq.conditions.statevars.cend();
	const auto xf_end = eq.conditions.xfrac.cend();
	for (auto i = eq.conditions.xfrac.cbegin(); i !=xf_end; ++i) {
		stream << "X(" << i->first << ")=" << i->second << ", ";
	}
	for (auto i = eq.conditions.statevars.cbegin(); i != sv_end; ++i) {
		stream << i->first << "=" << i->second;
		// if this isn't the last one, add a comma
		if (std::distance(i,sv_end) != 1) stream << ", ";
	}
	stream << std::endl;
	// should be c + 2 - conditions, where c is the number of components
	size_t dof = eq.conditions.elements.size() + 2 - (eq.conditions.xfrac.size()+1) - eq.conditions.statevars.size();
	stream << "DEGREES OF FREEDOM " << dof << std::endl;

	stream << std::endl;

	double T,P,N;
	T = eq.conditions.statevars.find('T')->second;
	P = eq.conditions.statevars.find('P')->second;
	N = eq.conditions.statevars.find('N')->second;
	stream << "Temperature " << T << " K (" << (T-273.15) << " C), " << "Pressure " << P << " Pa" << std::endl;

	stream << std::scientific; // switch to scientific notation for doubles
    stream << "Number of moles of components " << N << ", Mass ????" << std::endl;
    stream << "Total Gibbs energy " << eq.mingibbs << " Enthalpy ???? " << "Volume ????" << std::endl;

    stream << std::endl;

    const auto ph_end = eq.ph_map.cend();
	const auto cond_spec_begin = eq.conditions.elements.cbegin();
	const auto cond_spec_end = eq.conditions.elements.cend();
    // double/double pair is for separate storage of numerator/denominator pieces of fraction
    std::map<std::string,std::pair<double,double>> global_comp;
    for (auto i = eq.ph_map.cbegin(); i != ph_end; ++i) {
    	const auto subl_begin = i->second.second.cbegin();
    	const auto subl_end = i->second.second.cend();
    	const double phasefrac = i->second.first;
    	std::map<std::string,std::pair<double,double>> phase_comp;
    	temp_buf << i->first << "\tStatus ENTERED  Driving force 0" << std::endl; // phase name
    	temp_buf << "Number of moles " << i->second.first * N << ", Mass ???? ";
    	temp_buf << "Mole fractions:" << std::endl;
    	for (auto j = subl_begin; j != subl_end; ++j) {
    		const double stoi_coef = j->first;
    		const double den = stoi_coef;
    		const auto spec_begin = j->second.cbegin();
    		const auto spec_end = j->second.cend();
    		const auto vacancy_iterator = (j->second).find("VA"); // this may point to spec_end
    		/*
    		 * To make sure all mole fractions sum to 1,
    		 * we have to normalize everything using the same
    		 * sublattices. That means even species which don't
    		 * appear in a given sublattice need to have that
    		 * sublattice's coefficient appear in its denominator.
    		 * To accomplish this task, we perform two loops in each sublattice:
    		 * 1) all species (except VA), to add to the denominator
    		 * 2) only species in that sublattice, to add to the numerator
    		 * With this method, all mole fractions will sum properly.
    		 */
    		for (auto k = cond_spec_begin; k != cond_spec_end; ++k) {
    			if (*k == "VA") continue; // vacancies don't contribute to mole fractions
				if (vacancy_iterator != spec_end) {
					phase_comp[*k].second += den * (1 - vacancy_iterator->second);
				}
				else {
					phase_comp[*k].second += den;
				}
    		}
    		for (auto k = spec_begin; k != spec_end; ++k) {
    			if (k->first == "VA") continue; // vacancies don't contribute to mole fractions
                double num = k->second * stoi_coef;
                phase_comp[k->first].first += num;
    		}
    	}
    	/* We've summed up over all sublattices in this phase.
    	 * Now add this phase's contribution to the overall composition.
    	 */
    	for (auto j = cond_spec_begin; j != cond_spec_end; ++j) {
    		if (*j == "VA") continue; // vacancies don't contribute to mole fractions
    		global_comp[*j].first += phasefrac * (phase_comp[*j].first / phase_comp[*j].second);
    		global_comp[*j].second += phasefrac;
    	}

    	const auto cmp_begin = phase_comp.cbegin();
    	const auto cmp_end = phase_comp.cend();
    	for (auto g = cmp_begin; g != cmp_end; ++g) {
    		temp_buf << g->first << " " << (g->second.first / g->second.second) << "  ";
    	}
    	temp_buf << std::endl << "Constitution:" << std::endl;

    	for (auto j = subl_begin; j != subl_end; ++j) {
    		double stoi_coef = j->first;
    		temp_buf << "Sublattice " << std::distance(subl_begin,j)+1 << ", Number of sites " << stoi_coef << std::endl;
    		const auto spec_begin = j->second.cbegin();
    		const auto spec_end = j->second.cend();
    		for (auto k = spec_begin; k != spec_end; ++k) {
                temp_buf << k->first << " " << k->second << " ";
    		}
    		temp_buf << std::endl;
    	}

    	// if we're at the last phase, don't add an extra newline
    	if (std::distance(i,ph_end) != 1) temp_buf << std::endl;
    }
    const auto glob_begin = global_comp.cbegin();
    const auto glob_end = global_comp.cend();
    stream << "Component\tMoles\tW-Fraction\tActivity\tPotential\tRef.state" << std::endl;
    for (auto h = glob_begin; h != glob_end; ++h) {
    	stream << h->first << " " << (h->second.first / h->second.second) * N << " ???? ???? ???? ????" << std::endl;
    }
    stream << std::endl;

    stream << temp_buf.rdbuf(); // include the temporary buffer with all the phase data

	return stream;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <vector>
#define MAX 100001
using namespace std;

class Graph{
    std::vector<int> v[MAX];
public:
    bool isCyclic(int node);
    int countCycles();
    void addEdge(int a,int b){v[x].push_back(y);}
};
int main(int argc, char const *argv[]) {
    int t;
    cin>>t;
    while(t--){
        Graph g;
        int n;
        cin>>n;
        int val;
        for(int i=1;i<=n;i++) {cin>>val; g.addEdge(i,((i+val+1)%n));}
        int count=0;
        /*for(int i=0;i<n;i++){
            int j=i;
            int cnt=0;
            while(cnt<=n){
                j=(j+arr[j]+1)%n;
                if(j==i) {count++;break;}
                cnt++;
            }

        }*/
        count=g.countCycles();
        cout<<count<<endl;
    }
    return 0;
}
<commit_msg>cyclic method left<commit_after>#include <iostream>
#include <vector>
#define MAX 100001
using namespace std;

class Graph{
public:
    std::vector<int> v[MAX];
    int node;
    bool isCyclic(int node);
    int countCycles();
    void addEdge(int a,int b){v[x].push_back(y);}
};

int Graph::countCycles(){
    bool visited[n],resstack[n];
    for(int i=1;i<=node;i++){visited[i]=0;resstack[i]=0;}
    int count=0;
    for(int i=1;i<=n;i++){
        count+=isCyclic(i);
    }
    return count;
}
int main(int argc, char const *argv[]) {
    int t;
    cin>>t;
    while(t--){
        Graph g;
        int n;
        cin>>n;
        g.node=n;
        int val;
        for(int i=1;i<=n;i++) {cin>>val; g.addEdge(i,((i+val+1)%n));}
        int count=0;
        /*for(int i=0;i<n;i++){
            int j=i;
            int cnt=0;
            while(cnt<=n){
                j=(j+arr[j]+1)%n;
                if(j==i) {count++;break;}
                cnt++;
            }

        }*/
        count=g.countCycles();
        cout<<count<<endl;
    }
    return 0;
}
<|endoftext|>


<commit_before>/**
 * @file ClusterProxyHelper.cc
 * @author Rafal Slota
 * @copyright (C) 2013 ACK CYFRONET AGH
 * @copyright This software is released under the MIT license cited in 'LICENSE.txt'
 */

#ifdef linux
/* For pread()/pwrite()/utimensat() */
#define _XOPEN_SOURCE 700
#endif /* linux */

#include <fuse.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <dirent.h>
#include <errno.h>
#include <sys/time.h>
#include <cstring>
#ifdef HAVE_SETXATTR
#include <sys/xattr.h>
#endif

#include <limits.h>

#include <boost/algorithm/string.hpp>
#include "glog/logging.h"
#include "clusterProxyHelper.h"
#include "helpers/storageHelperFactory.h"
#include <google/protobuf/descriptor.h>
#include "veilErrors.h"

#include <iostream>

using namespace std;
using namespace boost::algorithm;
using namespace veil::protocol::remote_file_management;
using namespace veil::protocol::communication_protocol;

namespace veil {
namespace helpers {

ClusterMsg ClusterProxyHelper::commonClusterMsgSetup(string inputType, string inputData) {

    RemoteFileMangement rfm;
    rfm.set_message_type(utils::tolower(inputType));
    rfm.set_input(inputData);

    ClusterMsg clm;
    clm.set_protocol_version(PROTOCOL_VERSION);
    clm.set_synch(true);
    clm.set_module_name(RFM_MODULE_NAME);
    clm.set_message_decoder_name(RFM_DECODER);
    clm.set_message_type(utils::tolower(rfm.GetDescriptor()->name()));

    clm.set_input(rfm.SerializeAsString());

    return clm;
}

string ClusterProxyHelper::requestMessage(string inputType, string answerType, string inputData) {
    ClusterMsg clm = commonClusterMsgSetup(inputType, inputData);

    clm.set_answer_type(utils::tolower(answerType));
    clm.set_answer_decoder_name(RFM_DECODER);

    Answer answer = sendCluserMessage(clm);

    return answer.worker_answer();
}

string ClusterProxyHelper::requestAtom(string inputType, string inputData) {
    ClusterMsg clm = commonClusterMsgSetup(inputType, inputData);

    clm.set_answer_type(utils::tolower(Atom::descriptor()->name()));
    clm.set_answer_decoder_name(COMMUNICATION_PROTOCOL_DECODER);

    Answer answer = sendCluserMessage(clm);

    Atom atom;
    if(answer.has_worker_answer()) {
        atom.ParseFromString(answer.worker_answer());
        return atom.value();
    }

    return "";
}

Answer ClusterProxyHelper::sendCluserMessage(ClusterMsg &msg) {
    boost::shared_ptr<CommunicationHandler> connection = m_connectionPool ? m_connectionPool->selectConnection(SimpleConnectionPool::DATA_POOL) : config::getConnectionPool()->selectConnection();
    if(!connection)
    {
        LOG(ERROR) << "Cannot select connection from connectionPool";
        return Answer();
    }

    Answer answer = connection->communicate(msg, 2);
    if(answer.answer_status() != VEIO)
        config::getConnectionPool()->releaseConnection(connection);

    if(answer.answer_status() != VOK)
        LOG(WARNING) << "Cluster send non-ok message. status = " << answer.answer_status();

    return answer;
}

//////////////////////
// Helper callbacks //
//////////////////////

int ClusterProxyHelper::sh_getattr(const char *path, struct stat *stbuf)
{
    // Just leave defaults and ignore this call
    return 0;
}

int ClusterProxyHelper::sh_access(const char *path, int mask)
{
    // We dont need this method, return success
    return 0;
}

int ClusterProxyHelper::sh_mknod(const char *path, mode_t mode, dev_t rdev)
{
    LOG(INFO) << "CluserProxyHelper mknod(path: " << string(path) << ")";

    CreateFile msg;
    msg.set_file_id(string(path));

    return translateError(requestAtom(msg.GetDescriptor()->name(), msg.SerializeAsString()));
}

int ClusterProxyHelper::sh_unlink(const char *path)
{
    LOG(INFO) << "CluserProxyHelper unlink(path: " << string(path) << ")";

    DeleteFileAtStorage msg;
    msg.set_file_id(string(path));

    return translateError(requestAtom(msg.GetDescriptor()->name(), msg.SerializeAsString()));
}

int ClusterProxyHelper::sh_chmod(const char *path, mode_t mode)
{
    return 0;
}

int ClusterProxyHelper::sh_chown(const char *path, uid_t uid, gid_t gid)
{
    return 0;
}

int ClusterProxyHelper::sh_truncate(const char *path, off_t size)
{
    LOG(INFO) << "CluserProxyHelper truncate(path: " << string(path) << ", size: " << size << ")";

    TruncateFile msg;
    msg.set_file_id(string(path));
    msg.set_length(size);

    return translateError(requestAtom(msg.GetDescriptor()->name(), msg.SerializeAsString()));
}

int ClusterProxyHelper::sh_open(const char *path, struct fuse_file_info *fi)
{
    return m_bufferAgent.onOpen(string(path), fi);
}

int ClusterProxyHelper::sh_read(const char *path, char *buf, size_t size, off_t offset,
            struct fuse_file_info *fi)
{
    LOG(INFO) << "CluserProxyHelper read(path: " << string(path) << ", size: " << size << ", offset: " << offset << ")";

    ReadFile msg;
    msg.set_file_id(string(path));
    msg.set_size(size);
    msg.set_offset(offset);

    FileData answer;

    if(!answer.ParseFromString(
        requestMessage(msg.GetDescriptor()->name(), answer.GetDescriptor()->name(), msg.SerializeAsString())))
    {
        LOG(WARNING) << "Cannot parse answer for file: " << string(path);
        return translateError(VEIO);
    }

    LOG(INFO) << "CluserProxyHelper read answer_status: " << answer.answer_status() << ", read real size: " << answer.data().size();

    if(answer.answer_status() == VOK) {
        size_t readSize = (answer.data().size() > size ? size : answer.data().size());

        memcpy(buf, answer.data().data(), readSize);

        if(answer.data().size() != size)
            LOG(WARNING) << "read for file: " << string(path) << " returned " << answer.data().size() << "bytes. Expected: " << size;

        return readSize;

    } else if(answer.answer_status() == "ok:TODO2") {
        /// TODO: implement big read
        LOG(ERROR) << "Cluster requested to read file (" << string(path) << ") directly over TCP/IP which is not implemented yet";
        return -ENOTSUP;
    } else
        return translateError(answer.answer_status());
}

int ClusterProxyHelper::sh_write(const char *path, const char *buf, size_t size,
             off_t offset, struct fuse_file_info *fi)
{
    LOG(INFO) << "CluserProxyHelper write(path: " << string(path) << ", size: " << size << ", offset: " << offset << ")";

    return m_bufferAgent.onWrite(string(path), string(buf, size), size, offset, fi);
}

int ClusterProxyHelper::sh_release(const char *path, struct fuse_file_info *fi)
{
    return m_bufferAgent.onRelease(string(path), fi);;
}

int ClusterProxyHelper::sh_flush(const char *path, struct fuse_file_info *fi)
{
    return m_bufferAgent.onFlush(string(path), fi);
}

int ClusterProxyHelper::sh_fsync(const char *path, int isdatasync,
             struct fuse_file_info *fi)
{
    /* Just a stub.     This method is optional and can safely be left
       unimplemented */

    (void) path;
    (void) isdatasync;
    (void) fi;
    return 0;
}

int ClusterProxyHelper::sh_mkdir(const char *path, mode_t mode)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_readdir(const char *path, void *buf, fuse_fill_dir_t filler,
               off_t offset, struct fuse_file_info *fi)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_statfs(const char *path, struct statvfs *stbuf)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_rmdir(const char *path)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_symlink(const char *from, const char *to)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_rename(const char *from, const char *to)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_link(const char *from, const char *to)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_readlink(const char *path, char *buf, size_t size)
{
    return ENOTSUP;
}

#ifdef HAVE_POSIX_FALLOCATE
int ClusterProxyHelper::sh_fallocate(const char *path, int mode,
            off_t offset, off_t length, struct fuse_file_info *fi)
{
    return ENOTSUP;
}
#endif  /* HAVE_POSIX_FALLOCATE */

#ifdef HAVE_UTIMENSAT
int ClusterProxyHelper::sh_utimens(const char *path, const struct timespec ts[2])
{
    return 0;
}
#endif /* HAVE_UTIMENSAT */

#ifdef HAVE_SETXATTR
/* xattr operations are optional and can safely be left unimplemented */
int ClusterProxyHelper::sh_setxattr(const char *path, const char *name, const char *value,
            size_t size, int flags)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_getxattr(const char *path, const char *name, char *value,
            size_t size)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_listxattr(const char *path, char *list, size_t size)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_removexattr(const char *path, const char *name)
{
    return ENOTSUP;
}

#endif /* HAVE_SETXATTR */

int ClusterProxyHelper::doWrite(std::string path, const std::string &buf, size_t, off_t offset, ffi_type)
{
    WriteFile msg;
    msg.set_file_id(path);
    msg.set_data(buf);
    msg.set_offset(offset);

    WriteInfo answer;

    if(!answer.ParseFromString(
        requestMessage(msg.GetDescriptor()->name(), answer.GetDescriptor()->name(), msg.SerializeAsString())))
    {
        LOG(WARNING) << "Cannot parse answer for file: " << string(path);
        return translateError(VEIO);
    }

    LOG(INFO) << "CluserProxyHelper write answer_status: " << answer.answer_status() << ", write real size: " << answer.bytes_written();

    int error = translateError(answer.answer_status());
    if(error == 0) return answer.bytes_written();
    else           return error;
    return 0;
}

int ClusterProxyHelper::doRead(std::string path, std::string &buf, size_t, off_t, ffi_type)
{
    return 0;
}

ClusterProxyHelper::ClusterProxyHelper(std::vector<std::string> args)
  : m_bufferAgent(
        boost::bind(&ClusterProxyHelper::doWrite, this, _1, _2, _3, _4, _5),
        boost::bind(&ClusterProxyHelper::doRead, this, _1, _2, _3, _4, _5))
{
    if(args.size() >= 3) { // If arguments are given, use them to establish connection instead default VeilHelpers configuration
        m_clusterHostname   = args[0];
        m_clusterPort       = utils::fromString<unsigned int>(args[1]);
        m_proxyCert         = args[2];

        m_connectionPool.reset(new SimpleConnectionPool(m_clusterHostname, m_clusterPort, m_proxyCert, NULL));
    } else { // Otherwise init local config using global values
        m_clusterHostname   = config::clusterHostname;
        m_clusterPort       = config::clusterPort;
        m_proxyCert         = config::proxyCert;
    }
}

ClusterProxyHelper::~ClusterProxyHelper()
{
}

} // namespace helpers
} // namespace veil
<commit_msg>VFS-292: debug<commit_after>/**
 * @file ClusterProxyHelper.cc
 * @author Rafal Slota
 * @copyright (C) 2013 ACK CYFRONET AGH
 * @copyright This software is released under the MIT license cited in 'LICENSE.txt'
 */

#ifdef linux
/* For pread()/pwrite()/utimensat() */
#define _XOPEN_SOURCE 700
#endif /* linux */

#include <fuse.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <dirent.h>
#include <errno.h>
#include <sys/time.h>
#include <cstring>
#ifdef HAVE_SETXATTR
#include <sys/xattr.h>
#endif

#include <limits.h>

#include <boost/algorithm/string.hpp>
#include "glog/logging.h"
#include "clusterProxyHelper.h"
#include "helpers/storageHelperFactory.h"
#include <google/protobuf/descriptor.h>
#include "veilErrors.h"

#include <iostream>

using namespace std;
using namespace boost::algorithm;
using namespace veil::protocol::remote_file_management;
using namespace veil::protocol::communication_protocol;

namespace veil {
namespace helpers {

ClusterMsg ClusterProxyHelper::commonClusterMsgSetup(string inputType, string inputData) {

    RemoteFileMangement rfm;
    rfm.set_message_type(utils::tolower(inputType));
    rfm.set_input(inputData);

    ClusterMsg clm;
    clm.set_protocol_version(PROTOCOL_VERSION);
    clm.set_synch(true);
    clm.set_module_name(RFM_MODULE_NAME);
    clm.set_message_decoder_name(RFM_DECODER);
    clm.set_message_type(utils::tolower(rfm.GetDescriptor()->name()));

    clm.set_input(rfm.SerializeAsString());

    return clm;
}

string ClusterProxyHelper::requestMessage(string inputType, string answerType, string inputData) {
    ClusterMsg clm = commonClusterMsgSetup(inputType, inputData);

    clm.set_answer_type(utils::tolower(answerType));
    clm.set_answer_decoder_name(RFM_DECODER);

    Answer answer = sendCluserMessage(clm);

    return answer.worker_answer();
}

string ClusterProxyHelper::requestAtom(string inputType, string inputData) {
    ClusterMsg clm = commonClusterMsgSetup(inputType, inputData);

    clm.set_answer_type(utils::tolower(Atom::descriptor()->name()));
    clm.set_answer_decoder_name(COMMUNICATION_PROTOCOL_DECODER);

    Answer answer = sendCluserMessage(clm);

    Atom atom;
    if(answer.has_worker_answer()) {
        atom.ParseFromString(answer.worker_answer());
        return atom.value();
    }

    return "";
}

Answer ClusterProxyHelper::sendCluserMessage(ClusterMsg &msg) {
    boost::shared_ptr<CommunicationHandler> connection = m_connectionPool ? m_connectionPool->selectConnection(SimpleConnectionPool::DATA_POOL) : config::getConnectionPool()->selectConnection();
    if(!connection)
    {
        LOG(ERROR) << "Cannot select connection from connectionPool";
        return Answer();
    }

    Answer answer = connection->communicate(msg, 2);
    if(answer.answer_status() != VEIO)
        config::getConnectionPool()->releaseConnection(connection);

    if(answer.answer_status() != VOK)
        LOG(WARNING) << "Cluster send non-ok message. status = " << answer.answer_status();

    return answer;
}

//////////////////////
// Helper callbacks //
//////////////////////

int ClusterProxyHelper::sh_getattr(const char *path, struct stat *stbuf)
{
    // Just leave defaults and ignore this call
    return 0;
}

int ClusterProxyHelper::sh_access(const char *path, int mask)
{
    // We dont need this method, return success
    return 0;
}

int ClusterProxyHelper::sh_mknod(const char *path, mode_t mode, dev_t rdev)
{
    LOG(INFO) << "CluserProxyHelper mknod(path: " << string(path) << ")";

    CreateFile msg;
    msg.set_file_id(string(path));

    return translateError(requestAtom(msg.GetDescriptor()->name(), msg.SerializeAsString()));
}

int ClusterProxyHelper::sh_unlink(const char *path)
{
    LOG(INFO) << "CluserProxyHelper unlink(path: " << string(path) << ")";

    DeleteFileAtStorage msg;
    msg.set_file_id(string(path));

    return translateError(requestAtom(msg.GetDescriptor()->name(), msg.SerializeAsString()));
}

int ClusterProxyHelper::sh_chmod(const char *path, mode_t mode)
{
    return 0;
}

int ClusterProxyHelper::sh_chown(const char *path, uid_t uid, gid_t gid)
{
    return 0;
}

int ClusterProxyHelper::sh_truncate(const char *path, off_t size)
{
    LOG(INFO) << "CluserProxyHelper truncate(path: " << string(path) << ", size: " << size << ")";

    TruncateFile msg;
    msg.set_file_id(string(path));
    msg.set_length(size);

    return translateError(requestAtom(msg.GetDescriptor()->name(), msg.SerializeAsString()));
}

int ClusterProxyHelper::sh_open(const char *path, struct fuse_file_info *fi)
{
    LOG(INFO) << "CluserProxyHelper open(path: " << string(path) << ")";

    return m_bufferAgent.onOpen(string(path), fi);
}

int ClusterProxyHelper::sh_read(const char *path, char *buf, size_t size, off_t offset,
            struct fuse_file_info *fi)
{
    LOG(INFO) << "CluserProxyHelper read(path: " << string(path) << ", size: " << size << ", offset: " << offset << ")";

    ReadFile msg;
    msg.set_file_id(string(path));
    msg.set_size(size);
    msg.set_offset(offset);

    FileData answer;

    if(!answer.ParseFromString(
        requestMessage(msg.GetDescriptor()->name(), answer.GetDescriptor()->name(), msg.SerializeAsString())))
    {
        LOG(WARNING) << "Cannot parse answer for file: " << string(path);
        return translateError(VEIO);
    }

    LOG(INFO) << "CluserProxyHelper read answer_status: " << answer.answer_status() << ", read real size: " << answer.data().size();

    if(answer.answer_status() == VOK) {
        size_t readSize = (answer.data().size() > size ? size : answer.data().size());

        memcpy(buf, answer.data().data(), readSize);

        if(answer.data().size() != size)
            LOG(WARNING) << "read for file: " << string(path) << " returned " << answer.data().size() << "bytes. Expected: " << size;

        return readSize;

    } else if(answer.answer_status() == "ok:TODO2") {
        /// TODO: implement big read
        LOG(ERROR) << "Cluster requested to read file (" << string(path) << ") directly over TCP/IP which is not implemented yet";
        return -ENOTSUP;
    } else
        return translateError(answer.answer_status());
}

int ClusterProxyHelper::sh_write(const char *path, const char *buf, size_t size,
             off_t offset, struct fuse_file_info *fi)
{
    LOG(INFO) << "CluserProxyHelper write(path: " << string(path) << ", size: " << size << ", offset: " << offset << ")";

    return m_bufferAgent.onWrite(string(path), string(buf, size), size, offset, fi);
}

int ClusterProxyHelper::sh_release(const char *path, struct fuse_file_info *fi)
{
    LOG(INFO) << "CluserProxyHelper release(path: " << string(path) << ")";
    return m_bufferAgent.onRelease(string(path), fi);;
}

int ClusterProxyHelper::sh_flush(const char *path, struct fuse_file_info *fi)
{
    LOG(INFO) << "CluserProxyHelper flush(path: " << string(path) << ")";
    return m_bufferAgent.onFlush(string(path), fi);
}

int ClusterProxyHelper::sh_fsync(const char *path, int isdatasync,
             struct fuse_file_info *fi)
{
    /* Just a stub.     This method is optional and can safely be left
       unimplemented */

    (void) path;
    (void) isdatasync;
    (void) fi;
    return 0;
}

int ClusterProxyHelper::sh_mkdir(const char *path, mode_t mode)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_readdir(const char *path, void *buf, fuse_fill_dir_t filler,
               off_t offset, struct fuse_file_info *fi)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_statfs(const char *path, struct statvfs *stbuf)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_rmdir(const char *path)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_symlink(const char *from, const char *to)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_rename(const char *from, const char *to)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_link(const char *from, const char *to)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_readlink(const char *path, char *buf, size_t size)
{
    return ENOTSUP;
}

#ifdef HAVE_POSIX_FALLOCATE
int ClusterProxyHelper::sh_fallocate(const char *path, int mode,
            off_t offset, off_t length, struct fuse_file_info *fi)
{
    return ENOTSUP;
}
#endif  /* HAVE_POSIX_FALLOCATE */

#ifdef HAVE_UTIMENSAT
int ClusterProxyHelper::sh_utimens(const char *path, const struct timespec ts[2])
{
    return 0;
}
#endif /* HAVE_UTIMENSAT */

#ifdef HAVE_SETXATTR
/* xattr operations are optional and can safely be left unimplemented */
int ClusterProxyHelper::sh_setxattr(const char *path, const char *name, const char *value,
            size_t size, int flags)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_getxattr(const char *path, const char *name, char *value,
            size_t size)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_listxattr(const char *path, char *list, size_t size)
{
    return ENOTSUP;
}

int ClusterProxyHelper::sh_removexattr(const char *path, const char *name)
{
    return ENOTSUP;
}

#endif /* HAVE_SETXATTR */

int ClusterProxyHelper::doWrite(std::string path, const std::string &buf, size_t, off_t offset, ffi_type)
{
    WriteFile msg;
    msg.set_file_id(path);
    msg.set_data(buf);
    msg.set_offset(offset);

    WriteInfo answer;

    if(!answer.ParseFromString(
        requestMessage(msg.GetDescriptor()->name(), answer.GetDescriptor()->name(), msg.SerializeAsString())))
    {
        LOG(WARNING) << "Cannot parse answer for file: " << string(path);
        return translateError(VEIO);
    }

    LOG(INFO) << "CluserProxyHelper write answer_status: " << answer.answer_status() << ", write real size: " << answer.bytes_written();

    int error = translateError(answer.answer_status());
    if(error == 0) return answer.bytes_written();
    else           return error;
    return 0;
}

int ClusterProxyHelper::doRead(std::string path, std::string &buf, size_t, off_t, ffi_type)
{
    return 0;
}

ClusterProxyHelper::ClusterProxyHelper(std::vector<std::string> args)
  : m_bufferAgent(
        boost::bind(&ClusterProxyHelper::doWrite, this, _1, _2, _3, _4, _5),
        boost::bind(&ClusterProxyHelper::doRead, this, _1, _2, _3, _4, _5))
{
    if(args.size() >= 3) { // If arguments are given, use them to establish connection instead default VeilHelpers configuration
        m_clusterHostname   = args[0];
        m_clusterPort       = utils::fromString<unsigned int>(args[1]);
        m_proxyCert         = args[2];

        m_connectionPool.reset(new SimpleConnectionPool(m_clusterHostname, m_clusterPort, m_proxyCert, NULL));
    } else { // Otherwise init local config using global values
        m_clusterHostname   = config::clusterHostname;
        m_clusterPort       = config::clusterPort;
        m_proxyCert         = config::proxyCert;
    }
}

ClusterProxyHelper::~ClusterProxyHelper()
{
}

} // namespace helpers
} // namespace veil
<|endoftext|>